some small changes, mostly in drier loading

1 files changed, 39 insertions, 10 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index f5bc45d85..2c8db8b6c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -226,6 +226,19 @@
         }
         return OK;
     }
+    uint32_t Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+
+        Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
+
+        Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+        NumberOfDrierLoaderCycles=0;
+        //storeLoadArmParameters();
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+
+        return OK;
+    }
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
 
@@ -600,7 +613,7 @@
     {
         numberOfCycles++;
 
-        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0);
+        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,numberOfCycles,0);
 
         if ((fabs (TotalLoadedLen  -DrierPrevLocation)>50  )&&(ReadValue == NOTBUSY))
         {
@@ -618,7 +631,7 @@
             }
             else
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -803,6 +816,7 @@
     }
     uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
+        bool direction;
         numberOfCycles++;
         uint32_t temp = Read_Dryer_ENC_Position();
         Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
@@ -823,7 +837,7 @@
             }
             else //done enough cycles, go to the center point
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -831,7 +845,23 @@
         else  //timeout or no movement
         {
             Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
-            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
+            if (SecondTry == true)
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
+            else
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
+            if (abs(temp -DrierPrevLocation)<200)
+            {
+                if (temp<DrierPrevLocation)
+                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                else
+                    direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction);
+
+            }
+            else
+                MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+
+
             TimeoutsCounter = 0;
             CallbackCounter = 0;
             /*if(PullerControlId != 0xFF)
@@ -853,7 +883,6 @@
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
 
             MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
-            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
             load.color = fastBILNK;
             usnprintf(LoadErrorMsg, 100, "Stage %s - Drier load arm timeout",LoadStagesStr[LoadStages]);
             Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
@@ -1210,15 +1239,15 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
 
     NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-    MotorControlConfig[index].m_mesuredParam = 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
     {
-        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        NormalizedError = (-1*NormalizedError);
     }
+    MotorControlConfig[index].m_mesuredParam = 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);
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    //if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);