/************************************************************************************************************************ * Thread_print.c * Printing module is responsible for : * operating diffrent winding algorithms with predefined parameters from the UI * operating the dispensers according to predefined dispensing rate from the UI **************************************************************************************************************************/ #include "include.h" #include #include "thread.h" #include "thread_ex.h" #include "../control/control.h" #include "../control/pidalgo.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" #include "PMR/Hardware/HardwareMotorType.pb-c.h" #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include "PMR/Printing/JobSegment.pb-c.h" #include "PMR/Printing/JobTicket.pb-c.h" #include #include #include "StateMachines/Printing/PrintingSTM.h" #include "drivers/Motors/Motor.h" //#include "drivers/SSI_Comm/ssi_comm.h" #include "drivers/SSI_Comm/Dancer/Dancer.h" #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" #include "modules/General/process.h" #include "modules/ids/ids_ex.h" #include "Modules/AlarmHandling/AlarmHandling.h" #include "Control/MillisecTask.h" ////////////////////////////////State machine operation//////////////////////////////////// //the state machine operation is used to operate in runtime correct profile flow execution //by recieved esign flow of the user from the UI /////////////////////////////////////////////////////////////////////////////////////////// uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF}; uint32_t SpeedControlId=0xFF; uint32_t PoolerSpeedControlId=0xFF; double DancerError[NUM_OF_DANCERS] = {0.0}; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; uint32_t JobCounter = 0; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; uint32_t PreviousPosition = 0, CurrentPosition = 0; double CurrentRequestedLength = 0.0; double CurrentProcessedLength = 0.0; double TotalProcessedLength = 0.0; double LengthCalculationMultiplier; uint32_t PoolerPreviousPosition = 0, PoolerCurrentPosition = 0; double PoolerTotalProcessedLength = 0.0; double PoolerLengthCalculationMultiplier; double TempPoolerTotalProcessedLength = 0.0; double TempTotalProcessedLength = 0.0; bool PrepareState = false; // job parameters bool EnableLubrication = false; bool EnableIntersegment = false; double IntersegmentLength = 0; int CurrentSegmentId = 0; typedef void (* ProcessedLengthFunc)(void); ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; // segment/intersegment/distance to spool finished void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue); bool SegmentState = false; bool PreSegmentState = false; bool DTSState = false; void SendSegmentFail(void); double KeepNormalizedError = 0; bool ThreadControlActive = false; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); //////////////////////////////////////////////////////////////////////////// /******************************************************************** * * Name : GTIME_Delta_Time_Pass * * Parameters : start_time. * * Return : time pass from start time * * Description : * *********************************************************************/ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read) { uint32_t Time_Pass; // #define MAX_COUNTER 0x3FFF //14 bits #define MAX_COUNTER 0x3FFFFF //22 bits if (Current_Read < Previous_Read) Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1; else Time_Pass = Current_Read - Previous_Read; return (Time_Pass); } /***************************************************************************************** * * * * * * * **************************************************************************************/ uint32_t initialpos = 0xFFFF; uint32_t Poolerinitialpos = 0xFFFF; void ThreadUpdateProcessLength (double length, void *Funcptr) { REPORT_MSG(length,"ThreadUpdateProcessLength"); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; initialpos = 0xFFFF; Poolerinitialpos = 0xFFFF; } char Lenstr[150]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; // if (ThreadControlActive == false) // return OK; // if (PrepareState == true) // return OK; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; // if (CurrentRequestedLength == 0.0) // return OK; if (index != FEEDER_MOTOR) { LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); // if (CurrentPosition == 0) // return OK; //unusable data if (initialpos == 0xFFFF) { PreviousPosition = CurrentPosition; initialpos = 0; } positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PreviousPosition = CurrentPosition; // total length = (position diff / full cycle) * pulley perimeter //(positionDiff/pulseperround)*((2*PI*motor_Radius) //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; length = (double)(positionDiff)*LengthCalculationMultiplier; CurrentProcessedLength+=length; static int pooler_counter = 0; pooler_counter++; TotalProcessedLength+= (length/100); TempTotalProcessedLength = TotalProcessedLength; if (pooler_counter%10 == 0) { if (PrepareState == true) { //later - add temperatures TemperatureListString(Lenstr); SendJobProgress(0.0,0,false, Lenstr); } else { SendJobProgress(TotalProcessedLength,0,false, NULL); } } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); } return OK; } uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; if (ThreadControlActive == false) return OK; if (PrepareState == true) return OK; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; // if (CurrentRequestedLength == 0.0) // return OK; if (index != POOLER_MOTOR) { LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); // if (CurrentPosition == 0) // return OK; //unusable data if (Poolerinitialpos == 0xFFFF) { PoolerPreviousPosition = PoolerCurrentPosition; Poolerinitialpos = 0; } positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PoolerPreviousPosition = PoolerCurrentPosition; length = (double)(positionDiff)*PoolerLengthCalculationMultiplier; PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; return OK; } float SpeedSamples[MAX_CONTROL_SAMPLES] = {0}; uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int index=MAX_THREAD_MOTORS_NUM; int32_t i, avreageSampleValue = 0; //double tempcalcspeed = 0; uint32_t calculated_speed; float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i2) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } } return OK; } uint32_t _speed; uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) { int index; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { int MotorId = ThreadMotorIdToMotorId[index]; _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId); } return OK; } //double eNormalizedError[100]; //int TranslatedreadValue[100]; /*#define MAX_THREAD_CONTROL_LOG 100 double calculatedError[MAX_THREAD_CONTROL_LOG+1]; double NormError[MAX_THREAD_CONTROL_LOG+1]; double mIntegral[MAX_THREAD_CONTROL_LOG+1]; int MotorId[MAX_THREAD_CONTROL_LOG+1]; int readValue[MAX_THREAD_CONTROL_LOG+1]; int AveragereadValue[MAX_THREAD_CONTROL_LOG+1]; int calculatedspeed[MAX_THREAD_CONTROL_LOG+1]; int timestamp[MAX_THREAD_CONTROL_LOG+1];*/ int controlIndex = 0; bool keepdata = true; /*int32_t KeepReadValue = 0; void testDancersControl() { int mm20,mm10,mm5,mm2,mm1; mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2); mm2 = mm20/10; mm5 = mm20/4; mm10 = mm20/2; mm1 = mm20/20; ThreadControlActive = true; SetOriginMotorSpeed(30.0); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20); ThreadControlActive = false; }*/ bool dancerinvalid = false; int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; char TMessage[60]; uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; if (ThreadControlActive == false) return OK; if (PrepareState == true) return OK; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true) // return OK; DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) { MotorFailedSample[index]++; Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; } if (ReadValue == 0x3FFF) { MotorFailedSample[index]++; if (dancerinvalid == false) { dancerinvalid = true; LOG_ERROR(index, "Dancer value invalid."); } return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; 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]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; #ifdef TEST_LONGER_PID_THREAD else // test: handle tension once in pvinputfilterfactormode milliseconds return OK; #endif for (i=0;i eOneSecond) { if (ReadBreakSensor()==ERROR) { BreakSensorCounter++; BreakSensorLatchCounter++; if (BreakSensorCounter>=BreakSensordebouncetimemilli) { //consider applying the debouce parameters later usnprintf(TMessage, 60, "ReadBreakSensor Error"); //BreakSensordebouncetimemilli JobEndReason = JOB_THREAD_BREAK; ThreadControlActive = false; SendJobProgress(0.0,0,false, TMessage); SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); LOG_ERROR(index, "ReadBreakSensor Error"); return OK; } //passed limit }//ReadBreakSensor()==ERROR else //reset counter - we are looking for consequent calls { if (BreakSensorCounter) { LOG_ERROR(BreakSensorCounter, "ReadBreakSensor Spike"); } BreakSensorCounter = 0; } } } } //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { keepdata = false; usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); //JobAbortedByUser = true; ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; SendJobProgress(0.0,0,false, TMessage); //EndState(CurrentJob,TMessage ); SendSegmentFail(); /*switch (index) { case POOLER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,true); break; case FEEDER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,true); break; case WINDER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true); break; }*/ LOG_ERROR (DancerId, "Dancer Failure"); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; DancerError[DancerId] = NormalizedError; MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((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 { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); } else { //KeepNormalizedError = NormalizedError; } if ((JobCounter % 1000) == 0) { if (JobCounter >= 20000) { MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue); MotorSpeedSamplePointer[index]++; if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES) MotorSpeedSamplePointer[index] = 0; for (i=0;i MotorControlConfig[index].m_ingnoreValue) { /*if (keepdata == true) { calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; MotorId[controlIndex] = index; readValue[controlIndex] = ReadValue; AveragereadValue[controlIndex] = avreageSampleValue; calculatedspeed[controlIndex] = calculated_speed; NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam; mIntegral[controlIndex] = MotorControlConfig[index].m_integral; timestamp[controlIndex] = msec_millisecondCounter; if (controlIndex++>=MAX_THREAD_CONTROL_LOG) controlIndex = 0; }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } else MotorFailedSample[index]++; } return OK; } //******************************************************************************************************************** uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId) { return CurrentControlledSpeed[MotorId]; } //******************************************************************************************************************** double ThreadGetMotorCalculatedError(int DancerId) { switch (DancerId) { case FEEDER_DANCER: return (double)MotorControlConfig[FEEDER_MOTOR].m_calculatedError; case POOLER_DANCER: return (double)MotorControlConfig[POOLER_MOTOR].m_calculatedError; case WINDER_DANCER: return (double)MotorControlConfig[WINDER_MOTOR].m_calculatedError; } return 0; } //******************************************************************************************************************** uint32_t ThreadInitialTestStub(HardwareMotor * request) { //MotorsConfigMessage(request); ThreadPrepareState(request); ThreadPreSegmentState(request,0); return OK; } bool InitialProcess = false; //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; CurrentSegmentId = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; PrepareState = true; AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false); EnableLubrication = JobTicket->enablelubrication; EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ MotorControlConfig[Motor_i].m_params.MAX = 1; MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; MotorControlConfig[Motor_i].m_isReady = true; MotorControlConfig[Motor_i].m_mesuredParam = 0; MotorControlConfig[Motor_i].m_preError = 0; MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (SpeedControlId != 0xFF) { RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); SpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (PoolerSpeedControlId != 0xFF) { if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); PoolerSpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); PoolerSpeedControlId = AddControlCallback(PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } // if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled // AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled continue; } //testDancersControl(); PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions InitialProcess = true; return OK; } void SetOriginMotorSpeed(float process_speed) { int i,Motor_i, HW_Motor_Id; for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) // double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround * MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround) / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius); //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; CurrentControlledSpeed[Motor_i] = (int) motor_speed; for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) MotorSpeedSamples[Motor_i][i] = motor_speed; } } //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { //set the speed only before the first segment, speed is constant across all job segments and intersegments //JobSegment* Segment = SegmentDetails; float process_speed = dyeingspeed; if (dyeingspeed == 0) { LOG_ERROR (dyeingspeed," job speed zero"); return ERROR; } REPORT_MSG (dyeingspeed," ThreadPreSegmentState"); if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; PrepareState = false; // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); #endif if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0) { MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1); } if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].maxfrequency > 0) { MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1); } if (EnableLubrication == true) { IDS_StartLubrication(); } } // activate control fr all motors //set speed for both rocker motors //wait for all motors to get to the required speed (set the target speed for the control to check) //call the job state machine when the thread system is ready if ((InitialProcess==false) && (EnableIntersegment == true)) //&& (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0 {//add initial presegment and cleaning before first segment ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded); REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength"); SegmentState = false; PreSegmentState = true; DTSState = false; } else { ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); JobCounter = 0; InitialProcess = false; } return OK; } int REPSegmentId = 0; void SendSegmentFail(void) { if (SegmentState == true) SegmentReady(Module_Thread,ModuleFail); else if (PreSegmentState == true) PreSegmentReady(Module_Thread,ModuleFail); else if (DTSState == true) DistanceToSpoolReady(Module_Thread,ModuleFail); } void ThreadInterSegmentEnded(void) { REPORT_MSG (REPSegmentId,"ThreadInterSegmentEnded"); //ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); } void ThreadSegmentEnded(void) { REPORT_MSG (REPSegmentId," ThreadSegmentEnded"); SegmentReady(Module_Thread,ModuleDone); } void ThreadDistanceToSpoolEnded(void) { REPORT_MSG (REPSegmentId," ThreadDistanceToSpoolEnded"); DistanceToSpoolReady(Module_Thread,ModuleDone); } double seglength = 0.0; //******************************************************************************************************************** uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId) { JobSegment* Segment = SegmentDetails; REPSegmentId = SegmentId; seglength = Segment->length; CurrentSegmentId = SegmentId; REPORT_MSG (seglength," ThreadSegmentState"); ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded); SegmentState = true; PreSegmentState = false; DTSState = false; return OK; } //******************************************************************************************************************** uint32_t ThreadDistanceToSpoolState(void ) { seglength = dryerbufferlength; REPORT_MSG (seglength,"ThreadDistanceToSpoolState"); ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded); SegmentState = false; PreSegmentState = false; DTSState = true; return OK; } char Endstr[150]; //******************************************************************************************************************** uint32_t ThreadEndState(void ) { int Motor_i; ThreadControlActive = false; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); ThreadUpdateProcessLength (0.0,(void *)NULL); //TotalProcessedLength = 0.0; SetOriginMotorSpeed(0); #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogClose(); #endif if (SpeedControlId != 0xFF) { if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK) LOG_ERROR(SpeedControlId,"RemoveControl Failed"); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) LOG_ERROR(PoolerSpeedControlId,"RemoveControl Failed"); PoolerSpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed"); } MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); IDS_StopLubrication(); return OK; } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }