Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

1 files changed, 6 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6db7f616e..dc9cfcd6d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -58,6 +58,7 @@ typedef struct
     float               m_integral;
     float               m_calculatedError;
     bool                m_isReady;
+    uint32_t            m_ingnoreValue;
     PID_Config_Params   m_params;
 }MotorControlConfig_t;
 
@@ -387,7 +388,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         if (ReadValue < 10)
         {
             MotorFailedSample[index]++;
-            REPORT_MSG(ReadValue, "Dancer value read too small.");
+            Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
             return OK;
         }
         if (ReadValue == 0x3FFF)
@@ -469,7 +470,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             //KeepNormalizedError = NormalizedError;
         }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
         {
             if (keepdata == true)
             {
@@ -560,6 +561,7 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             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;
@@ -768,7 +770,7 @@ char Endstr[150];
      if (SpeedControlId != 0xFF)
      {
          if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK)
-             LOG_ERROR(Motor_i,"RemoveControl Failed");
+             LOG_ERROR(SpeedControlId,"RemoveControl Failed");
          SpeedControlId = 0xFF;
      }
      if (PoolerSpeedControlId != 0xFF)
@@ -783,7 +785,7 @@ char Endstr[150];
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {
              if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK)
-                 ControlIdtoMotorId[Motor_i] == 0xFF;
+                 ControlIdtoMotorId[Motor_i] = 0xFF;
              else
                  LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed");
          }