merge

6 files changed, 184 insertions, 76 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 3af29a043..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
@@ -83,8 +83,9 @@
         void* buffer = NULL;
         uint32_t Bytes = 0;
         FRESULT Fresult = FR_OK;
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
 
-        LoadArmInfo.LoadArmBackLash = 0;
+        LoadArmInfo.LoadArmBackLash = 10;
         LoadArmInfo.LoadArmRounds = 0xFF;
         Fresult = f_mkdir(SW_INFO_DIR);
         if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
@@ -99,6 +100,7 @@
             }
         }
 
+        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
         memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
         //NumberOfDrierLoaderCycles = loadLoadArmParameters();
         LoadStages++;
@@ -108,7 +110,9 @@
     }
     uint32_t Thread_Load_Reduce_Heat(void)
     {
+
         //Heaters Off, Dryer Blower Off, Blower Low,
+        REPORT_MSG(LoadStages, "Thread Load State Machine step ");
         memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters));
         ProcessParametersClear.dryerzone1temp = 0;
         ProcessParametersClear.dryerzone2temp = 0;
@@ -129,52 +133,66 @@
         {
             Turn_the_Blower_On();//Turn on with the Default_Voltage
             if (BlowerCfg.heatingvoltage)
-                Cancel_Gradual_Increase_Blower(BlowerCfg.heatingvoltage);
+                Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
             else
-                Cancel_Gradual_Increase_Blower(BlowerCfg.voltage-500);
+                Control_Voltage_To_Blower(BlowerCfg.voltage-500);
             Control_Dryer_Fan(STOP,75);//use START or STOP,  0 - 100%
             LoadStages++;
-            ThreadLoadStateMachine(LoadStages);
+            //ThreadLoadStateMachine(LoadStages);
         }
         return OK;
     }
     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,NumberOfDrierLoaderCycles,0);
             //storeLoadArmParameters();
+            SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
             LoadStages++;
-            ThreadLoadStateMachine(LoadStages);
+            //ThreadLoadStateMachine(LoadStages);
         }
         return OK;
     }
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
+//        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         NumberOfDrierLoaderCycles=0;
         //storeLoadArmParameters();
         LoadStages++;
-        ThreadLoadStateMachine(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)
         {
+            //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
         {
+            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/10, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,LoadArmInfo.LoadArmBackLash,1000);
+                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, 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
@@ -183,19 +201,18 @@
     uint8_t CallbackCounter = 0;
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
     {
+        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
@@ -204,6 +221,7 @@
     }
     uint32_t Thread_Load_Center_Head_Rockers(void)
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
@@ -212,6 +230,7 @@
     }
     uint32_t Thread_Load_Open_Covers(void)
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         //Open Dyeing Head Cover And Dryer Lid
         //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
         //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
@@ -220,30 +239,33 @@
         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);
-        MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 2,  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);
         return OK;
     }
     uint32_t Thread_Load_Lift_Dancers(void)
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 200,  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;
     }
     uint32_t Thread_Load_Lift_Rockers(void)
     //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
     {
+        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;
     }
     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");
         if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
         {
             REPORT_MSG(LIMIT, "No cone in winder");
@@ -257,10 +279,11 @@
     }
     uint32_t Thread_Load_Close_Rockers(void)
     {
+        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;
     }
@@ -268,15 +291,17 @@
     uint32_t Thread_Load_Close_Dancers(void)
     //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
     {
+        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;
     }
     uint32_t Thread_Load_Close_Lids(void)
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         //Close Dyeing Head Cover And Dryer Lid
         //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
         //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
@@ -284,12 +309,13 @@
         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);
-        MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 0,  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);
 
         return OK;
     }
     uint32_t Thread_Load_Resume_Heating(void)
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ActivateHeadMagnet();
 
         if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
@@ -300,25 +326,80 @@
         {
             Control_Dryer_Fan(START,75);//use START or STOP,  0 - 100%
             LoadStages++;
-            ThreadLoadStateMachine(LoadStages);
+            //ThreadLoadStateMachine(LoadStages);
         }
         return OK;
     }
     uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void)
     //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position
     {
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
         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;
@@ -328,17 +409,19 @@
     }
     uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
     {
-        RemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
         ControlId = 0xFF;
         ThreadAbortJoggingFunc();
         LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        //ThreadLoadStateMachine(LoadStages);
         return OK;
     }
     uint32_t Thread_Load_Jog_Thread(void)
     //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading
     {
-        ThreadJoggingFunc(40);
+        REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        ThreadJoggingFunc(20);
         ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         return OK;
     }
@@ -424,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);
@@ -445,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]++;
@@ -490,14 +580,15 @@ void ThreadLoadRequest(MessageContainer* requestContainer)
     container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
 
 
-    int LoadStages = (int)request->dryerzone1temp;
-    ThreadLoadStateMachine(LoadStages);
+    int LoadStage = (int)request->dryerzone1temp;
+    LoadStages = LoadStage;
+    ThreadLoadStateMachine(LoadStage);
 
-    //if (status)
+    /*if (status)
     {
         responseContainer.has_error = true;
         responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
-    }
+    }*/
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     my_free(responseContainer.data.data);
     //USBCDCD_sendData(container_buffer, container_size,10);
@@ -524,11 +615,11 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer)
     container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
 
 
-
+/*
     {
         responseContainer.has_error = true;
         responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
-    }
+    }*/
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     my_free(responseContainer.data.data);
     SendChars(container_buffer, container_size);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index becdf054a..18aceb767 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -100,14 +100,13 @@ uint32_t Winder_Prepare(void)
      * report ready to the job STM
      */
 
-    if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
+    /*if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
     {
         REPORT_MSG(LIMIT, "No cone in winder");
         PrepareReady(Module_Winder,ModuleFail);
         AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
         return ERROR;
-        //Winder_PrepareStage2(0,0);
-    }
+    }*/
     if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
     {
         //REPORT_MSG(LIMIT, "Winder_Prepare at limit");
@@ -376,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 76d4b090a..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;
@@ -463,10 +462,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 case POOLER_MOTOR:
                     AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,true);
                     break;
-                case WINDER_MOTOR:
+                case FEEDER_MOTOR:
                     AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,true);
                     break;
-                case FEEDER_MOTOR:
+                case WINDER_MOTOR:
                     AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true);
                     break;
             }
@@ -546,10 +545,6 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     return OK;
 }
 bool InitialProcess = false;
-uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
-{
-    return OK;
-}
 
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
@@ -650,7 +645,7 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             }
 //            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
+//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
     }