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 | 45 |
1 files changed, 38 insertions, 7 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index cfff7773b..1ae1145fb 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -72,6 +72,7 @@ void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); double KeepNormalizedError = 0; +bool ThreadControlActive = false; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); @@ -122,8 +123,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; initialpos = 0xFFFF; } -double MotorSentData[1000] = {0}; -uint32_t PosDif[1000] = {0}; +double MotorSentData[100] = {0}; +uint32_t PosDif[100] = {0}; int MotorDataIndex = 0; @@ -175,7 +176,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; MotorDataIndex+=1; - if (MotorDataIndex == 999) MotorDataIndex = 0; + if (MotorDataIndex == 99) MotorDataIndex = 0; static int pooler_counter = 0; pooler_counter++; if (pooler_counter%10 == 0) @@ -248,6 +249,12 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } +double calculatedError[1000]; +int readValue[1000]; +int calculatedspeed[1000]; +int controlIndex = 0; +int32_t KeepReadValue = 0; + uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -259,6 +266,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) int DancerId; static int pooler_counter = 0; int32_t TranslatedReadValue, avreageSampleValue = 0; + double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; char Message[60]; @@ -324,10 +332,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit + { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); + } else - KeepNormalizedError = NormalizedError; + { + //KeepNormalizedError = NormalizedError; + } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; + if (index == POOLER_MOTOR) + { + if (KeepReadValue != TranslatedReadValue) + { + calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; + readValue[controlIndex] = TranslatedReadValue; + calculatedspeed[controlIndex] = calculated_speed; + controlIndex++; + if (controlIndex >= 999) controlIndex = 0; + KeepReadValue = TranslatedReadValue; + } + } if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { CurrentControlledSpeed[index] = calculated_speed; @@ -393,7 +417,7 @@ bool InitialProcess = false; 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.epsilon = 0.1; - MotorControlConfig[Motor_i].m_params.dt = 50; + MotorControlConfig[Motor_i].m_params.dt = 1000; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; @@ -489,10 +513,16 @@ uint32_t ThreadPreSegmentState(void *JobDetails) float process_speed; if (JobTicket->processparameters) - process_speed= JobTicket->processparameters->dyeingspeed; + if (JobTicket->processparameters->dyeingspeed) + process_speed= JobTicket->processparameters->dyeingspeed; + else + { + LOG_ERROR (-1," unknown job speed"); + return ERROR; + } SetOriginMotorSpeed(process_speed); - //ControlStart(); + ThreadControlActive = true; // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); //only for testing - when control works, these motors will take their speed from the dryer @@ -556,6 +586,7 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) uint32_t ThreadEndState(void *JobDetails) { int Motor_i; + ThreadControlActive = false; ThreadUpdateProcessLength (0.0,(void *)NULL); SetOriginMotorSpeed(0); |