diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 713 |
1 files changed, 535 insertions, 178 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 69f640c45..bc4e6cb8f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -37,6 +37,8 @@ #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 @@ -44,9 +46,15 @@ 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; @@ -58,6 +66,10 @@ 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; @@ -140,15 +152,22 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea * **************************************************************************************/ 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) { - REPORT_MSG(length,"ThreadUpdateProcessLength"); + ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; } -char Lenstr[160]; +char Lenstr[190]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -206,6 +225,16 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } } + /*FeederSpeedStore[FeederSpeedIndex++] = length; + if (FeederSpeedIndex>=SPEED_STORE_SIZE) + { + FeederSpeedIndex = 0; + FeederSpeedAverage = 0; + for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++) + FeederSpeedAverage+=FeederSpeedStore[Speed_i]; + FeederSpeedAverage = FeederSpeedAverage/SPEED_STORE_SIZE; + ReportWithPackageFilter(ThreadFilter,"Avg len 100ms last 2 sec",__FILE__,(int)PoolerTotalProcessedLength,(int)(FeederSpeedAverage*1000),RpWarning,(int)(PullerSpeedAverage*1000),0); + }*/ TotalProcessedLength += (length/100); TempTotalProcessedLength = TotalProcessedLength; #ifdef FEEDER_LENGTH_CALCULATION @@ -242,7 +271,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) return OK; } - uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; @@ -297,9 +325,22 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) length = 0; } + /*PullerSpeedStore[PullerSpeedIndex++] = length; + if (PullerSpeedIndex>=SPEED_STORE_SIZE) + { + PullerSpeedIndex = 0; + PullerSpeedAverage = 0; + for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++) + PullerSpeedAverage+=PullerSpeedStore[Speed_i]; + PullerSpeedAverage = PullerSpeedAverage/SPEED_STORE_SIZE; + //ReportWithPackageFilter(ThreadFilter,"Average Speed 2 second",__FILE__,__LINE__,(int)(FeederSpeedAverage*100),RpWarning,(int)(PullerSpeedAverage*100),0); + }**/ //} - +#ifdef BTSR_NO_TFU + if (CurrentControlledSpeed[WINDER_MOTOR]>100) + length = dyeingspeed/10; +#endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION @@ -322,9 +363,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { - usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + 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,RpWarning,(int)PoolerTotalProcessedLength,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -393,7 +434,72 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) } 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; @@ -447,20 +553,15 @@ void testDancersControl() } #endif int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; -char TMessage[150]; //char time[150]; -uint16_t BreakSensorCounter = 0; -uint16_t BreakSensorLatchCounter = 0; 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 len; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; @@ -480,6 +581,23 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } 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) @@ -511,6 +629,30 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } +#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 == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite @@ -529,61 +671,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; - if (BreakSensorenabled == true) - { - if (index == POOLER_MOTOR) - { - 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); - SendSegmentFail(); - //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); - //EndState(CurrentJob,"ReadBreakSensor Error" ); - ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); - return OK; - } //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"); - 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 OK; - } - } - - } - } - } //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) @@ -595,7 +682,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) 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) @@ -647,21 +739,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + if (index < WINDER_MOTOR) + calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; - if (FirstCalcInJob == true) + //if (0)//(JobCounter % 1000 == 0) +#ifdef FOUR_WINDERS + if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) { - if (index == FEEDER_MOTOR) + if (index >= WINDER_MOTOR) { - FirstCalcInJob = false; - len = 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), + // 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); - ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + (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 @@ -670,6 +767,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { 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); } @@ -684,7 +786,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); }*/ #ifdef TEST_PID_THREAD - int len; if ((JobCounter % 2000) == index*100) //if (keepdata == true) { @@ -696,7 +797,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam; mIntegral[controlIndex] = MotorControlConfig[index].m_integral; timestamp[controlIndex] = msec_millisecondCounter;*/ - len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", + 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); @@ -740,64 +841,166 @@ double ThreadGetMotorCalculatedError(int DancerId) } //******************************************************************************************************************** -uint32_t ThreadInitialTestStub(HardwareMotor * request) -{ - - - //MotorsConfigMessage(request); - ThreadPrepareState(request); - ThreadPreSegmentState(request,0); - return OK; -} uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) { - /* - FEEDER_MOTOR, - DRYER_MOTOR, - POOLER_MOTOR, - WINDER_MOTOR, - 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; - - */ 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.Kd = 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.Kd = 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.Kd = 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_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_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_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_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; - if (tension < 100) - return OK; //do not handle tension of zero switch (DancerId) { case HARDWARE_DANCER_TYPE__LeftDancer: @@ -811,10 +1014,13 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2; break; case HARDWARE_DANCER_TYPE__RightDancer: - return ERROR; + 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; @@ -832,24 +1038,37 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize; movement = current - request; } - MotorSetMaxSpeed (HW_Motor_Id, 500); - MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000); + 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); } - usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d", - DancerId,request,current,movement,direction,HW_Motor_Id,address); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + 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; } //******************************************************************************************************************** - uint32_t ThreadPrepareState(void *JobDetails) +bool SkipOpenLids = true; +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; @@ -873,13 +1092,15 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; + PrepareWaitCount = 0; +#ifndef BTSR_NO_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,status,RpFatalError,(int)windertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,status,RpFatalError,(int)pullertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,status,RpFatalError,(int)feedertension,0); - + 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) { @@ -893,19 +1114,57 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) } 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) + /*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,RpFatalError,LIMIT,0); + 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,RpFatalError,LIMIT,0); + 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 @@ -913,61 +1172,81 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) { 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 +#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] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0); //reset the samples value for control beginning - // 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; + 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); + 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 +#ifndef BTSR_NO_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) { - 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); + RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); + SpeedControlId = 0xFF; } - 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 + //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 + 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) { - 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); + 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); + } +#ifndef BTSR_NO_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) @@ -995,6 +1274,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) 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); @@ -1009,25 +1289,87 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } -// 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 == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - continue; +#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 } - HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! #ifdef TEST_PID_THREAD testDancersControl(); #endif - PrepareReady(Module_Thread,ModuleDone); + if (PrepareWaitCount == 0) + PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions - return OK; + return status; } +uint32_t UpdatePidDuringRun(HardwarePidControl *request) +{ + int Motor_i = MAX_THREAD_MOTORS_NUM,i; + double temp_dt; + + for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) + { + if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype) + { + Motor_i = i; + break; + } + } + if (Motor_i == MAX_THREAD_MOTORS_NUM) + return ERROR; + + if (request->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->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->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->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 <= WINDER_MOTOR; Motor_i++) + for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) @@ -1100,7 +1442,11 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; - PrepareState = false; + 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) @@ -1111,7 +1457,7 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps } - ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); + 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); @@ -1235,7 +1581,7 @@ char Endstr[150]; int Motor_i; ThreadControlActive = false; uint32_t status = OK; - usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); + 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); @@ -1258,7 +1604,7 @@ char Endstr[150]; PoolerSpeedControlId = 0xFF; } - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { @@ -1271,18 +1617,29 @@ char Endstr[150]; } Task_sleep(100); - for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) + 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) |