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

1 files changed, 24 insertions, 15 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 1f9897905..1575123e4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -68,7 +68,7 @@
 
     uint8_t NumberOfDrierLoaderCycles = 0;
     uint32_t status = OK;
-    uint32_t ControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF;
+    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
     uint32_t Thread_Load_Dryer_UnLoading(void);
@@ -425,11 +425,11 @@
     {
         Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
-        if(ControlId != 0xFF)
+        if(LoadingControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
-            ControlId = 0xFF;
+            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
+            LoadingControlId = 0xFF;
         }
         if (CallbackCounter)
         {
@@ -452,11 +452,11 @@
     {
         Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,__LINE__,MotorId,RpMessage,CallbackCounter,0);
-        /*if(ControlId != 0xFF)
+        /*if(LoadingControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
-            ControlId = 0xFF;
+            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
+            LoadingControlId = 0xFF;
         }*/
         MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
         return OK;
@@ -499,8 +499,8 @@
 
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4);
 
-        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
-        Report("AddControlCallback feeder",__FILE__,__LINE__,ControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0);
+        LoadingControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0);
         CallbackCounter++;
         Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
@@ -515,8 +515,8 @@
     uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
-        ControlId = 0xFF;
+        SafeRemoveControlCallback(LoadingControlId, Thread_Load_Jog_ThreadStop );
+        LoadingControlId = 0xFF;
         ThreadAbortJoggingFunc();
         LoadStages++;
         ThreadLoadStateMachine(LoadStages);
@@ -527,17 +527,17 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ThreadJoggingFunc(20);
-        ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        LoadingControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         return OK;
     }
     uint32_t Thread_Load_End(void)
     {
         REPORT_MSG(LoadStages,"Loading Ended");
-        if(ControlId != 0xFF)
+        if(LoadingControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
-            ControlId = 0xFF;
+            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
+            LoadingControlId = 0xFF;
         }
         if(PullerControlId != 0xFF)
         {
@@ -551,6 +551,15 @@
             RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
             WinderControlId = 0xFF;
         }
+
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RDANCER);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LDANCER1);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID);
+        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
         LoadStages = THREAD_LOAD_INIT;
         return OK;
     }