Version 1.5.0.1: Thread load changes, 4 winders, RFID, ARC head

2 files changed, 113 insertions, 123 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index cd7c146fe..d16683967 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -126,6 +126,7 @@
 
     uint32_t Thread_Load_Dryer_UnLoading(void);
     uint32_t ThreadLoadingReport(void);
+    uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage
     bool ThreadLoaded(void);
     bool ThreadLoadingActive(void)
     {
@@ -245,38 +246,41 @@
     }
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
+        uint32_t angle, temp = Read_Dryer_ENC_Position();
 
-        /*if(PullerControlId != 0xFF)
-        {
-            MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
-            PullerControlId = 0xFF;
-        }*/
-        if(WinderControlId != 0xFF)
+        ReportWithPackageFilter(DiagnosticsFilter,"Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,(int)numberOfCycles,(int)DrierPrevLocation,RpMessage,temp,0);
+
+        angle = Calculate_Arm_Distance(DrierPrevLocation,temp);
+
+        DrierPrevLocation = temp;
+        if (CallbackCounter)
         {
-            MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
-            RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
-            WinderControlId = 0xFF;
+            CallbackCounter--;
         }
+        if ((angle>14000  )&&(BusyFlag == NOTBUSY)) // OK - take another round
+        {
+            Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
 
-        MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
-        /*Task_sleep(5)
-        MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-        MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
+            Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+            NumberOfDrierLoaderCycles=0;
+            //storeLoadArmParameters();
+            LoadStages++;
 
-        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
+            ThreadLoadStateMachine(LoadStages);
+        }
+        else
+        {
+            load.color = fastBILNK;
+            usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM]);
+            Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
+
+            LoadStatus = ERROR;
+            ThreadLoadingReport();
+            TryAgain = true;
+            //ThreadLoadButton(LoadStages);
+        }
 //move to exact location?
 //        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
-        CallbackCounter = 0;
-        Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
-
-        Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
-        NumberOfDrierLoaderCycles=0;
-        //storeLoadArmParameters();
-        LoadStages++;
-        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
-
-        ThreadLoadStateMachine(LoadStages);
         return OK;
     }
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
@@ -484,6 +488,8 @@
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
 
+        Report("Thread_Load_Close dancers tension",__FILE__,__LINE__,(int)windertension,RpMessage,(int)pullertension,0);
+
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
         status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,&current);
 
@@ -666,7 +672,7 @@
             }
             else
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
         }
@@ -826,6 +832,7 @@
            ThreadLoadControlId = 0xFF;
         }
 
+        LoadingStopArmReset();
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING);
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING);
@@ -860,7 +867,7 @@
 
         return OK;
     }
-    uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
+    /*uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
         bool direction;
         numberOfCycles++;
@@ -884,7 +891,7 @@
             }
             else //done enough cycles, go to the center point
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -902,7 +909,7 @@
                     direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
                 else
                     direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction,10);
 
             }
             else
@@ -911,12 +918,6 @@
 
             TimeoutsCounter = 0;
             CallbackCounter = 0;
-            /*if(PullerControlId != 0xFF)
-            {
-                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
-                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
-                PullerControlId = 0xFF;
-            }*/
             if(WinderControlId != 0xFF)
             {
                 MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
@@ -925,9 +926,6 @@
             }
 
             MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
-            /*Task_sleep(5)
-            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
-            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
 
             MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
             load.color = fastBILNK;
@@ -940,79 +938,24 @@
             //ThreadLoadButton(LoadStages);
         }
         return OK;
-    }
+    }*/
     uint32_t Thread_Load_Dryer_UnLoading(void)
     {
         uint32_t temp;
         REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading");
         //LoadArmRounds = 0;
         //uint32_t numberOfSteps = 0;
-        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
-        SetOriginMotorSpeed(22);
-//        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
-//        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
-
-// Rockers are up already, so puller handling is not needed
-        //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
-        MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
-        MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
-        MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
-        MotorControlConfig[POOLER_MOTOR].m_params.Ki = 0;//MotorsControl[POOLER_MOTOR].integraltime;
-        MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp;
-        MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue;
-        MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon;
-        MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype;
-        MotorControlConfig[POOLER_MOTOR].m_ingnoreValue =  MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
-        MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0;
-        MotorControlConfig[POOLER_MOTOR].m_integral = 0;
-        MotorControlConfig[POOLER_MOTOR].m_isEnabled = true;
-        MotorControlConfig[POOLER_MOTOR].m_isReady = true;
-        MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0;
-        MotorControlConfig[POOLER_MOTOR].m_preError = 0;
-        MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
-        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
-
-        PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR);
-        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);*/
-////////////////////////
-        MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
-        MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
-        MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime;
-        MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain;
-        MotorControlConfig[WINDER_MOTOR].m_params.Ki = 0;//MotorsControl[WINDER_MOTOR].integraltime;
-        MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp;
-        MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue;
-        MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon;
-        MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype;
-        MotorControlConfig[WINDER_MOTOR].m_ingnoreValue =  MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
-        MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0;
-        MotorControlConfig[WINDER_MOTOR].m_integral = 0;
-        MotorControlConfig[WINDER_MOTOR].m_isEnabled = true;
-        MotorControlConfig[WINDER_MOTOR].m_isReady = true;
-        MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0;
-        MotorControlConfig[WINDER_MOTOR].m_preError = 0;
-        MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
-        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize);
-
-        WinderControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR);
-        Report("AddControlCallback Winder",__FILE__,__LINE__,WinderControlId,RpMessage,IfTypeThread*0x100+WINDER_MOTOR,0);
-
-////////////////////////
-        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].directionthreadwize);
-        MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
 
         CallbackCounter++;
-        //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-        //                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
-        //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
-        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
-//shlomo
-        UnloadingStart = msec_millisecondCounter;
-        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        numberOfCycles = 0;
 
-        if (SecondTry == true)
+        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
+        DrierCenterLocation = DrierPrevLocation;
+        DrierPrevLocation = temp;
+        UnloadingStart = msec_millisecondCounter;
+        //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+        //numberOfCycles = 0;
+        LoadingArmReset(Thread_Load_Set_Load_Arm_To_Stopper_Callback,300000);
+        /*if (SecondTry == true)
         {
             MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);
             if (LoadArmRounds == 0) //prev trial stopped
@@ -1028,8 +971,8 @@
             {
                 LoadArmRounds = (int)dryerbufferlength;
             }
-        }
-        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
+        }*/
+        /*MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
         DrierCenterLocation = DrierPrevLocation;
         DrierPrevLocation = temp;
         Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0);
@@ -1046,6 +989,8 @@
 
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmRounds = (int)dryerbufferlength;
+         *
+         */
         load.color = BLINK;
         return OK;
     }
@@ -1172,6 +1117,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
             LoadStatus = OK;
             SecondTry = true;
             load.color = BLINK;
+            if (LoadStages > THREAD_LOAD_INITIAL_TENSION)
+                ThreadLoadingRestartReport();
             Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
             ThreadLoadStateMachine(LoadStages);
         }
@@ -1180,6 +1127,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
             SecondTry = false;
             Report("Calling State machine  2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
             load.color = BLINK;
+            if (LoadStages > THREAD_LOAD_INITIAL_TENSION)
+                ThreadLoadingRestartReport();
             ThreadLoadStateMachine(LoadStages+1);
         }
         else //((SecondTry == false)&&(TryAgain == false))???????
@@ -1191,6 +1140,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
     else
     {
         Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0);
+        if (LoadStages > THREAD_LOAD_INITIAL_TENSION)
+            ThreadLoadingRestartReport();
         ThreadLoadStateMachine(LoadStages);
         SecondTry = false;
     }
@@ -1521,22 +1472,53 @@ uint32_t ThreadLoadingReport(void)
     return OK;
 
 }
+bool ThreadLoadingRestartFlag = false;
+uint32_t ThreadLoadingRestartReport(void) //sending after a failure in the finalizing stage
+{
+    MessageContainer responseContainer;
+    StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT;
+
+    if (ThreadLoadingToken[0] == 0)
+        return OK;
+    ThreadLoadingRestartFlag = true;
+
+    response.has_state = true;
+    response.state = THREAD_LOADING_STATE__ReadyForLoading;
+    response.errorreason = DefaultErrSrt;
+    Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0);
+    //-------------------------------------------------------------------------------------------
+    responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, false, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size);
+    responseContainer.has_continuous = true;
+    responseContainer.continuous = true;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+
+    return OK;
+
+}
 
 uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer)
 {
-    TryThreadLoadingRequest *request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    TryThreadLoadingRequest *request;
     TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT;
     MessageContainer responseContainer;
-    MessageState = 2;
+    //MessageState = 2;
+    if (requestContainer)
+        request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     ThreadLoadingReport();
     Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0);
 
-    responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size);
-    responseContainer.continuous = false;
-    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
-    size_t container_size = message_container__pack(&responseContainer, container_buffer);
-    my_free(responseContainer.data.data);
-    SendChars((char*)container_buffer, container_size);
+    if (requestContainer)
+    {
+        responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size);
+        responseContainer.continuous = false;
+        uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+        size_t container_size = message_container__pack(&responseContainer, container_buffer);
+        my_free(responseContainer.data.data);
+        SendChars((char*)container_buffer, container_size);
+    }
 
     ThreadLoadButton(LoadStages);
     return OK;
@@ -1544,7 +1526,8 @@ uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer)
 uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 {
 
-    ThreadLoadingReport();
+    if(ThreadLoadingActive())
+        ThreadLoadingReport();
     return OK;
 }
 
@@ -1567,13 +1550,16 @@ uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer)
     MessageContainer responseContainer;
     MessageState = 2;
     ThreadLoadingReport();
-    if (request->processparameters)
+    if (ThreadLoadingRestartFlag == false)
     {
-        dryerbufferlength = request->processparameters->dryerbufferlength;
-        LoadArmRounds = (int)(request->processparameters->dryerbufferlength);
+        if (request->processparameters)
+        {
+            dryerbufferlength = request->processparameters->dryerbufferlength;
+            LoadArmRounds = (int)(request->processparameters->dryerbufferlength);
 
+        }
+        Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0);
     }
-    Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0);
 
     responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size);
     responseContainer.continuous = false;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 275e7e5b7..d7bfb1b6e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -162,7 +162,7 @@ int FeederSpeedIndex = 0,Speed_i;
 
 void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
-    REPORT_MSG(length,"ThreadUpdateProcessLength");
+    ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0);
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
@@ -337,7 +337,10 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }**/
 
     //}
-
+#ifdef FOUR_WINDERS
+        if (CurrentControlledSpeed[WINDER_MOTOR]>100)
+            length = dyeingspeed/10;
+#endif
     PoolerTotalProcessedLength+= (length/100);
     TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
 #ifndef FEEDER_LENGTH_CALCULATION
@@ -642,11 +645,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             c++;
         }
-        if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR))
+        //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR))
+        /*if (index >= WINDER_MOTOR)
         {
             //pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
-        }
+        }*/
 #endif
         if (index == POOLER_MOTOR)
         {
@@ -738,12 +742,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
         //if (0)//(JobCounter % 1000 == 0)
 #ifdef FOUR_WINDERS
-        if (JobCounter % 500 < 4)//(FirstCalcInJob == true)
+        if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true)
         {
             if (index >= WINDER_MOTOR)
             {
            //     FirstCalcInJob = false;
-                len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index,
+                len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1,
                                 TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
                                 (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));