/************************************************************************************************************************ * 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 "PMR/Printing/ThreadParameters.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 "drivers/FPGA/FPGA_SPI_Comm.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" #include "drivers/Flash_ram/MCU_E2Prom.h" #include "drivers/SSI_Comm/SSI_Comm.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 /////////////////////////////////////////////////////////////////////////////////////////// double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; #ifdef FOUR_WINDERS 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,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS}; uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; #else 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}; #endif uint32_t SpeedControlId=0xFF; uint32_t PoolerSpeedControlId=0xFF; double DancerError[NUM_OF_DANCERS] = {0.0}; double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; double InitialDryerSpeed = 0.0; uint32_t JobCounter = 0; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; int MotorTiming[MAX_THREAD_MOTORS_NUM]; int MotorTimer[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 InitialProcess = false; bool PrepareState = false; // job parameters bool EnableLubrication = false; bool EnableIntersegment = false; double IntersegmentLength = 0; bool Thread_Rockers_Bypass = false; 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; ReportWithPackageFilter(ThreadFilter,"Length rollover",__FILE__,Time_Pass,(int)Current_Read,RpWarning,(int)Previous_Read,0); } else Time_Pass = Current_Read - Previous_Read; return (Time_Pass); } /***************************************************************************************** * * * * * * * **************************************************************************************/ uint32_t initialpos = 0xFFFF; uint32_t Poolerinitialpos = 0xFFFF; #define SPEED_STORE_SIZE 20 float PullerSpeedStore[SPEED_STORE_SIZE]; float PullerSpeedAverage; int PullerSpeedIndex = 0; float FeederSpeedStore[SPEED_STORE_SIZE]; float FeederSpeedAverage; int FeederSpeedIndex = 0,Speed_i; void ThreadUpdateProcessLength (double length, void *Funcptr) { ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; } char Lenstr[190]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; #ifndef FEEDER_LENGTH_CALCULATION if (ThreadControlActive == false) return OK; if (PrepareState == true) return OK; #endif if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; // if (CurrentRequestedLength == 0.0) // return OK; if (index != FEEDER_MOTOR) { ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); return 0xFFFFFFFF; } CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); if (CurrentPosition != 0) { if (initialpos == 0xFFFF) { PreviousPosition = CurrentPosition; initialpos = 0; } prevprev = PreviousPosition; if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction positionDiff = Control_Delta_Position_Pass(PreviousPosition,CurrentPosition); else 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; if (length > 1000) { usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev); SendJobProgress(0.0,0,false, Lenstr); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); length = 0; } } /*FeederSpeedStore[FeederSpeedIndex++] = length; if (FeederSpeedIndex>=SPEED_STORE_SIZE) { FeederSpeedIndex = 0; FeederSpeedAverage = 0; for (Speed_i = 0;Speed_i=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); } #endif return OK; } uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; #ifdef FEEDER_LENGTH_CALCULATION if (ThreadControlActive == false) return OK; if (PrepareState == true) return OK; #endif if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; // if (CurrentRequestedLength == 0.0) // return OK; if (index != POOLER_MOTOR) { ReportWithPackageFilter(ThreadFilter,"Wrong Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0); return 0xFFFFFFFF; } PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); //if (PoolerCurrentPosition != 0) //{ if (Poolerinitialpos == 0xFFFF) { PoolerPreviousPosition = PoolerCurrentPosition; Poolerinitialpos = 0; } prevprev = PoolerPreviousPosition; if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction positionDiff = Control_Delta_Position_Pass(PoolerPreviousPosition,PoolerCurrentPosition); else positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PoolerPreviousPosition = PoolerCurrentPosition; // total length = (position diff / full cycle) * pulley perimeter //(positionDiff/pulseperround)*((2*PI*motor_Radius) //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; length = (double)(positionDiff)*PoolerLengthCalculationMultiplier; if (length > 1000) { usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev); SendJobProgress(0.0,0,false, Lenstr); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); length = 0; } /*PullerSpeedStore[PullerSpeedIndex++] = length; if (PullerSpeedIndex>=SPEED_STORE_SIZE) { PullerSpeedIndex = 0; PullerSpeedAverage = 0; for (Speed_i = 0;Speed_i100) length = dyeingspeed/10; #endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION CurrentProcessedLength+=length; static int puller_counter = 0; puller_counter++; if (puller_counter%10 == 0) { if (PrepareState == true) { TemperatureListString(Lenstr); SendJobProgress(0.0,0,false, Lenstr); } else { SendJobProgress(PoolerTotalProcessedLength,0,false, NULL); } } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); } #endif 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; double calculated_speed; float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += SpeedSamples[i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0)) { MotorControlConfig[index].m_mesuredParam = ReadValue; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError); calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (fabs(calculated_speed-CurrentControlledSpeed[index])>2) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } } return OK; } float _speed; uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) { int index; if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { int MotorId = ThreadMotorIdToMotorId[index]; _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId); } return OK; } uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; char TMessage[150]; #ifdef FOUR_WINDERS char ATMessage[MAX_THREAD_MOTORS_NUM][150]; int c = 0; #endif uint32_t checkBreakSensor(uint32_t index) { if (BreakSensorenabled == true) { if (JobCounter > 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); strcpy(AlarmReasonStr,TMessage); SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return ERROR; } //passed limit }//ReadBreakSensor()==ERROR else //reset counter - we are looking for consequent calls { if (BreakSensorCounter) { ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); } BreakSensorCounter = 0; } if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3)) { BreakSensorCounter++; BreakSensorLatchCounter++; if (BreakSensorCounter>=BreakSensordebouncetimemilli) { //consider applying the debouce parameters later usnprintf(TMessage, 60, "thread speed too low"); strcpy(AlarmReasonStr,TMessage); JobEndReason = JOB_THREAD_BREAK; ThreadControlActive = false; SendJobProgress(0.0,0,false, TMessage); SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return ERROR; } } } } return OK; } int controlIndex = 0; bool keepdata = true; int32_t KeepReadValue = 0; //double eNormalizedError[100]; //int TranslatedreadValue[100]; #ifdef TEST_PID_THREAD #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]; 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; char time[150]; int len; ThreadControlActive = true; SetOriginMotorSpeed(50.0); MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = 0; MotorsControl[FEEDER_MOTOR].controloutputtype = 0.001; MotorControlConfig[FEEDER_MOTOR].m_params.dt = 0.001; DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint = 10000; MotorsControl[FEEDER_MOTOR].pvinputfilterfactormode = 1; len = usnprintf(time, 150, "params: speed 50, divider %d p %d * %d i %d * %d Dt*1000 %d Norm Coef %d initial speed %d",NORMAL_COEF_DIVIDER,(int)MotorsControl[FEEDER_MOTOR].proportionalgain,(int)MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue, (int)MotorsControl[FEEDER_MOTOR].integraltime,(int)MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp,(int)(MotorsControl[FEEDER_MOTOR].controloutputtype*1000), (int)(NormalizedErrorCoEfficient[FEEDER_MOTOR]*1000000000),OriginalMotorSpd_2PPS[FEEDER_MOTOR]); ReportWithPackageFilter(ThreadFilter,time,__FILE__,__LINE__,111,RpError,111,0); Task_sleep(100); 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; } #endif int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; //char time[150]; bool FirstCalcInJob = true; 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; double calculated_speed; double NormalizedError; if (ThreadControlActive == false) return OK; #ifndef TEST_PID_THREAD if (PrepareState == true) return OK; #endif if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; if (index == POOLER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec { if (checkBreakSensor(index) == ERROR) return OK; } if (MotorTiming[index]>1) { MotorTimer[index]++; if (MotorTimer[index]>=MotorTiming[index]) { MotorTimer[index]=0; } else { return OK; } } if(MotorControlConfig[index].m_isEnabled ) { //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true) // return OK; DancerId = ThreadMotorIdToDancerId[index]; /* if (ReadValue < 10) { MotorFailedSample[index]++; ReportWithPackageFilter(ThreadFilter,"Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; }*/ if (ReadValue == 0x3FFF) { if (Read_Dryer_Status(DancerId) != OK) { ReportWithPackageFilter(ThreadFilter,"Dancer value invalid.",__FILE__,ReadValue,(int)DancerId,RpError,(int)Read_Dryer_Status(DancerId),0); MotorFailedSample[index]++; return OK; } } KeepReadValue = ReadValue; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (abs(TranslatedReadValue) > 0x2000) { TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment } if ((index == POOLER_MOTOR)||((index == FEEDER_MOTOR)&&(DancersCfg[DancerId].assemblydirectionright == true))) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } #ifdef BTSR_ROTATED_WINDER_TFU if (index == WINDER_MOTOR) TranslatedReadValue = (-1*TranslatedReadValue); #endif #ifdef FOUR_WINDERS if (index == WINDER_MOTOR) { c++; } if (index == WINDER_2_MOTOR) { c++; } if (index == WINDER_3_MOTOR) { c++; } if (index == WINDER_4_MOTOR) { c++; } //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR)) /*if (index >= WINDER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); }*/ #endif if (index == WINDER_MOTOR) { //pooler dancer is right sided: data is opposite JobCounter++; } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= (int)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<(int)MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; //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); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); //JobAbortedByUser = true; ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; #ifdef FOUR_WINDERS if (DancerId>HARDWARE_DANCER_2) JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0; #endif SendJobProgress(0.0,0,false, TMessage); strcpy(AlarmReasonStr,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; }*/ ReportWithPackageFilter(ThreadFilter,"Dancer Failure",__FILE__,DancerId,(int)avreageSampleValue,RpError,(int)JobCounter,0); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit { NormalizedError = (-1*NormalizedError); } 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); /*else { //KeepNormalizedError = NormalizedError; }*/ /*if ((JobCounter % 100) == 0) { //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit //{ // ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); //} /`*if (JobCounter >= 3000) { MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue); MotorSpeedSamplePointer[index]++; if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES) MotorSpeedSamplePointer[index] = 0; for (i=0;i= WINDER_MOTOR) { // FirstCalcInJob = false; usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1, TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); } JobCounter++; } #endif #ifndef TEST_PID_THREAD if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) #else if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit #endif { if (calculated_speed>5.0) { /*if (calculated_speed>(CurrentControlledSpeed[index]+100)) { ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,index,0); calculated_speed=CurrentControlledSpeed[index]+100; }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } else { if (calculated_speed<0) ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); } /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit { ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); }*/ #ifdef TEST_PID_THREAD if ((JobCounter % 2000) == index*100) //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;*/ usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); //Task_sleep(100); //if (controlIndex++>=MAX_THREAD_CONTROL_LOG) // controlIndex = 0; } #endif } else { MotorFailedSample[index]++; //LOG_ERROR(index,"No change in speed"); } } return OK; } //******************************************************************************************************************** double 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 HandleJobThreadControlParameters(ThreadParameters* ThreadParams) { if (ThreadParams == NULL) { return OK; } if((ThreadParams->bypassrockers != true)&&(ThreadParams->bypassrockers != false)) { ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); return OK; } if ((ThreadParams->feederp>100000)||(ThreadParams->feederi>100000)) { ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); return OK; } if(ThreadParams->feederp) MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp; if(ThreadParams->feederi) MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi; if(ThreadParams->feederd) MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd; if(ThreadParams->pullerp) MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp; if(ThreadParams->pulleri) MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri; if(ThreadParams->pullerd) MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd; if(ThreadParams->winderp) { MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp; MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp; MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp; #endif } if(ThreadParams->winderi) { MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi; MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi; MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi; #endif } if(ThreadParams->winderd) { MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd; MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd; MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd; #endif } ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0); if(ThreadParams->bypassrockers) Thread_Rockers_Bypass = true; return OK; } bool RTFU_Up = false; uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag) { //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0); MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands return OK; } uint32_t Release_Right_TFU_Tension() { uint32_t status = OK; #ifndef BTSR_NO_FEEDER_TFU if (RTFU_Up == true) { Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0); RTFU_Up = false; status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000); } #endif return status; } int SecondFeederCorrection = 4; int PrepareWaitCount = 0; uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) { #ifndef BTSR_NO_FEEDER_TFU MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0); if (JobIsActive()==false) { Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0); Release_Right_TFU_Tension(); } if (PrepareWaitCount) { ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); PrepareWaitCount--; } if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } #endif return OK; } uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) { #ifndef BTSR_NO_FEEDER_TFU Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0); MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000); RTFU_Up = true; #endif return OK; } uint32_t Adjust_Right_TFU_Tension(double tension) { uint32_t status = OK; #ifndef BTSR_NO_FEEDER_TFU if (tension > 0.5) //0 = lower position, 1 = high position { if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) { PrepareWaitCount++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0); } } #endif return status; } uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension) { //MotorStop(MotorId,Hard_Hiz); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); if (PrepareWaitCount) { PrepareWaitCount--; } if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } return OK; } uint32_t ThreadPrepare_Tension (int DancerId, double tension) { int current, request = (int)tension,movement; int HW_Motor_Id; bool direction; uint32_t status = OK, address = 0; switch (DancerId) { case HARDWARE_DANCER_TYPE__LeftDancer: address = EEPROM_WINDER_TENSION_POSITION; HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER1; break; case HARDWARE_DANCER_TYPE__MiddleDancer: if (Is_PP_Machine() == false) //LP machine - old LTFU return OK; address = EEPROM_PULLER_TENSION_POSITION; HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2; break; case HARDWARE_DANCER_TYPE__RightDancer: return Adjust_Right_TFU_Tension(tension); //break; default: return ERROR; } if (tension < 100) return OK; //do not handle tension of zero status |= MCU_E2PromRead(address,¤t); if ((status!= OK )||(current == 0xFFFF)) return status; if (abs(current - request)<100) return status; else { if (current < request) //go down { direction = MotorsCfg[HW_Motor_Id].directionthreadwize; movement = request - current; } else { direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize; movement = current - request; } MotorSetMaxSpeed (HW_Motor_Id, 800); MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000); PrepareWaitCount++; ReportWithPackageFilter(ThreadFilter,"PrepareWaitCount",__FILE__,PrepareWaitCount,current,RpWarning,request,0); status |= MCU_E2PromProgram(address,request); } if (DancerId == HARDWARE_DANCER_TYPE__LeftDancer) { usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,current,RpWarning,request,0); } else { usnprintf(TMessage, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,address,current,RpWarning,request,0); } return status; } //******************************************************************************************************************** bool SkipOpenLids = false; uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i,i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; uint32_t status = OK; CurrentSegmentId = 0; float temp_dt = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; InitialProcess = true; initialpos = 0xFFFF; Poolerinitialpos = 0xFFFF; 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); AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,false); // status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); // double feedertension = 0; // double pullertension = 0; // double windertension = 0; EnableLubrication = JobTicket->enablelubrication; EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; PrepareWaitCount = 0; status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); #ifndef BTSR_NO_PULLER_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); #endif #ifndef BTSR_NO_FEEDER_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0); #endif FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING; } else { ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop); /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; } if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; }*/ // if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false)) if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false)) { if(Head_Type != HEAD_TYPE_ARC) { ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0); if (SkipOpenLids == false) { JobEndReason = JOB_LIDS_OPEN; usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); PrepareReady(Module_Thread,ModuleFail); return ERROR; } } } if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } } // if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false)) if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false)) { ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0); if (SkipOpenLids == false) { JobEndReason = JOB_LIDS_OPEN; usnprintf(AlarmReasonStr, 100, "Dryer lid is open!!!"); PrepareReady(Module_Thread,ModuleFail); return ERROR; } } //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];*/ #ifdef FOUR_WINDERS if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR; if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR; if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR; #endif 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 HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; if (MotorTiming[Motor_i]) { MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); } ////////////////////////////////////////////////// for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) { //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled MotorSamples[Motor_i][i] = 0; if (Motor_i == FEEDER_MOTOR) MotorSamples[Motor_i][i] = -500; // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; //MotorSpeedSamples[Motor_i][i] = 0; } MotorSamplePointer[Motor_i] = 0; ///////////////////////////////////////////////////// MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); #ifndef BTSR_NO_FEEDER_TFU 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 { ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); 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(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } #endif //#ifndef BTSR_NO_PULLER_TFU 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 { ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (PoolerSpeedControlId != 0xFF) { if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); 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(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } //#endif #ifndef BTSR_NO_FEEDER_TFU 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) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); #endif } #endif #ifndef BTSR_NO_PULLER_TFU 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) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #endif 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 { ReportWithPackageFilter(ThreadFilter,"Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #ifdef FOUR_WINDERS if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0); if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #endif } #ifdef TEST_PID_THREAD testDancersControl(); #endif if (PrepareWaitCount == 0) PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions return status; } uint32_t UpdatePidDuringRun(HardwarePidControl *request) { int Motor_i = MAX_THREAD_MOTORS_NUM,i; double temp_dt; for (i=0;ihardwarepidcontroltype) { Motor_i = i; break; } } if (Motor_i == MAX_THREAD_MOTORS_NUM) return ERROR; if (request->has_derivativetime == true) { MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0); } if (request->has_proportionalgain == true) { MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0); } if (request->has_integraltime == true) { MotorControlConfig[Motor_i].m_params.Ki = request->integraltime; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0); } if (request->has_epsilon == true) { MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0); } if (request->has_controloutputtype == true) { MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype; temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; if (MotorTiming[Motor_i]) { MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; } ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0); } ////////////////////////////////////////////////// return OK; } void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; for (Motor_i = 0; Motor_i < SCREW_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; InitialDryerSpeed = 0.0; CurrentControlledSpeed[Motor_i] = (int) motor_speed; if (process_speed > 1) ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0); // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) // MotorSpeedSamples[Motor_i][i] = motor_speed; } } void ThreadPreSegmentEnded(void) { InitialProcess = false; REPORT_MSG (0,"First ThreadPreSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } int DrierDivider = 20; uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag) { InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider); if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR]) { InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]; SafeRemoveControlCallback(ControlIdtoMotorId[DRYER_MOTOR], ThreadDryerRampUp ); ControlIdtoMotorId[DRYER_MOTOR] = 0xFF; //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp end",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); } if (InitialDryerSpeed == 0) { //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp Stopped",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); return OK; } MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); return OK; } bool Set_Thread_Rockers_Bypass (int value) { if (value == 0) Thread_Rockers_Bypass = false; else Thread_Rockers_Bypass = true; return Thread_Rockers_Bypass; } //******************************************************************************************************************** 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) { ReportWithPackageFilter(ThreadFilter,"job speed zero.",__FILE__,__LINE__,(int)dyeingspeed,RpError,(int)SegmentId,0); return ERROR; } ReportWithPackageFilter(ThreadFilter,"ThreadPreSegmentState",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)SegmentId,0); 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; if (PrepareWaitCount == 0) PrepareState = false; PullerSpeedIndex = 0; FeederSpeedIndex = 0; #ifndef TEST_PID_THREAD // set the new speed in the dryer motor to the speed of the new segment if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { DrierDivider = dyeingspeed/3; //ramp up drier in 5 cm/sec steps } else { DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps } ReportWithPackageFilter(ThreadFilter,"Dryer ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0); #endif #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); #endif if (Thread_Rockers_Bypass == false) { 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 if (InitialProcess==true) { ThreadUpdateProcessLength (dryerbufferMeters,(void *)ThreadPreSegmentEnded); REPORT_MSG (dryerbufferCentimeters," ThreadPreSegmentState DTS length (sample)"); 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 = dryerbufferMeters; REPORT_MSG (seglength,"ThreadDistanceToSpoolState"); //#ifdef FEEDER_LENGTH_CALCULATION ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded); /*#else ThreadUpdateProcessLength (0,(void *)NULL); //move DTS to job start DistanceToSpoolReady(Module_Thread,ModuleDone); #endif*/ SegmentState = false; PreSegmentState = false; DTSState = true; return OK; } char Endstr[150]; //******************************************************************************************************************** uint32_t ThreadEndState(void ) { int Motor_i; ThreadControlActive = false; uint32_t status = OK; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); ReportWithPackageFilter(ThreadFilter,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) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)ThreadLengthCBFunction,RpError,(int)SpeedControlId,0); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)PoolerThreadLengthCBFunction,RpError,(int)PoolerSpeedControlId,0); PoolerSpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); if(status == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0); } } Task_sleep(100); for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); Release_Right_TFU_Tension(); IDS_StopLubrication(); return OK; } void ThreadCheckArcHeadCovers(void) { if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } } } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }