diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
5 files changed, 167 insertions, 81 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index db3bfc46e..dad7a1db6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -55,6 +55,7 @@ extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM]; +extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM]; uint32_t InternalWinderConfigMessage(HardwareWinder* request); @@ -65,8 +66,5 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t DancerConfigMessage(HardwareDancer * request); -uint32_t ThreadPrepareState(void *JobDetails); -uint32_t Winder_Prepare(void); - #endif //MODULES_THREAD_THREAD_H_ diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index b61247fe4..3505ff3c6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -9,53 +9,132 @@ #include "Drivers/Peripheral_GPIO/GPIO.h" #include "drivers/Motors/Motor.h" #include "StateMachines/Printing/PrintingSTM.h" +#include "Modules/Control/Control.h" + +#include "drivers/FPGA/FPGA.h" + +#include "drivers/FPGA/FPGA_SPI_Comm.h" +#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" + bool Winder_ScrewHoming = false; -uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps); -uint32_t Winder_PrepareStage2(void); +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag); +uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); -double ScrewSpeed = 0; +bool ScrewCurrentDirection = false; //holds current screw direction +bool BusyfirstCall = true; //Ignores first call after activating the screw - too early +double ScrewSpeed = 0; //ScrewSpeed +uint32_t ScrewControlId = 0xFF; +uint32_t ScrewNumberOfSteps = 0; //holds the current number of steps for the next screw run - will be used to build the cone +uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw - will be used to build the cone uint32_t Winder_Init(void) { return OK; } +/* + *uint32_t Winder_Prepare(void *JobDetails) + + * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. + * report ready to the job STM + */ uint32_t Winder_Prepare(void) { uint32_t status = 0; -// double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); + //JobTicket* JobTicket = JobDetails; + + //float process_speed = JobTicket->processparameters->dyeingspeed; + double ScrewSpeed = 500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); // we will use pulley radius of the screw for this purpose, as of now /* * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done * report ready to the job STM */ - if (PollGPIO(GPI113_LS_SCREW_RIGHT)) + if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) { - //go to prepare stage 2 - Winder_PrepareStage2(); + Winder_PrepareStage2(0,0); } else { Winder_ScrewHoming = true; - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1);//make sur to move the cart home - status |= MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed); + status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2); } return status; } -uint32_t Winder_PrepareStage2(void) +/* + * uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) + * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done + * report ready to the job STM + */ + +uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) { - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; + uint32_t status=OK; + uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + + status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback); + //set motor location 0 here + return status; + +} + +uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) +{ + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + + ScrewCurrentDirection = false; + BusyfirstCall = true; + ScrewSpeed = 0; + ScrewControlId = 0xFF; + ScrewNumberOfSteps = 0; + DirectionChangeCounter = 0; + + PrepareReady(Module_Winder, ModuleDone); + return OK; +} + +/* + * this is the main operational function of the screw - run back and forth until stopped + */ +/* + +bool ScrewCurrentDirection = false; +bool BusyfirstCall = true; +double ScrewSpeed = 0; +uint32_t ScrewControlId = 0xFF; +uint32_t ScrewNumberOfSteps = 0; +uint32_t DirectionChangeCounter = 0; +*/ + +uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) +{ + if (BusyfirstCall) + { + BusyfirstCall = false; + return OK; + } + //ScrewCurrentDirection: false moves out, true moves home + if (BusyFlag == NOTBUSY) + { + if (ScrewCurrentDirection == false) + { + ScrewCurrentDirection = true; + } + else + { + ScrewCurrentDirection = false; + } + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. + DirectionChangeCounter++; + } +/* + * calculate new ScrewSpeed and call MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + * + */ /* - * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM + * calculate cone shape according to DirectionChangeCounter and update ScrewNumberOfSteps */ - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0);//make sur to move the cart out - //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here - return status; + return OK; } @@ -63,6 +142,7 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) { return PreSegmentReady(Module_Winder,ModuleDone); } + uint32_t Winder_Presegment(void *JobDetails) { JobTicket* JobTicket = JobDetails; @@ -70,72 +150,54 @@ uint32_t Winder_Presegment(void *JobDetails) float screw_speed = 0; float RotationsPerSecond; -// double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); + ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); -/*typedef struct -{ - uint32_t startoffsetpulses; - uint32_t spoolbackingrate; - uint32_t segmentoffsetpulses; - uint32_t milimetersperrotation; - int32 SpoolBottomBackingRate;// the angle of the bottom of the spool - double NumberOfRotationPerPassage; // how many rotations per spool passage -}InternalWinderConfigStruc; - * */ -// * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec) -// * calculate -// * 1. calculate speed according to JobTicket->processparameters->dyeingspeed - // calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation. + // * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec) + // * calculate + // * 1. calculate speed according to JobTicket->processparameters->dyeingspeed + // * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation. screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage; // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second. - RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * 3.1416); + RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI); // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm. - screw_speed = screw_speed*RotationsPerSecond; + ScrewSpeed = screw_speed*RotationsPerSecond; + + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); //screw_speed = InternalWinderCfg.milimetersperrotation // * 2. determine optimal micro-step setting // * 3. calculate cart travel length from winding parameters // * 4. start move of travel length // * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed + ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. + ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); - //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back. - MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady); + // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady); //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move; + PreSegmentReady(Module_Winder,ModuleDone); return OK; } uint32_t Winder_End(void) { //stop screw - return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); + ScrewNumberOfSteps = 0; + if (ScrewControlId != 0xFF) + RemoveControlCallback(ScrewControlId,ScrewDirectionChange); + return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); } void Winder_ScrewHomeLimitSwitchInterrupt(void) { uint32_t status; if (Winder_ScrewHoming) { - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP } - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } void Winder_ScrewOutLimitSwitchInterrupt(void) { uint32_t status; - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out -} -uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps) -{ - if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses) - { - PrepareReady(Module_Winder, ModuleDone); - return OK; - } - else - { - //do we want to do something? - PrepareReady(Module_Winder, ModuleFail); - return ERROR; - } - + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index ea75bffa3..b7401c77f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -2,6 +2,8 @@ #ifndef MODULES_THREAD_THREAD_EX_H_ #define MODULES_THREAD_THREAD_EX_H_ +#include "PMR/Printing/JobSpool.pb-c.h" + //typedef enum //{ #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer @@ -10,6 +12,8 @@ #define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1 //} DANCER_ENUM; +uint32_t InternalWindingConfigMessage(JobSpool* request); + uint32_t ThreadPrepareState(void *JobDetails); uint32_t ThreadPreSegmentState(void *JobDetails); uint32_t ThreadSegmentState(void *JobDetails, int SegmentId); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 0e276fe92..3c9adbc31 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -17,6 +17,7 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0}; int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0}; int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0}; double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0}; +int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0}; InternalWinderConfigStruc InternalWinderCfg = {0}; HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0}; @@ -97,10 +98,12 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; - + temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2); + DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); return OK; } uint32_t DancerConfigMessage(HardwareDancer * request) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 92bfa9c92..e9cc3ddde 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -22,6 +22,7 @@ #include "drivers/Heater/TemperatureSensor.h" #include "drivers/Heater/Heater.h" #include "drivers/Motors/Motor.h" +#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "modules/heaters/heaters.h" ////////////////////////////////State machine operation//////////////////////////////////// @@ -36,8 +37,9 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; uint32_t SpeedControlId=0xFF; +double DancerError[NUM_OF_DANCERS] = {0.0}; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; - +uint32_t JobCounter = 0; typedef struct { bool m_isEnabled; @@ -66,6 +68,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); + +double KeepNormalizedError = 0; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); @@ -118,7 +122,6 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) } double MotorSentData[1000] = {0}; uint32_t PosDif[1000] = {0}; -uint32_t tick[1000] = {0}; int MotorDataIndex = 0; @@ -163,13 +166,12 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) totalLength+=length; } -#warning control disabled +//#warning control disabled CurrentProcessedLength+=length; -#warning control disabled + PosDif[MotorDataIndex] = CurrentPosition; //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; - tick[MotorDataIndex] = UsersysTickGet(); MotorDataIndex+=1; if (MotorDataIndex == 999) MotorDataIndex = 0; static int pooler_counter = 0; @@ -177,6 +179,8 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) if (pooler_counter%10 == 0) { SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); + //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); + } if (pooler_counter>=100) { @@ -187,8 +191,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if (CurrentProcessedLength>=CurrentRequestedLength ) { - SendJobProgress(100,0,true); - #warning handle job wit several segments!!! // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -280,10 +282,14 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) - return; + return OK; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite + if (index == POOLER_MOTOR) + { + //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); + JobCounter++; + } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -291,12 +297,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode; + //Stop Execution if the dancer moves too much + + if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) + { + EndState(CurrentJob); + } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; + DancerError[DancerId] = NormalizedError; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); - if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit + if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); + else + KeepNormalizedError = NormalizedError; calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { @@ -309,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) pooler_counter++; if (pooler_counter>=1000) { - float error_integered = MotorControlConfig[index].m_calculatedError*1000; + //float error_integered = MotorControlConfig[index].m_calculatedError*1000; /*{ "HeaterGroupId": 0, "Zone1Temp": 80, @@ -324,7 +339,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed"); - HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed"); + HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed"); pooler_counter = 0; } } @@ -350,6 +365,8 @@ bool InitialProcess = false; { int Motor_i, HW_Motor_Id, Pid_Id; CurrentSegmentId = 0; + + JobCounter = 0; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { @@ -469,12 +486,13 @@ uint32_t ThreadPreSegmentState(void *JobDetails) //#warning rocker disabled if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5); if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10); + MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5); //#warning rocker disabled +// MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO // activate control fr all motors //set speed for both rocker motors @@ -486,6 +504,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) } else { + ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); InitialProcess = false; } @@ -527,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); } - StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); + MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); - StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); return OK; } |