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

1 files changed, 644 insertions, 79 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 5a1bb9063..b9bbf3aab 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"
@@ -29,11 +30,18 @@
 #include "drivers/I2C_Communication/DAC/Blower.h"
 #include "drivers/SSI_Comm/Dancer/Dancer.h"
 #include "drivers/motors/motor.h"
+#include "drivers/Flash_ram/MCU_E2Prom.h"
 
 #include "PMR/Stubs/StubHeatingTestRequest.pb-c.h"
 #include "PMR/Stubs/StubHeatingTestResponse.pb-c.h"
 #include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h"
 #include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h"
+#include "PMR/ThreadLoading/StartThreadLoadingRequest.pb-c.h"
+#include "PMR/ThreadLoading/StartThreadLoadingResponse.pb-c.h"
+#include "PMR/ThreadLoading/ContinueThreadLoadingRequest.pb-c.h"
+#include "PMR/ThreadLoading/ContinueThreadLoadingResponse.pb-c.h"
+#include "PMR/ThreadLoading/StopThreadLoadingRequest.pb-c.h"
+#include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h"
 
 //#include <stdint.h>
 //#include <stdbool.h>
@@ -60,17 +68,29 @@
     THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
     ProcessParameters ProcessParametersClear,ProcessParametersRecover;
 
+    uint32_t LoadStatus = OK;
+    char LoadErrorMsg[100];
     int32_t keepmicrostep;
     int32_t keepkvalrun,keepkvalhold;
     uint8_t CallbackCounter = 0;
     uint8_t TimeoutsCounter = 0;
+    bool SecondTry = false,TryAgain = false;
+    int MessageState = 0; //0 - none, 1 = start, 2 = continue
 
+    uint32_t UnloadingStart = 0;
     uint8_t NumberOfDrierLoaderCycles = 0;
     uint32_t status = OK;
-    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF;
+    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF;
+    uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF;
+    uint32_t LoadCounter = 0;
+    double TotalLoadedLen = 0.0;
+    double LengthCalculationMultiplier;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
+    uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue);
     uint32_t Thread_Load_Dryer_UnLoading(void);
+    uint32_t ThreadLoadingReport(void);
+    bool ThreadLoaded(void);
 
     typedef struct
     {
@@ -78,7 +98,7 @@
         uint32_t LoadArmBackLash;
     }LoadArmInfoStruc;
     LoadArmInfoStruc LoadArmInfo;
-    char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg";
+    //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg";
 
     //RUN MOTOR TO SWITCH WITH TIMEOUT
     //RUN MOTOR TO BREAK SENSOR
@@ -94,16 +114,31 @@
         else
             return false;
     }
-    uint32_t Thread_Load_Init(void)
+    bool InitCalled = false;
+    uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage)
     {
-        void* buffer = NULL;
-        uint32_t Bytes = 0;
-        FRESULT Fresult = FR_OK;
+        //void* buffer = NULL;
+        //uint32_t Bytes = 0;
+        //FRESULT Fresult = FR_OK;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        load.color = BLINK;
+        LoadStatus = OK;
+        LoadArmInfo.LoadArmBackLash = MCU_E2PromDryerBacklashRead();
+        REPORT_MSG(LoadArmInfo.LoadArmBackLash, "Thread Load MCU_E2PromDryerBacklashRead");
+        if (LoadArmInfo.LoadArmBackLash > 10)
+            LoadArmInfo.LoadArmBackLash = 5;
+        //LoadArmInfo.LoadArmRounds = 0xFF;
+        //EEPROM_STORAGE_DRYER_CYCLES
+        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds);
 
-        LoadArmInfo.LoadArmBackLash = 5;
-        LoadArmInfo.LoadArmRounds = 0xFF;
-        Fresult = f_mkdir(SW_INFO_DIR);
+
+        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);
+
+        //if (LoadArmInfo.LoadArmRounds <= 2)
+        //        LoadArmInfo.LoadArmRounds = 20;
+        Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,0);
+
+        /*Fresult = f_mkdir(SW_INFO_DIR);
         if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
         {
 
@@ -114,14 +149,18 @@
                    memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo));
                free (buffer);
             }
-        }
+        }*/
 
-        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
+        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpMessage,LoadArmInfo.LoadArmRounds,0);
         StopInitSequence();
         memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
         //NumberOfDrierLoaderCycles = loadLoadArmParameters();
-        LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        if (Stage == THREAD_LOAD_INIT)
+        {
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        InitCalled = true;
 
         return OK;
     }
@@ -143,9 +182,12 @@
         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");
+            strcpy(LoadErrorMsg,"Thread_Load_Init failed");
+            LoadStatus = ERROR;
+            TryAgain = true;
         }
         else
         {
@@ -160,18 +202,18 @@
         }
         return OK;
     }
-    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
        // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
 
-        /*NumberOfDrierLoaderCycles--;
+        /`*NumberOfDrierLoaderCycles--;
         if (NumberOfDrierLoaderCycles)
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                             MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
         }
-        else*/
+        else*`/
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
             //storeLoadArmParameters();
@@ -180,7 +222,7 @@
             ThreadLoadStateMachine(LoadStages);
         }
         return OK;
-    }
+    }*/
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
 
@@ -196,6 +238,8 @@
             RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
             WinderControlId = 0xFF;
         }
+
+        MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
         /*Task_sleep(5)
         MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
         MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
@@ -204,6 +248,8 @@
 //move to exact location?
 //        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         CallbackCounter = 0;
+        Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
+
         Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
         NumberOfDrierLoaderCycles=0;
         //storeLoadArmParameters();
@@ -213,7 +259,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");
@@ -230,27 +276,66 @@
         {
             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)
+    }*/
+    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition)
     {
-        CallbackCounter--;
-        Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
-        //storeLoadArmParameters();
-        LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        if (Condition == true)
+        {
+            CallbackCounter--;
+            Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+            //storeLoadArmParameters();
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        else
+        {
+            CallbackCounter--;
+            Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+            //storeLoadArmParameters();
+            if (SecondTry == true)
+            {
+                LoadStages++;
+                ThreadLoadStateMachine(LoadStages);
+                LoadStatus = OK;
+            }
+            else
+            {
+                TimeoutsCounter = 0;
+                load.color = fastBILNK;
+                usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!");
+                LoadStatus = ERROR;
+                ThreadLoadingReport();
+                TryAgain = true;
+            }
+        }
 
         return OK;
     }
 
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
     {
-        Report("Thread_Load_HomingCallback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+        Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0);
 //        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
+        if ((LoadStages == THREAD_LOAD_CLOSE_ROCKERS)||(LoadStages == THREAD_LOAD_LIFT_ROCKERS))
+        {
+            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);
+            }
+        }
+
         if (CallbackCounter)
         {
             CallbackCounter--;
@@ -265,17 +350,16 @@
                 {
                     LOG_ERROR(LoadStages,"Load sequence timeout");
                     TimeoutsCounter = 0;
+                    load.color = fastBILNK;
+                    usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId);
+                    LoadStatus = ERROR;
+                    ThreadLoadingReport();
+                    TryAgain = true;
                 }
                 else
                 {
                     LoadStages++;
-                    if (LoadStages == THREAD_LOAD_CLOSE_ROCKERS)
-                    {
-                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,  keepmicrostep);
-                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepmicrostep);
-                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepkvalrun);
-                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepkvalrun);
-                    }
+                    LoadStatus = OK;
                     if (LoadStages == THREAD_LOAD_CLOSE_DANCERS)
                     {
                         MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold);
@@ -284,6 +368,11 @@
                     {
                         ThreadLoadStateMachine(LoadStages);
                     }
+                    else
+                    {
+                        load.color = colorON;
+                        ThreadLoadingReport();
+                    }
                 }
             }
         }
@@ -311,16 +400,18 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
         //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
+        HeadCard_Actuators_Control(ACTOT, LOW,true);
+
         return OK;
     }
     uint32_t Thread_Load_Lift_Dancers(void)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
+        /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
         MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
         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);
+        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, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
 
@@ -331,6 +422,19 @@
     {
         keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
         keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
+        if (keepkvalrun>=25)
+            keepkvalrun = 25;
+
+        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);
+
+        Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0);
+
+        Report("Thread_Load_Lift_right dancer",__FILE__,__LINE__,0,RpMessage,0,0);
+        keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
+        MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
+        CallbackCounter++;
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);
+
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
         MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
@@ -347,10 +451,11 @@
     uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        load.color = BLINK;
         if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
         {
             REPORT_MSG(LIMIT, "No cone in winder");
-            return ERROR;
+            //return ERROR;
         }
         CallbackCounter++;
         MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500,  Thread_Load_HomingCallback,10000);
@@ -360,6 +465,17 @@
     }
     uint32_t Thread_Load_Close_Rockers(void)
     {
+        keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
+        keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
+        if (keepkvalrun>=25)
+            keepkvalrun = 25;
+        Report("Thread_Load_Close_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0);
+
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
+        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70);
+        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70);
+
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
@@ -376,7 +492,10 @@
         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, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);
+
+//        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);
+        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, (5000*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
 
         return OK;
     }
@@ -390,7 +509,8 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
+        HeadCard_Actuators_Control(ACTOT, LOW,false);
 
         return OK;
     }
@@ -399,7 +519,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");
         }
@@ -416,7 +536,7 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,2000);
+        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,4000);
         return OK;
     }
 
@@ -430,11 +550,20 @@
             RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
             LoadingControlId = 0xFF;
         }
+        if (SpeedTControlId != 0xFF)
+        {
+           RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
+           SpeedTControlId = 0xFF;
+        }
+        Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);
+
         if (CallbackCounter)
         {
             CallbackCounter--;
             if (CallbackCounter == 0)
             {
+                LoadStatus = OK;
+
                 LoadStages++;
                 if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
                 {
@@ -447,34 +576,87 @@
 
         return OK;
     }
+    float numberOfSteps = 0;
+    float numberOfCycles = 0;
+    double DrierPrevLocation = 0;
     uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
-        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(LoadingControlId != 0xFF)
+        numberOfCycles++;
+
+        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0);
+
+        if ((fabs (TotalLoadedLen  -DrierPrevLocation)>50  )&&(ReadValue == NOTBUSY))
         {
-            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
-            LoadingControlId = 0xFF;
-        }*/
-        MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
+            DrierPrevLocation = TotalLoadedLen ;
+            //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+            //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
+            if (LoadStages != THREAD_LOAD_DRYER_LOADING)
+            {
+                return OK;
+            }
+            if (numberOfCycles<LoadArmInfo.LoadArmRounds)
+            {
+                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 10000);
+            }
+            else
+            {
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
+                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
+            }
+        }
+        else
+        {
+            LOG_ERROR(LoadStages,"Load sequence timeout - drier loading");
+            TryAgain = true;
+            SecondTry = false;
+            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
+            TimeoutsCounter = 0;
+            load.color = fastBILNK;
+            usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
+            LoadStatus = ERROR;
+            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
+            if(LoadingControlId != 0xFF)
+            {
+                MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
+                LoadingControlId = 0xFF;
+            }
+            if (SpeedTControlId != 0xFF)
+            {
+               RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
+               SpeedTControlId = 0xFF;
+            }
+            Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);
+
+            ThreadLoadingReport();
+        }
         return OK;
     }
 
     uint32_t Thread_Load_Dryer_Loading(void)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+
+        if (dryerbufferlength)
+            LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
         if (LoadArmInfo.LoadArmRounds <= 2)
-            LoadArmInfo.LoadArmRounds = 20;
-        float numberOfSteps = 0;
+            LoadArmInfo.LoadArmRounds = 30;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
         SetOriginMotorSpeed(19);
 //        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
 //        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
 
-        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        numberOfSteps -= 100;
+        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds);
+        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ;
+        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
+
+        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation);
+        numberOfCycles = 0;
+        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmInfo.LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+//        numberOfSteps -= 100;
         //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
         Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0);
         MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
@@ -498,16 +680,27 @@
 
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
 
-        LoadingControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        LoadingControlId = AddControlCallback(NULL,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);
+        if (SpeedTControlId != 0xFF)
+        {
+            RemoveControlCallback(SpeedTControlId,ThreadLoadLengthCBFunction);
+            SpeedTControlId = 0xFF;
+        }
+        //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+        LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep);
+        TotalLoadedLen = 0;
+        DrierPrevLocation = 200; //initial safe value
+        SpeedTControlId = AddControlCallback("Thread length",ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0);
+        Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 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,
-                               numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
-
+                               numberOfSteps-400,    Thread_Load_Dryer_Loading_Callback, 10000);
+//shlomo
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmInfo.LoadArmBackLash = 0;
-        LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+        //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
         //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
         return OK;
     }
@@ -526,12 +719,13 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ThreadJoggingFunc(20);
-        LoadingControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,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");
+        ThreadLoadingReport();
         if(LoadingControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
@@ -550,6 +744,11 @@
             RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
             WinderControlId = 0xFF;
         }
+        if (SpeedTControlId != 0xFF)
+        {
+           RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
+           SpeedTControlId = 0xFF;
+        }
 
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING);
@@ -559,21 +758,102 @@
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+        if (LoadStages!=THREAD_LOAD_END)
+        {
+            usnprintf(LoadErrorMsg, 100, "Load sequence stopped by user on stage %d",LoadStages);
+            LoadStatus = ERROR;
+        }
+
         LoadStages = THREAD_LOAD_INIT;
+        load.color = colorON;
+        return OK;
+    }
+    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT};
+    int Screw_Dir = false;
+
+    uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+        if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
+        {
+            return OK;
+        }
+        Screw_Dir = 1-Screw_Dir;
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
+    {
+        numberOfCycles++;
+        uint32_t temp = Read_Dryer_ENC_Position();
+        Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
+        //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
+        if ((abs (temp -DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY))
+        {
+            DrierPrevLocation = temp;
+            if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
+            {
+                return OK;
+            }
+            if (numberOfCycles<LoadArmInfo.LoadArmRounds)
+            {
+                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                                       numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 10000);
+            }
+            else
+            {
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,10000);
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-numberOfCycles);
+                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
+            }
+        }
+        else
+        {
+            LOG_ERROR(LoadStages,"Load sequence timeout - drier Unloading");
+            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-(numberOfCycles-2));//it takes two cycles to identify a stop of the arm
+            TimeoutsCounter = 0;
+            CallbackCounter = 0;
+            if(PullerControlId != 0xFF)
+            {
+                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
+                PullerControlId = 0xFF;
+            }
+            if(WinderControlId != 0xFF)
+            {
+                MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
+                WinderControlId = 0xFF;
+            }
+
+            MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+            /*Task_sleep(5)
+            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
+
+            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
+            load.color = fastBILNK;
+            usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
+            LoadStatus = ERROR;
+            TryAgain = true;
+            ThreadLoadingReport();
+        }
         return OK;
     }
     uint32_t Thread_Load_Dryer_UnLoading(void)
     {
+        uint32_t temp;
         REPORT_MSG(LoadStages, "Thread UnLoad State Machine step");
-        LoadArmInfo.LoadArmRounds = 0;
+        //LoadArmInfo.LoadArmRounds = 0;
         //uint32_t numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
-        SetOriginMotorSpeed(19);
+        SetOriginMotorSpeed(22);
 //        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
 //        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
 
         //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
+        /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
         MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
         MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
@@ -593,7 +873,7 @@
         MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
 
         PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR);
-        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);
+        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);*/
 ////////////////////////
         MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
         MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
@@ -614,7 +894,7 @@
         MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
         MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize);
 
-        WinderControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR);
+        WinderControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR);
         Report("AddControlCallback Winder",__FILE__,__LINE__,WinderControlId,RpMessage,IfTypeThread*0x100+WINDER_MOTOR,0);
 
 ////////////////////////
@@ -624,33 +904,60 @@
         CallbackCounter++;
         //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
         //                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
-        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
-        LoadArmInfo.LoadArmBackLash = 5;
+        //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
+        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
+//shlomo
+        UnloadingStart = msec_millisecondCounter;
+        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmInfo.LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+        numberOfCycles = 0;
+
+        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
+        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds);
+        DrierPrevLocation = temp;
+        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmInfo.LoadArmRounds,0);
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
+//        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/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                               numberOfSteps-400,    Thread_Load_Dryer_UnLoading_Callback, 10000);
 
-        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/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+        Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000);
 
         //Keep Notation How Many Rotations In The Dryer
-        //LoadArmInfo.LoadArmBackLash = 0;
-        LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
-        FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
+        //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
+        load.color = BLINK;
         return OK;
     }
 
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
     {
         REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
+        ThreadLoaded();
         if (LoadStages!=ReadValue)
             LoadStages = ReadValue;
         switch (ReadValue)
         {
         case THREAD_LOAD_INIT:
             //LoadStages++;
-            Thread_Load_Init();
+            MessageState = 1;
+
+            Thread_Load_Init(ReadValue);
             break;
         case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
             //LoadStages++;
+            MessageState = 1;
+            if (InitCalled == false)
+            {
+                REPORT_MSG(ReadValue,"Thread_Load_Init called from 1");
+                Thread_Load_Init(ReadValue);
+            }
+            else
+            {
+                REPORT_MSG(ReadValue,"Thread_Load_Init called from 0");
+            }
             Thread_Load_Reduce_Heat();
+            InitCalled = false;
             break;
         case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY
             //LoadStages++;
@@ -705,6 +1012,7 @@
         case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
             //LoadStages++;
             Thread_Load_Jog_Thread();
+            ThreadLoadingReport();
             break;
         case THREAD_LOAD_END:
             LoadStages = THREAD_LOAD_END;
@@ -723,19 +1031,90 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
         Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
         return ERROR;
     }
+    if (CallbackCounter)
+    {
+        Report("ThreadLoadButton called when the loading is active",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+        return ERROR;
+    }
     Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
-    if (LoadStages > ReadValue)
+    Report("ThreadLoadButton params",__FILE__,LoadStatus,TryAgain,RpMessage,SecondTry,0);
+    if (LoadStatus == ERROR)
     {
-        ThreadLoadStateMachine(LoadStages+1);
+        if ((SecondTry == false)&&(TryAgain == true))
+        {
+            LoadStatus = OK;
+            SecondTry = true;
+            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            ThreadLoadStateMachine(LoadStages);
+        }
+        else
+        {
+            SecondTry = false;
+            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
+            ThreadLoadStateMachine(LoadStages+1);
+        }
     }
     else
     {
-        ThreadLoadStateMachine(ReadValue);
+        Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0);
+        ThreadLoadStateMachine(LoadStages);
     }
     return OK;
 }
-uint32_t LoadCounter = 0;
 
+uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    uint32_t positionDiff = 0,prevprev;
+    double length = 0.0;
+    char lenstr[150];
+    static int counter = 0;
+    int index = MAX_THREAD_MOTORS_NUM;
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+//    if (CurrentRequestedLength == 0.0)
+//        return OK;
+    if (index != HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)
+    {
+        LOG_ERROR (IfIndex, "Wrong Motor");
+        return 0xFFFFFFFF;
+    }
+    currentPosition = MotorGetPosition(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+    if (currentPosition != 0)
+    {
+        if (Tinitialpos == 0xFFFF)
+        {
+            previousPosition = currentPosition;
+            Tinitialpos = 0;
+        }
+        prevprev = previousPosition;
+        positionDiff = Control_Delta_Position_Pass(currentPosition,previousPosition);
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+        previousPosition = currentPosition;
+
+        // total length = (position diff / full cycle) * pulley perimeter
+        //(positionDiff/pulseperround)*((2*PI*motor_Radius)
+
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+        length = (double)(positionDiff)*LengthCalculationMultiplier;
+
+        if (counter++ % 50 == 0)
+        {
+            usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x TotalLoadedLen %d",(int)length*100,(int)positionDiff,previousPosition,prevprev, (int)TotalLoadedLen);
+//            SendJobProgress(0.0,0,false, lenstr);
+            ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalLoadedLen,RpWarning,(int)TotalLoadedLen,0);
+//            length = 0;
+
+        }
+
+    }
+    TotalLoadedLen += length;
+
+    return OK;
+}
 uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
@@ -782,7 +1161,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
     MotorControlConfig[index].m_mesuredParam = NormalizedError;
-    MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
+    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
     {
@@ -794,6 +1173,37 @@ 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)
     {
@@ -801,6 +1211,33 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }*/
  return OK;
 }
+bool ThreadLoaded(void)
+{
+    uint32_t ReadValue;
+
+    int32_t PoolerValue,WinderValue,FeederValue;
+    //this function tests if there is a thread in the machine. all dancers are in lowermost position then there is no thread.
+
+    ReadValue = Control_Read_Dancer_Position(FEEDER_DANCER,0);
+    FeederValue = ReadValue - DancersCfg[FEEDER_DANCER].zeropoint;
+    Report("Feeder out",__FILE__,FeederValue,ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0);
+    ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0);
+    WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint;
+    Report("Winder out",__FILE__,WinderValue,ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0);
+    ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0);
+    PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint;
+        //pooler dancer is right sided: data is opposite
+    PoolerValue = (-1*PoolerValue);
+    Report("Puller out",__FILE__,PoolerValue,ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0);
+
+    if (((PoolerValue)<(-1200))&&((WinderValue)<(-1200))&&((FeederValue)<(-1200)))
+    {
+        Report("Thread is out",__FILE__,(-1*PoolerValue),(-1*WinderValue),RpWarning,(-1*FeederValue),0);
+        return true;
+    }
+    else
+        return false;
+}
 void ThreadLoadRequest(MessageContainer* requestContainer)
 {
 //#ifdef DEBUG_TEST_FUNCTIONS
@@ -835,6 +1272,7 @@ void ThreadLoadRequest(MessageContainer* requestContainer)
 //    return ERROR;
 //#endif
 }
+
 void ThreadLoadPollRequest(MessageContainer* requestContainer)
 {
     uint8_t* container_buffer;
@@ -860,3 +1298,130 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer)
     SendChars(container_buffer, container_size);
     stub_heating_test_poll_request__free_unpacked(request,NULL);
 }
+char ThreadLoadingToken[36+1] = {0};
+char DefaultErrSrt[] = "OK";
+uint32_t ThreadLoadingReport(void)
+{
+    MessageContainer responseContainer;
+    bool last = false;
+    StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT;
+
+    if (ThreadLoadingToken[0] == 0)
+        return OK;
+
+    if (MessageState == 0)
+    {
+        response.has_state = true;
+        response.state = THREAD_LOADING_STATE__None;
+        response.errorreason = DefaultErrSrt;
+
+    }
+    else if (MessageState == 1)
+    {
+        response.has_state = true;
+        if (LoadStatus == OK)
+        {
+            response.state = THREAD_LOADING_STATE__Preparing;
+            if (LoadStages == THREAD_LOAD_INITIAL_TENSION)
+            {
+                response.state = THREAD_LOADING_STATE__ReadyForLoading;
+                response.errorreason = DefaultErrSrt;
+            }
+        }
+        else
+        {
+            response.state = THREAD_LOADING_STATE__PreparationError;
+            response.errorreason = LoadErrorMsg;
+        }
+    }
+    else if (MessageState == 2)
+    {
+        response.has_state = true;
+        if (LoadStatus == OK)
+        {
+            response.state = THREAD_LOADING_STATE__Finalizing;
+            if (LoadStages > THREAD_LOAD_JOG_THREAD)
+            {
+                response.state = THREAD_LOADING_STATE__Completed;
+                response.errorreason = DefaultErrSrt;
+                //last = true;
+            }
+        }
+        else
+        {
+            response.state = THREAD_LOADING_STATE__FinalizationError;
+            response.errorreason = LoadErrorMsg;
+        }
+    }
+    else return ERROR;
+    Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0);
+
+    //-------------------------------------------------------------------------------------------
+    responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size);
+    responseContainer.continuous = true;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+
+    return OK;
+
+}
+
+uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer)
+{
+    StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    ustrncpy (ThreadLoadingToken, requestContainer->token,36);
+    //MessageState = 1;
+    Report("StartThreadLoadingFunc",__FILE__,__LINE__,0,RpWarning,(int)LoadStages,0);
+    ThreadLoadingReport();
+    return OK;
+
+}
+
+uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer)
+{
+    ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT;
+    MessageContainer responseContainer;
+    MessageState = 2;
+    ThreadLoadingReport();
+    if (request->processparameters)
+    {
+        dryerbufferlength = request->processparameters->dryerbufferlength;
+        LoadArmInfo.LoadArmRounds = (int)(request->processparameters->dryerbufferlength);
+
+    }
+    Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0);
+
+    responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+    if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+    {
+        ThreadLoadStateMachine(LoadStages);
+    }
+    return OK;
+
+}
+uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer)
+{
+    StopThreadLoadingRequest *request = stop_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    StopThreadLoadingResponse Cresponse = STOP_THREAD_LOADING_RESPONSE__INIT;
+    MessageContainer responseContainer;
+    MessageState = 2;
+    Thread_Load_End();
+    Report("StopThreadLoadingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)LoadStages,0);
+
+    responseContainer = createContainer(MESSAGE_TYPE__StopThreadLoadingResponse, requestContainer->token, true, &Cresponse, &stop_thread_loading_response__pack, &stop_thread_loading_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+    return OK;
+
+}