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 | 165 |
1 files changed, 91 insertions, 74 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 42a56ae8a..1c52a9b68 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -352,9 +352,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(); @@ -696,18 +696,19 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; - //if (JobCounter % eHundredMillisecond == 50) - if (FirstCalcInJob == true) + if (0)//(JobCounter % 1000 == 0) + //if (JobCounter < 100)//(FirstCalcInJob == true) { - if (index == POOLER_MOTOR) + if (index == FEEDER_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; + len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d", + 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,TMessage,__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); } } @@ -720,6 +721,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,CurrentControlledSpeed[index]+100,0); + calculated_speed=CurrentControlledSpeed[index]+100; + } CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } @@ -845,6 +851,7 @@ uint32_t Release_Right_TFU_Tension() return status; } int SecondFeederCorrection = 4; +int PrepareWaitCount = 0; uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) { MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands @@ -854,6 +861,18 @@ uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadVa 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); + } + return OK; } uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) @@ -869,16 +888,16 @@ uint32_t Adjust_Right_TFU_Tension(double tension) uint32_t status = OK; if (tension > 0.5) //0 = lower position, 1 = high position { + //PrepareWaitCount++; if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) { - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 40, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); + 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,GPI_LS_RDANCER_UP,0); } } return status; } -int PrepareWaitCount = 0; uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) { if (PrepareWaitCount) @@ -886,7 +905,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); PrepareWaitCount--; } - if (PrepareWaitCount == 0) + if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); @@ -1046,71 +1065,73 @@ uint32_t ThreadPrepareState(void *JobDetails) { 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 + 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!!! + 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); - } + 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; - // 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 + 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); + } + 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); + } 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) @@ -1152,10 +1173,6 @@ uint32_t ThreadPrepareState(void *JobDetails) 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 TEST_PID_THREAD @@ -1382,7 +1399,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); |