diff options
| author | Roy Ben-Shabat <Roy@Twine-s.com> | 2018-11-29 18:59:55 +0200 |
|---|---|---|
| committer | Roy Ben-Shabat <Roy@Twine-s.com> | 2018-11-29 18:59:55 +0200 |
| commit | ae614a63f593122cd28c644625db179f298dd640 (patch) | |
| tree | 4ee706e22277d88a5d5dbd0c4fa9dc1ec0377477 /Software/Embedded_SW/Embedded/Modules/Thread | |
| parent | dd724f118f584eea84a11af90292a88c62ecd022 (diff) | |
| parent | 536b311b7dd2f571e38b9c38c1190ce01c549815 (diff) | |
| download | Tango-ae614a63f593122cd28c644625db179f298dd640.tar.gz Tango-ae614a63f593122cd28c644625db179f298dd640.zip | |
Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 17 |
1 files changed, 10 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 5390ed0a7..15ea5157b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -319,7 +319,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) #define MAX_THREAD_CONTROL_LOG 300 double calculatedError[MAX_THREAD_CONTROL_LOG+1]; double NormError[MAX_THREAD_CONTROL_LOG+1]; -double Integral[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]; @@ -452,13 +452,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //EndState(CurrentJob,Message ); SegmentReady(Module_Thread,ModuleFail); AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,true); - LOG_ERROR (index, "Dancer Failure"); + LOG_ERROR (DancerId, "Dancer Failure"); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; 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_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { @@ -478,9 +478,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) 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; - NormError[controlIndex]= NormalizedError; - Integral[controlIndex] = MotorControlConfig[index].m_integral; if (controlIndex++>=MAX_THREAD_CONTROL_LOG) controlIndex = 0; } @@ -555,8 +556,10 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) 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.epsilon = 0.1; - MotorControlConfig[Motor_i].m_params.dt = 1000; + 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_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; |