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

6 files changed, 139 insertions, 52 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 8c6164219..638611c8b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -74,5 +74,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
+void SetOriginMotorSpeed(float process_speed);
+
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index edfa3b0b9..1c28a5447 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -35,26 +35,26 @@
 //#include <stdint.h>
 //#include <stdbool.h>
 
-    typedef enum
+/*    typedef enum
     {
-        THREAD_LOAD_INIT,
-        THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
-        THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
-        THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
-        THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
-        THREAD_LOAD_LIFT_DANCERS,
-        THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
-        THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
-        THREAD_LOAD_CLOSE_ROCKERS,
-        THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
-        THREAD_LOAD_CLOSE_LIDS,
-        THREAD_LOAD_RESUME_HEATING,
-        THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
-        THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
+        0 THREAD_LOAD_INIT,
+        1 THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
+        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
+        3 THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
+        4 THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
+        5 THREAD_LOAD_LIFT_DANCERS,
+        6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
+        7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
+        8 THREAD_LOAD_CLOSE_ROCKERS,
+        9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
+        10 THREAD_LOAD_CLOSE_LIDS,
+        11 THREAD_LOAD_RESUME_HEATING,
+        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+        13 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                     //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
-        THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
-        THREAD_LOAD_END
-    }THREAD_LOAD_STAGES_ENUM;
+        14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
+        15 THREAD_LOAD_END
+    }THREAD_LOAD_STAGES_ENUM;*/
     THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
     ProcessParameters ProcessParametersClear,ProcessParametersRecover;
     uint8_t NumberOfDrierLoaderCycles = 0;
@@ -69,7 +69,7 @@
         uint32_t LoadArmBackLash;
     }LoadArmInfoStruc;
     LoadArmInfoStruc LoadArmInfo;
-    char LoadArmPath[50] = "0://SWInfo//LoadArm.msg";
+    char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg";
 
     //RUN MOTOR TO SWITCH WITH TIMEOUT
     //RUN MOTOR TO BREAK SENSOR
@@ -85,7 +85,7 @@
         FRESULT Fresult = FR_OK;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
 
-        LoadArmInfo.LoadArmBackLash = 40;
+        LoadArmInfo.LoadArmBackLash = 10;
         LoadArmInfo.LoadArmRounds = 0xFF;
         Fresult = f_mkdir(SW_INFO_DIR);
         if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
@@ -100,10 +100,11 @@
             }
         }
 
+        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
         memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
         //NumberOfDrierLoaderCycles = loadLoadArmParameters();
         LoadStages++;
-        //ThreadLoadStateMachine(LoadStages);
+        ThreadLoadStateMachine(LoadStages);
 
         return OK;
     }
@@ -145,17 +146,18 @@
     {
        // 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,NumberOfDrierLoaderCycles,0);
             //storeLoadArmParameters();
+            SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
             LoadStages++;
             //ThreadLoadStateMachine(LoadStages);
         }
@@ -168,18 +170,23 @@
         NumberOfDrierLoaderCycles=0;
         //storeLoadArmParameters();
         LoadStages++;
+        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+
         //ThreadLoadStateMachine(LoadStages);
         return OK;
     }
     uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
     {
+        uint32_t numberOfSteps = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         if (LoadArmInfo.LoadArmRounds != 0xFF)
         {
-            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+            //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
+            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;
+            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0);
             NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds;
             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*LoadArmInfo.LoadArmRounds, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
+                                            numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
         }
         else //number of circles is not known - compare to position of the motor to verify location at the stopper
         {
@@ -196,18 +203,16 @@
     {
         Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
-        if(ControlId != 0xFF)
-        {
-            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
-            ControlId = 0xFF;
-        }
         if (CallbackCounter)
         {
             CallbackCounter--;
             if (CallbackCounter == 0)
             {
                 LoadStages++;
-                //ThreadLoadStateMachine(LoadStages);
+                if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+                {
+                    //ThreadLoadStateMachine(LoadStages);
+                }
             }
         }
         else
@@ -241,9 +246,9 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 20,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
+        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_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000);
+        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,20000);
 
         return OK;
     }
@@ -252,9 +257,9 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 700,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 700,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000);
 
         return OK;
     }
@@ -276,9 +281,9 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000);
 
         return OK;
     }
@@ -288,9 +293,9 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
+        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,4000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000);
+        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,10000);
 
         return OK;
     }
@@ -333,15 +338,68 @@
         MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000);
         return OK;
     }
+
+    uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
+    {
+        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id");
+        if(ControlId != 0xFF)
+        {
+            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
+            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
+            ControlId = 0xFF;
+        }
+        if (CallbackCounter)
+        {
+            CallbackCounter--;
+            if (CallbackCounter == 0)
+            {
+                LoadStages++;
+                if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+                {
+                    //ThreadLoadStateMachine(LoadStages);
+                }
+            }
+        }
+        else
+            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
+        return OK;
+    }
+
     uint32_t Thread_Load_Dryer_Loading(void)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         LoadArmInfo.LoadArmRounds = 20;
+        uint32_t numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
-        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
+        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;
+        MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
+        MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime;
+        MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp;
+        MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue;
+        MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon;
+        MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype;
+        MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue =  MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_integral = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true;
+        MotorControlConfig[FEEDER_MOTOR].m_isReady = true;
+        MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_preError = 0;
+        MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize);
+
         ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        CallbackCounter++;
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                               MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds,    Thread_Load_HomingCallback, 100000);
+                               numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
 
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmInfo.LoadArmBackLash = 0;
@@ -352,7 +410,7 @@
     uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        RemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
+        SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
         ControlId = 0xFF;
         ThreadAbortJoggingFunc();
         LoadStages++;
@@ -363,7 +421,7 @@
     //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        ThreadJoggingFunc(40);
+        ThreadJoggingFunc(20);
         ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         return OK;
     }
@@ -449,17 +507,18 @@
     return OK;
 }
 
-uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue)
+uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
     int i;
     int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
+    int index=MAX_THREAD_MOTORS_NUM;
     //double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
 
-    DancerId = ThreadMotorIdToDancerId[index];
+    DancerId = FEEDER_DANCER;//ThreadMotorIdToDancerId[index];
     if (ReadValue < 10)
     {
         Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
@@ -470,11 +529,17 @@ uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue)
         return OK;
     }
     TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-    if (index == POOLER_MOTOR)
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+    /*if (index == POOLER_MOTOR)
     {
         //pooler dancer is right sided: data is opposite
         TranslatedReadValue = (-1*TranslatedReadValue);
-    }
+    }*/
     //TranslatedReadValue = 0;//test
     MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
     MotorSamplePointer[index]++;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 9b95ab52c..18aceb767 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -375,15 +375,12 @@ void Winder_ScrewOutLimitSwitchInterrupt(void)
 
 uint32_t  Screw_timerBase = TIMER3_BASE;        //Timer handle
 
-bool     Screw_Restart = false;
 void ScrewTimerInterruptInit(void)
 {
     ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
     ROM_TimerEnable(Screw_timerBase, TIMER_A);
     ROM_IntEnable(INT_TIMER3A);
     ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
-    Screw_Restart = false;
-
 }
 void ScrewsStopControlTimer (void)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index a22be25dc..cd71c73b1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -46,6 +46,29 @@ uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId);
 uint32_t Winder_End(void);
 
+typedef enum
+{
+    THREAD_LOAD_INIT,
+    THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
+    THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
+    THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
+    THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
+    THREAD_LOAD_LIFT_DANCERS,
+    THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
+    THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
+    THREAD_LOAD_CLOSE_ROCKERS,
+    THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
+    THREAD_LOAD_CLOSE_LIDS,
+    THREAD_LOAD_RESUME_HEATING,
+    THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+    THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
+                                //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
+    THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
+    THREAD_LOAD_END
+}THREAD_LOAD_STAGES_ENUM;
+uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue);
+
+
 void ThreadLoadPollRequest(MessageContainer* requestContainer);
 void ThreadLoadRequest(MessageContainer* requestContainer);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index b414611ff..435d7aef3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -106,7 +106,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
-char DancerConfigPath[50] = "0://SWInfo//DancCfg.msg";
+char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
 
 uint32_t StoreDancerConfigMessage()
 {
@@ -128,6 +128,7 @@ uint32_t StoreDancerConfigMessage()
         Dancers[Dancer_i].has_hardwaredancertype = true;
         DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0,0);
         DancerConfig.n_dancers++;
+        DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
     }
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index e15a27351..aee5fefac 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -75,7 +75,6 @@ void ThreadSegmentEnded(void);
 void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue);
-void SetOriginMotorSpeed(float process_speed);
 
 double KeepNormalizedError = 0;
 bool ThreadControlActive = false;