MotorStop, improve IDS, Dispensers pressure

1 files changed, 5 insertions, 3 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 7b5da0e65..e9cc3ddde 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -37,6 +37,7 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 uint32_t    SpeedControlId=0xFF;
 
+double DancerError[NUM_OF_DANCERS] = {0.0};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 uint32_t JobCounter = 0;
 typedef struct
@@ -304,6 +305,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }
         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_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
@@ -544,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
          {
              RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
          }
-        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+        MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
 
     return OK;
 }