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

1 files changed, 55 insertions, 11 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index c5cdb00d6..856812c12 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -18,6 +18,7 @@
 #include "Modules/control/pidalgo.h"
 
 #include "Modules/heaters/heaters_ex.h"
+#include "Modules/General/buttons.h"
 #include "StateMachines/Initialization/InitSequence.h"
 
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
@@ -151,7 +152,7 @@
         ProcessParametersClear.headzone6temp  = 0;
         ProcessParametersClear.dyeingspeed = 40;
         ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength;
-        if (HandleProcessParameters(&ProcessParametersClear)!= OK)
+        if (HandleProcessParameters(&ProcessParametersClear,false)!= OK)
         {
             LOG_ERROR (LoadStages, "Thread_Load_Init failed");
         }
@@ -223,7 +224,7 @@
         ThreadLoadStateMachine(LoadStages);
         return OK;
     }
-    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
+    /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
     {
         uint32_t numberOfSteps = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
@@ -238,14 +239,14 @@
         }
         else //number of circles is not known - compare to position of the motor to verify location at the stopper
         {
-            Report("Thread_Load_Set_Load_Arm_To_Start_Position unknown cycles",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
-                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
 
         }
         //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
         return OK;
-    }
+    }*/
     uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
         CallbackCounter--;
@@ -265,6 +266,12 @@
         {
             if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING))
             {
+                if(LoadStages == THREAD_LOAD_CLOSE_ROCKERS)
+                {
+                    MotorMove(MotorId,MotorsCfg[MotorId].directionthreadwize,400);
+			        Report("Move rockers further down",__FILE__,__LINE__,MotorId,RpMessage,400,0);
+                    Task_sleep(200);
+                }
                 MotorSetMicroStep(MotorId, keepmicrostep);
                 MotorSetKvalRun(MotorId, keepkvalrun);
             }
@@ -273,6 +280,7 @@
         if (CallbackCounter)
         {
             CallbackCounter--;
+            load.color = BLINK;
             if (ReadValue != LIMIT)
             {
                 //returned with a timeout
@@ -284,10 +292,12 @@
                 {
                     LOG_ERROR(LoadStages,"Load sequence timeout");
                     TimeoutsCounter = 0;
+                    load.color = fastBILNK;
                 }
                 else
                 {
                     LoadStages++;
+                    load.color = colorON;
                     if (LoadStages == THREAD_LOAD_CLOSE_DANCERS)
                     {
                         MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold);
@@ -296,6 +306,10 @@
                     {
                         ThreadLoadStateMachine(LoadStages);
                     }
+                    else
+                    {
+                        load.color = fastBILNK;
+                    }
                 }
             }
         }
@@ -334,7 +348,7 @@
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 600,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
 
         return OK;
     }
@@ -403,7 +417,7 @@
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 600,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
 
         return OK;
     }
@@ -426,7 +440,7 @@
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ActivateHeadMagnet();
 
-        if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
+        if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
         {
             LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
         }
@@ -494,8 +508,8 @@
 
         if (dryerbufferlength)
             LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
-        //if (LoadArmInfo.LoadArmRounds <= 2)
-        //    LoadArmInfo.LoadArmRounds = 20;
+        if (LoadArmInfo.LoadArmRounds <= 2)
+            LoadArmInfo.LoadArmRounds = 30;
         float numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
         SetOriginMotorSpeed(19);
@@ -591,6 +605,7 @@
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
         LoadStages = THREAD_LOAD_INIT;
+        load.color = colorOFF;
         return OK;
     }
     uint32_t Thread_Load_Dryer_UnLoading(void)
@@ -664,7 +679,7 @@
         //Keep Notation How Many Rotations In The Dryer
         LoadArmInfo.LoadArmBackLash = 5;
         LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
-        //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
+        load.color = BLINK;
         return OK;
     }
 
@@ -825,6 +840,35 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
     }
+    if (index == POOLER_MOTOR)
+    {
+        if ((TranslatedReadValue)<(-1200))
+        {
+            if(PullerControlId != 0xFF)
+            {
+                Report("Puller stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
+                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
+                PullerControlId = 0xFF;
+            }
+        }
+
+    }
+    if (index == WINDER_MOTOR)
+    {
+        if ((TranslatedReadValue)<(-1200))
+        {
+            if(WinderControlId != 0xFF)
+            {
+                Report("Winder stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
+                MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
+                WinderControlId = 0xFF;
+            }
+
+        }
+
+    }
 /*    LoadCounter++;
     if ((LoadCounter % 5001) == 0)
     {