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

5 files changed, 541 insertions, 14 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 9c42dde01..8c6164219 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -13,6 +13,7 @@
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
 #include "drivers/Motors/Motor.h"
+#include "../control/pidalgo.h"
 
 #include "thread_ex.h"
 
@@ -26,6 +27,18 @@ typedef struct
     double NumberOfRotationPerPassage;  // how many rotations per spool passage
     double diameter;
 }InternalWinderConfigStruc;
+typedef struct
+{
+    bool                m_isEnabled;
+    int32_t             m_SetParam;
+    float               m_mesuredParam;
+    float               m_preError;
+    float               m_integral;
+    float               m_calculatedError;
+    bool                m_isReady;
+    uint32_t            m_ingnoreValue;
+    PID_Config_Params   m_params;
+}MotorControlConfig_t;
 
 
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
@@ -50,6 +63,7 @@ extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
+extern    MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
new file mode 100644
index 000000000..34395b3e6
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -0,0 +1,520 @@
+/*
+ * ThreadLoad.c
+ *
+ *  Created on: Jan 16, 2019
+ *      Author: shlomo
+ */
+#include <DataDef.h>
+#include "include.h"
+#include "thread_ex.h"
+
+#include "Common/SWUpdate/FileSystem.h"
+
+#include "StateMachines/Printing/PrintingSTM.h"
+
+#include "Modules/thread/thread.h"
+#include "Modules/Control/control.h"
+#include "Modules/General/process.h"
+#include "Modules/control/pidalgo.h"
+
+#include "PMR/Hardware/HardwareMotor.pb-c.h"
+#include "PMR/Hardware/HardwareMotorType.pb-c.h"
+#include "PMR/Hardware/HardwareDancerType.pb-c.h"
+#include "PMR/Printing/ProcessParameters.pb-c.h"
+
+#include "drivers/Flash_Memory/fatfs/ff.h"
+#include "drivers/I2C_Communication/DAC/Blower.h"
+#include "drivers/SSI_Comm/Dancer/Dancer.h"
+#include "drivers/motors/motor.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 <stdint.h>
+//#include <stdbool.h>
+
+    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_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;
+    THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
+    ProcessParameters ProcessParametersClear,ProcessParametersRecover;
+    uint8_t NumberOfDrierLoaderCycles = 0;
+    uint32_t status = OK;
+    uint32_t ControlId = 0xFF;
+    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
+    uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
+
+    typedef struct
+    {
+        uint32_t LoadArmRounds;
+        uint32_t LoadArmBackLash;
+    }LoadArmInfoStruc;
+    LoadArmInfoStruc LoadArmInfo;
+    char LoadArmPath[50] = "0://SWInfo//LoadArm.msg";
+
+    //RUN MOTOR TO SWITCH WITH TIMEOUT
+    //RUN MOTOR TO BREAK SENSOR
+    //RUN MOTOR TO DANCER LOCATION (POOLER GOES TO THE OTHER DIRECTION)
+
+    //RUN MOTOR A FULL CYCLE
+    //RUN A MOTOR NUMBER OF STEPS
+    //RUN CONTROL FOR A SINGLE DANCER
+    uint32_t Thread_Load_Init(void)
+    {
+        void* buffer = NULL;
+        uint32_t Bytes = 0;
+        FRESULT Fresult = FR_OK;
+
+        LoadArmInfo.LoadArmBackLash = 0;
+        LoadArmInfo.LoadArmRounds = 0xFF;
+        Fresult = f_mkdir(SW_INFO_DIR);
+        if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
+        {
+
+            Fresult = FileRead(LoadArmPath, &Bytes, &buffer);
+            if (Fresult == FR_OK)
+            {
+               if (Bytes>=sizeof(LoadArmInfo))
+                   memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo));
+               free (buffer);
+            }
+        }
+
+        memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
+        //NumberOfDrierLoaderCycles = loadLoadArmParameters();
+        LoadStages++;
+        ThreadLoadStateMachine(LoadStages);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Reduce_Heat(void)
+    {
+        //Heaters Off, Dryer Blower Off, Blower Low,
+        memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters));
+        ProcessParametersClear.dryerzone1temp = 0;
+        ProcessParametersClear.dryerzone2temp = 0;
+        ProcessParametersClear.dryerzone3temp = 0;
+        ProcessParametersClear.mixertemp      = 0;
+        ProcessParametersClear.headzone1temp  = 0;
+        ProcessParametersClear.headzone2temp  = 0;
+        ProcessParametersClear.headzone3temp  = 0;
+        ProcessParametersClear.headzone4temp  = 0;
+        ProcessParametersClear.headzone5temp  = 0;
+        ProcessParametersClear.headzone6temp  = 0;
+        ProcessParametersClear.dyeingspeed = 40;
+        if (HandleProcessParameters(&ProcessParametersClear)!= OK)
+        {
+            LOG_ERROR (LoadStages, "Thread_Load_Init failed");
+        }
+        else
+        {
+            Turn_the_Blower_On();//Turn on with the Default_Voltage
+            if (BlowerCfg.heatingvoltage)
+                Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
+            else
+                Control_Voltage_To_Blower(BlowerCfg.voltage-500);
+            Control_Dryer_Fan(STOP,75);//use START or STOP,  0 - 100%
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        return OK;
+    }
+    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+
+        NumberOfDrierLoaderCycles--;
+        if (NumberOfDrierLoaderCycles)
+        {
+            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
+        {
+            //storeLoadArmParameters();
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        return OK;
+    }
+    uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+        NumberOfDrierLoaderCycles=0;
+        //storeLoadArmParameters();
+        LoadStages++;
+        ThreadLoadStateMachine(LoadStages);
+        return OK;
+    }
+    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
+    {
+        if (LoadArmInfo.LoadArmRounds != 0xFF)
+        {
+            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);
+        }
+        else //number of circles is not known - compare to position of the motor to verify location at the stopper
+        {
+            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);
+
+        }
+        //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
+        return OK;
+    }
+    uint8_t CallbackCounter = 0;
+    uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
+    {
+        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);
+            }
+        }
+        else
+            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
+        return OK;
+    }
+    uint32_t Thread_Load_Open_Covers(void)
+    {
+        //Open Dyeing Head Cover And Dryer Lid
+        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
+        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
+        CallbackCounter++;
+        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);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Lift_Dancers(void)
+    {
+        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);
+        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);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Lift_Rockers(void)
+    //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
+    {
+        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);
+        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);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second
+    {
+        if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
+        {
+            REPORT_MSG(LIMIT, "No cone in winder");
+            return ERROR;
+        }
+        CallbackCounter++;
+        MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500,  Thread_Load_HomingCallback,10000);
+
+        return OK;
+
+    }
+    uint32_t Thread_Load_Close_Rockers(void)
+    {
+        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);
+        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);
+
+        return OK;
+    }
+
+    uint32_t Thread_Load_Close_Dancers(void)
+    //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
+    {
+        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);
+        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);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Close_Lids(void)
+    {
+        //Close Dyeing Head Cover And Dryer Lid
+        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
+        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
+        CallbackCounter++;
+        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);
+
+        return OK;
+    }
+    uint32_t Thread_Load_Resume_Heating(void)
+    {
+        if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
+        {
+            LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
+        }
+        else
+        {
+            Control_Dryer_Fan(START,75);//use START or STOP,  0 - 100%
+            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
+    {
+        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(void)
+    {
+        LoadArmInfo.LoadArmRounds = 20;
+        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
+        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
+        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        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);
+
+        //Keep Notation How Many Rotations In The Dryer
+        //LoadArmInfo.LoadArmBackLash = 0;
+        //LoadArmInfo.LoadArmRounds = 0xFF;
+        FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
+        return OK;
+    }
+    uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
+    {
+        RemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
+        ControlId = 0xFF;
+        ThreadAbortJoggingFunc();
+        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);
+        ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,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");
+        return OK;
+    }
+
+    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
+    {
+        REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
+        switch (ReadValue)
+        {
+        case THREAD_LOAD_INIT:
+            //LoadStages++;
+            Thread_Load_Init();
+            break;
+        case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
+            //LoadStages++;
+            Thread_Load_Reduce_Heat();
+            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++;
+            Thread_Load_Set_Load_Arm_To_Start_Position();
+            break;
+        case THREAD_LOAD_OPEN_COVERS:  //OPEN DYEING HEAD COVER AND DRYER LID
+            //LoadStages++;
+            Thread_Load_Open_Covers();
+            break;
+        case THREAD_LOAD_LIFT_DANCERS:
+            //LoadStages++;
+            Thread_Load_Lift_Dancers();
+            break;
+        case THREAD_LOAD_LIFT_ROCKERS: //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
+            //LoadStages++;
+            Thread_Load_Lift_Rockers();
+            break;
+        case THREAD_LOAD_INITIAL_TENSION://CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
+            //LoadStages++;
+            Thread_Load_Initial_Tension();
+            break;
+        case THREAD_LOAD_CLOSE_ROCKERS:
+            //LoadStages++;
+            Thread_Load_Close_Rockers();
+            break;
+        case THREAD_LOAD_CLOSE_DANCERS: //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
+            //LoadStages++;
+            Thread_Load_Close_Dancers();
+            break;
+        case THREAD_LOAD_CLOSE_LIDS:
+            //LoadStages++;
+            Thread_Load_Close_Lids();
+            break;
+        case THREAD_LOAD_RESUME_HEATING:
+            //LoadStages++;
+            Thread_Load_Resume_Heating();
+            break;
+        case THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT://JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+            //LoadStages++;
+            Thread_Load_Jog_Feeder_To_Middle_Point();
+            break;
+        case 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
+            //LoadStages++;
+            Thread_Load_Dryer_Loading();
+            break;
+        case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
+            //LoadStages++;
+            Thread_Load_Jog_Thread();
+            break;
+        case THREAD_LOAD_END:
+            Thread_Load_End();
+            break;
+        default:
+            LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE");
+            break;
+    }
+    return OK;
+}
+
+uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue)
+{
+    //read value is the dancer angle
+    int i;
+    int DancerId;
+    int32_t TranslatedReadValue, avreageSampleValue = 0;
+    //double tempcalcspeed = 0;
+    uint32_t calculated_speed;
+    double NormalizedError;
+    char Message[60];
+
+    DancerId = ThreadMotorIdToDancerId[index];
+    if (ReadValue < 10)
+    {
+        Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+        return OK;
+    }
+    if (ReadValue == 0x3FFF)
+    {
+        return OK;
+    }
+    TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+    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]++;
+    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+        MotorSamplePointer[index] = 0;
+    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+        avreageSampleValue += MotorSamples[index][i];
+    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+
+
+    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_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
+    /*if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+    {
+        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+    }*/
+    calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+    if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    {
+        CurrentControlledSpeed[index] = calculated_speed;
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+    }
+ return OK;
+}
+void ThreadLoadRequest(MessageContainer* requestContainer)
+{
+//#ifdef DEBUG_TEST_FUNCTIONS
+    MessageContainer responseContainer;
+    uint8_t* container_buffer;
+    //uint32_t status = OK;
+
+    StubHeatingTestRequest* request = stub_heating_test_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size);
+    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+
+
+    int LoadStages = (int)request->dryerzone1temp;
+    ThreadLoadStateMachine(LoadStages);
+
+    //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);
+    SendChars(container_buffer, container_size);
+    stub_heating_test_request__free_unpacked(request,NULL);
+
+//#else
+//    LOG_ERROR (-1, "Heating Control not on debug");
+//    return ERROR;
+//#endif
+}
+void ThreadLoadPollRequest(MessageContainer* requestContainer)
+{
+    uint8_t* container_buffer;
+
+    StubHeatingTestPollRequest* request = stub_heating_test_poll_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubHeatingTestPollResponse response = STUB_HEATING_TEST_POLL_RESPONSE__INIT;
+    MessageContainer responseContainer;
+
+    response.has_heater1percentage = true;
+    response.heater1percentage = LoadStages;
+    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, requestContainer->token, false, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size);
+    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);
+    stub_heating_test_poll_request__free_unpacked(request,NULL);
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 34be77177..511a4ef93 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -23,6 +23,8 @@ typedef enum threadMotorsEnum
 
 extern double TotalProcessedLength;
 
+void ThreadSetBreakSensorLimit(int limit);
+
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadConfigBreakSensor(void *request);
 
@@ -41,6 +43,4 @@ uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId);
 uint32_t Winder_End(void);
 
-
-
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index f641737ee..f525e4a04 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -133,3 +133,8 @@ uint32_t thread_init(void)
 
     return OK;
 }
+void ThreadSetBreakSensorLimit(int limit)
+{
+    if (limit)
+        BreakSensordebouncetimemilli = limit;
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index bfd936b5c..c48eec522 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -49,18 +49,6 @@ uint32_t    PoolerSpeedControlId=0xFF;
 double DancerError[NUM_OF_DANCERS] = {0.0};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 uint32_t JobCounter = 0;
-typedef struct
-{
-    bool                m_isEnabled;
-    int32_t             m_SetParam;
-    float               m_mesuredParam;
-    float               m_preError;
-    float               m_integral;
-    float               m_calculatedError;
-    bool                m_isReady;
-    uint32_t            m_ingnoreValue;
-    PID_Config_Params   m_params;
-}MotorControlConfig_t;
 
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];