1.3.5.0 changes

====================================
1. preparations for keeping data in internal flash
2. keep applicative data in external flash: software parameters, dispensers data, etc.
3. moved to new alarms definitions, tamper alarms, improved temperature alarms, cone presence alarm,
4. support for Machine Studio 4
5. some tests for memory allocations
6. thread load proto code
7. Hardware: doors tampering switches, drier fan, blower PWM control, new backplane support, new motor drivers support,
8. FPGA jtag code loading
9. Heaters: alarms on set temperature band. test facilities.
10. motors - additional complex actions for thread load support. new drivers support
11. preparations for control debugging
12. read motor status and mid tank level
13. collect and store dispenser usage data
14. IDS - controlled operation of motor and valve. backlash movement after refill
15. bug fixes in job handling

1 files changed, 520 insertions, 0 deletions

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);
+}