State machine and job handling. motor speed/position is not clean

13 files changed, 276 insertions, 128 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index e7225f543..c3a17e6a8 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -7,6 +7,7 @@
 #include "drivers/FPGA/Moters_Driver/L6470.h"
 #include "Drivers/Motors/Motor.h"
 #include "modules/control/millisecTask.h"
+#include "modules/thread/thread.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
@@ -267,11 +268,14 @@ int MotorGetFPGAResponse(TimerMotors_t MotorId,uint32_t *Data)  //returns -1 on
 }
 //---------------- SET ------------------------------
 
-
+//uint32_t MotorSentData[NUM_OF_MOTORS][100] = {0};
+//int MotorDataIndex[NUM_OF_MOTORS] = {0};
 int MotorSendFPGARequest(TimerMotors_t MotorId,uint32_t Data, uint32_t Length)  //returns -1 on busy or other error
 {
     INT2SHORT Int2Short;
-
+//    MotorSentData[MotorId][MotorDataIndex[MotorId]] = Data;
+//    MotorDataIndex[MotorId]+=1;
+//    if (MotorDataIndex[MotorId] == 99) MotorDataIndex[MotorId] = 0;
     //if((*FpgaMotMap[MotorId].NBUSY == NOTBUSY) && (*FpgaMotMap[MotorId].SPI_Busy == NOTBUSY))
     {
         Int2Short.uint = Data;
@@ -335,6 +339,9 @@ typedef enum
 MOTOR_CONFIG_STAGES_ENUM ConfigStages[NUM_OF_MOTORS] = {MOTOR_CONFIG_INIT};
 uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
 {
+    char MultiStep[8] = {1,2,4,8,16,32,64,128};
+    char i;
+    char good = 0xFF;
     uint32_t temp = 0;
     if (_motorId > NUM_OF_MOTORS) return ERROR;
     switch (ConfigStages[_motorId])
@@ -343,150 +350,161 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
         ConfigStages[_motorId]++;
         temp = x_SET_PARAM | x_ACC;
     	temp = temp << 24;
-    	temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8);
+    	temp |= ((int)(MotorsCfg[_motorId].maxchangeslope)<<8);
         MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
         break;
     case MOTOR_CONFIG_ACC:
         ConfigStages[_motorId]++;
         temp = x_SET_PARAM | x_DEC;
 	    temp = temp << 24;
-	    temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope)<<8);
+	    temp |= ((int)(MotorsCfg[_motorId].maxchangeslope)<<8);
         MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
         break;
     case MOTOR_CONFIG_DEC:
         ConfigStages[_motorId]++;
         temp = x_SET_PARAM | x_MAX_SPEED;
     temp = temp << 24;
-    temp |= MaxSpdCalc(MotorDriverCfg[_motorId].maxfrequency)<<8;
+    temp |= MotorsCfg[_motorId].maxfrequency<<8;
         MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
         break;
     case MOTOR_CONFIG_MAX_SPEED:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_microstep)
+        if(MotorsCfg[_motorId].has_microstep)
         {
-            temp = x_SET_PARAM | x_STEP_MODE;
-        temp = temp << 24;
-        temp |= (!x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1)<<16;
-            MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
-            break;
+            for (i = 0; i < 8; i++)
+            {
+                if (MotorsCfg[_motorId].microstep == MultiStep[i])
+                {
+                    good = i;
+                    break;
+                }
+            }
+            if(good != 0xFF)
+            {
+                temp = x_SET_PARAM | x_STEP_MODE;
+                temp = temp << 24;
+                temp |= (!x_SYNC_EN | good | x_SYNC_SEL_1)<<16;
+                MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
+                break;
+            }
         }
     case MOTOR_CONFIG_STEP_MODE:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_configword)
+        if(MotorsCfg[_motorId].has_configword)
         {
             temp = x_SET_PARAM | x_CONFIG;
             temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].configword<<8;
+        temp |= MotorsCfg[_motorId].configword<<8;
             MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
             break;
         }
     case MOTOR_CONFIG_CONFIG:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_kvalhold)
+        if(MotorsCfg[_motorId].has_kvalhold)
         {
             temp = x_SET_PARAM | x_KVAL_HOLD;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalhold<<16;
+        temp |= MotorsCfg[_motorId].kvalhold<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_KVAL_HOLD:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_kvalrun)
+        if(MotorsCfg[_motorId].has_kvalrun)
         {
             temp = x_SET_PARAM | x_KVAL_RUN;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalrun<<16;
+        temp |= MotorsCfg[_motorId].kvalrun<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_KVAL_RUN:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_kvalacc)
+        if(MotorsCfg[_motorId].has_kvalacc)
         {
             temp = x_SET_PARAM | x_KVAL_ACC;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalacc<<16;
+        temp |= MotorsCfg[_motorId].kvalacc<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_KVAL_ACC:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_kvaldec)
+        if(MotorsCfg[_motorId].has_kvaldec)
         {
             temp = x_SET_PARAM | x_KVAL_DEC;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvaldec<<16;
+        temp |= MotorsCfg[_motorId].kvaldec<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_KVAL_DEC:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_stslp)
+        if(MotorsCfg[_motorId].has_stslp)
         {
             temp = x_SET_PARAM | x_ST_SLP;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].stslp<<16;
+        temp |= MotorsCfg[_motorId].stslp<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_ST_SLP:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_intspd)
+        if(MotorsCfg[_motorId].has_intspd)
         {
             temp = x_SET_PARAM | x_INT_SPD;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].intspd<<8;
+        temp |= MotorsCfg[_motorId].intspd<<8;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_INT_SPD:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_fnslpacc)
+        if(MotorsCfg[_motorId].has_fnslpacc)
         {
             temp = x_SET_PARAM | x_FN_SLP_ACC;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].fnslpacc<<16;
+        temp |= MotorsCfg[_motorId].fnslpacc<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_FN_SLP_ACC:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_fnslpdec)
+        if(MotorsCfg[_motorId].has_fnslpdec)
         {
             temp = x_SET_PARAM | x_FN_SLP_DEC;
             temp = temp << 24;
-            temp |= MotorDriverCfg[_motorId].fnslpdec<<16;
+            temp |= MotorsCfg[_motorId].fnslpdec<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_FN_SLP_DEC:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_overcurrentthreshold)
+        if(MotorsCfg[_motorId].has_overcurrentthreshold)
         {
             temp = x_SET_PARAM | x_OCD_TH;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].overcurrentthreshold<<16;
+        temp |= MotorsCfg[_motorId].overcurrentthreshold<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_OCD_TH:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_stallthreshold)
+        if(MotorsCfg[_motorId].has_stallthreshold)
         {
             temp = x_SET_PARAM | x_STALL_TH;
             temp = temp << 24;
-            temp |= MotorDriverCfg[_motorId].stallthreshold<<16;
+            temp |= MotorsCfg[_motorId].stallthreshold<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
     case MOTOR_CONFIG_STALL_TH:
         ConfigStages[_motorId]++;
-        if(MotorDriverCfg[_motorId].has_thermalcompensationfactor)
+        if(MotorsCfg[_motorId].has_thermalcompensationfactor)
         {
             temp = x_SET_PARAM | x_K_THERM;
             temp = temp << 24;
-            temp |= MotorDriverCfg[_motorId].thermalcompensationfactor<<16;
+            temp |= MotorsCfg[_motorId].thermalcompensationfactor<<16;
             if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
             break;
         }
@@ -524,99 +542,99 @@ uint32_t FPGA_MotorConfig1(TimerMotors_t _motorId, MotorDriverConfigStruc *Motor
     if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     temp = x_SET_PARAM | x_ACC;
     temp = temp << 24;
-    temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope))<<8;
+    temp |= ((int)(MotorsCfg[_motorId].maxchangeslope))<<8;
     if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
 
     temp = x_SET_PARAM | x_DEC;
     temp = temp << 24;
-    temp |= ((int)(MotorDriverCfg[_motorId].maxchangeslope))<<8;
+    temp |= ((int)(MotorsCfg[_motorId].maxchangeslope))<<8;
     if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
 
     temp = x_SET_PARAM | x_MAX_SPEED;
     temp = temp << 24;
-    temp |= MotorDriverCfg[_motorId].maxfrequency<<8;
+    temp |= MotorsCfg[_motorId].maxfrequency<<8;
     if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
 
-    if(MotorDriverCfg[_motorId].has_microstep)
+    if(MotorsCfg[_motorId].has_microstep)
     {
         temp = x_SET_PARAM | x_STEP_MODE;
         temp = temp << 24;
-        temp |= (!x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1)<<16;
+        temp |= (!x_SYNC_EN | MotorsCfg[_motorId].microstep | x_SYNC_SEL_1)<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
 
-    if(MotorDriverCfg[_motorId].has_configword)
+    if(MotorsCfg[_motorId].has_configword)
     {
         temp = x_SET_PARAM | x_CONFIG;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].configword<<8;
+        temp |= MotorsCfg[_motorId].configword<<8;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
     ///////////////////////////////////////////////////////////////
 
-    if(MotorDriverCfg[_motorId].has_kvalhold)
+    if(MotorsCfg[_motorId].has_kvalhold)
     {
         temp = x_SET_PARAM | x_KVAL_HOLD;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalhold<<16;
+        temp |= MotorsCfg[_motorId].kvalhold<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_kvalrun)
+    if(MotorsCfg[_motorId].has_kvalrun)
     {
         temp = x_SET_PARAM | x_KVAL_RUN;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalrun<<16;
+        temp |= MotorsCfg[_motorId].kvalrun<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_kvalacc)
+    if(MotorsCfg[_motorId].has_kvalacc)
     {
         temp = x_SET_PARAM | x_KVAL_ACC;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvalacc<<16;
+        temp |= MotorsCfg[_motorId].kvalacc<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_kvaldec)
+    if(MotorsCfg[_motorId].has_kvaldec)
     {
         temp = x_SET_PARAM | x_KVAL_DEC;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].kvaldec<<16;
+        temp |= MotorsCfg[_motorId].kvaldec<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
 
     ///////////////////////////////////////////////////////////////
-    if(MotorDriverCfg[_motorId].has_stslp)
+    if(MotorsCfg[_motorId].has_stslp)
     {
         temp = x_SET_PARAM | x_ST_SLP;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].stslp<<16;
+        temp |= MotorsCfg[_motorId].stslp<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_intspd)
+    if(MotorsCfg[_motorId].has_intspd)
     {
         temp = x_SET_PARAM | x_INT_SPD<<8;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].intspd;
+        temp |= MotorsCfg[_motorId].intspd;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_fnslpacc)
+    if(MotorsCfg[_motorId].has_fnslpacc)
     {
         temp = x_SET_PARAM | x_FN_SLP_ACC<<16;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].fnslpacc<<16;
+        temp |= MotorsCfg[_motorId].fnslpacc<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-    if(MotorDriverCfg[_motorId].has_fnslpdec)
+    if(MotorsCfg[_motorId].has_fnslpdec)
     {
         temp = x_SET_PARAM | x_FN_SLP_DEC<<16;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].fnslpdec<<16;
+        temp |= MotorsCfg[_motorId].fnslpdec<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
-        if(MotorDriverCfg[_motorId].has_overcurrentthreshold)
+        if(MotorsCfg[_motorId].has_overcurrentthreshold)
     {
         temp = x_SET_PARAM | x_OCD_TH;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].overcurrentthreshold<<16;
+        temp |= MotorsCfg[_motorId].overcurrentthreshold<<16;
         if( MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback) < 0 ) return ERROR;
     }
 
@@ -666,7 +684,7 @@ void FPGA_SetGoMotHome(TimerMotors_t _motorId)
 {
 
     Fpga_Spi[_motorId].TX_MOSI = x_GO_HOME; //TODO to check how to set the number of bytes to send
-    Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 8;// move the command to the MSB TODO necessary???
+    Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary???
     //Fpga_Spi[_motorId].AMT_OF_Words = 1;
 
     FPGA_SPI_Transnit(_motorId);
@@ -676,7 +694,7 @@ void FPGA_SetMotHome(TimerMotors_t _motorId)
 {
 
     Fpga_Spi[_motorId].TX_MOSI = x_RESET_POS; //TODO to check how to set the number of bytes to send
-    Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 8;// move the command to the MSB TODO necessary???
+    Fpga_Spi[_motorId].TX_MOSI = Fpga_Spi[_motorId].TX_MOSI << 24;// move the command to the MSB TODO necessary???
     //Fpga_Spi[_motorId].AMT_OF_Words = 1;
 
     FPGA_SPI_Transnit(_motorId);
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
index 78e65e664..d7e3a8f53 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -27,6 +27,6 @@ uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorC
 
 int SPISendFPGARequest(TEMPERATURE_SENSOR_ID_ENUM SensorId,uint32_t Data, uint32_t Length);
 int SPIGetFPGAResponse(TEMPERATURE_SENSOR_ID_ENUM SensorId,uint32_t *Data);
-void FPGA_SetMotHome(_motorId);
+void FPGA_SetMotHome(TimerMotors_t _motorId);
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index 818c90c96..9f853a3e5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -45,7 +45,7 @@ SPI MotFPGA_Spi;
 MotorDriverResponseStruct MotorDriverResponse[NUM_OF_MOTORS];
 MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS];
 
-MotorDriverConfigStruc  MotorDriverCfg[NUM_OF_MOTORS];
+//MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS];
 TimerMotors_t FastMotorToMotorId[4] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER};
 callback_fptr MotorMoveModuleCallback = 0;
 callback_fptr MotorSetSpeedModuleCallback = 0;
@@ -81,7 +81,7 @@ uint32_t MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorConfig
         setup(MotorConfig);
         return OK;
     #else
-        return FPGA_MotorConfig( _motorId, &MotorConfig);
+        return FPGA_MotorConfig( _motorId, MotorConfig);
     #endif
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index 86dd570b1..ea33ed6a1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -107,7 +107,7 @@ extern MotorDriverRequestStruct MotorDriverRequest[NUM_OF_MOTORS];
 
 extern MotorDriverResponseStruct MotorDriverResponse[NUM_OF_MOTORS];
 
-extern MotorDriverConfigStruc  MotorDriverCfg[NUM_OF_MOTORS];
+extern HardwareMotor  MotorsCfg[NUM_OF_MOTORS];
 
 //initialize general motors driver database
 uint32_t MotorsInit(void);
@@ -166,4 +166,6 @@ uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, int
 
 uint32_t MotorGetStatusFromFPGA_Res(TimerMotors_t _motorId);
 
+uint32_t SetMotHome(TimerMotors_t _motorId);
+
 #endif /* DRIVERS_MOTOR_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
index d1fa015ed..89319786d 100644
--- a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
@@ -773,9 +773,9 @@ void setup(MotorDriverConfigStruc *MotorConfig)
             SetParam(x_FN_SLP_ACC, 0x14);
             SetParam(x_FN_SLP_DEC, 0x14);
     }
-    else //if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)feedder
+    else //if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)feeder
     {
-        //for QSH4218-51-10-049 fidder
+        //for QSH4218-51-10-049 feeder
         SetParam(x_KVAL_HOLD, 0x35);
         SetParam(x_KVAL_RUN, 0x7F);
         SetParam(x_KVAL_ACC, 0x7F);
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 80a7ce48a..9dc03601c 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -49,7 +49,6 @@ typedef struct AlarmHandlingMessage{
 void AlarmHandlingInit(void)
 {
     Error_Block eb;
-    int i;
 
     Error_init(&eb);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 40eee95c8..a0ba50078 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -55,7 +55,7 @@
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
-#define MAX_TANGO_CONTROL_DEVICES 200
+#define MAX_TANGO_CONTROL_DEVICES 80
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
 
@@ -221,7 +221,7 @@ int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
 
 }
 
-
+uint32_t millisecondCounter = 0;
 void OneMilliSecondControlInterrupt(UArg arg0)
 {
     ControlMessageStruc Message;
@@ -235,11 +235,12 @@ void OneMilliSecondControlInterrupt(UArg arg0)
 
     //send message to the control task
     Message.messageId = OneMillisec;
-    Message.tick = UsersysTickGet();
+    Message.tick = millisecondCounter++;
     Message.msglen = sizeof(ControlMessageStruc);
     if (ControlMsgQ != NULL)
         Mailbox_post(ControlMsgQ , &Message, BIOS_NO_WAIT);
-
+    if (millisecondCounter == 1000000000)
+        millisecondCounter = 0;
     ROM_TimerIntClear(Control_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     //
     // Enable all interrupts.
@@ -258,7 +259,7 @@ uint32_t ControlLoop(uint32_t tick)
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
-    Tick98          =   (tick%eHundredMillisecond == 98) ?true:false;
+    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
     Tick998         =   (tick%eOneSecond == 998)         ?true:false;
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
index 968af26ae..5759617cb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
@@ -9,5 +9,5 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
 void HeatingTestRequest(MessageContainer* requestContainer);
 void HeatingTestPollRequest(MessageContainer* requestContainer);
 
-
+uint32_t HeatersEnd(void);
 uint32_t Heaters_Init(void);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index d3224e9b3..db9e6f246 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -51,12 +51,12 @@ uint32_t  Heater_timerBase = TIMER2_BASE;        //Timer handle
 uint32_t OutputProportionalCycleTime = 0; //A/C Heaters Cycle time in milliseconds - one for all heaters
 char     TimeSliceAllocation[MAX_TIMESLICES] = {0xFF};
 int     DCTimeSliceAllocation[MAX_HEATERS_NUM] = {0};
-bool    HeatersReadey[MAX_HEATERS_NUM] = {true};
+bool    HeaterReady[MAX_HEATERS_NUM] = {true};
 
-bool InitialHeatingState = true;
 bool TimerActivated = false;
 
 Mailbox_Handle          HeatersControlMsgQ = NULL;
+bool InitialHeating = true;
 
 /******************** FUNCTIONS  ********************************************/
 uint32_t HeaterControlCBFunction(uint32_t deviceID, uint32_t readValue);
@@ -95,6 +95,27 @@ void HeatersControlStart(void)
     HeatersStartControlTimer();
 }
 
+uint32_t HeatersEnd(void)
+{
+    int i,status = OK;
+    HeatersControlStop();
+    for (i=0;i<MAX_HEATERS_NUM;i++)
+    {
+        if (i > HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2) //DC Heaters
+        {
+            status |= RemoveControlCallback(ControlIdtoHeaterId [i], DCHeaterControlCBFunction);
+        }
+        else if (i < HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2) //AC Heaters
+        {
+            status |=RemoveControlCallback(ControlIdtoHeaterId [i] ,HeaterControlCBFunction);
+        }
+        DeActivateHeater(i);
+    }
+    return status;
+
+}
+
+
 void HeatersStartControlTimer (void)
 {
     if (TimerActivated == true)
@@ -196,14 +217,30 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
     HeaterPIDConfig[HeaterId].m_preError = 0;
     HeaterPIDConfig[HeaterId].m_SetParam = SetTemperatue*100;//need to update SetParams on presegment stage
     if (HeaterId > HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2) //DC Heaters
+    {
         ControlIdtoHeaterId [HeaterId] = AddControlCallback( DCHeaterControlCBFunction, eHundredMillisecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),HeaterId2PT100Id[HeaterId],0);
+    }
     else if (HeaterId < HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2) //AC Heaters
+    {
         ControlIdtoHeaterId [HeaterId] = AddControlCallback( HeaterControlCBFunction, eHundredMillisecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),HeaterId2PT100Id[HeaterId],0);
+    }
+    HeaterReady[HeaterId] = false;
 
 
     return OK;
 }
-
+void HeaterPrepareReady(void)
+{
+    int i;
+    for (i=0;i<MAX_HEATERS_NUM;i++)
+    {
+        if (HeaterReady[i] == false)
+        {
+            return; //not all configured heaters are ready
+        }
+    }
+    PrepareReady(Module_Heaters,ModuleDone);
+}
 /*
  * HeaterControlCBFunction
  * called by: Communication from host
@@ -213,7 +250,6 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
 uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
-    static bool InitialHeating = true;
     /*char str[100];
     uint8_t len = 0;*/
     if (IfIndex>>8 != IfTypeHeaters)
@@ -267,7 +303,8 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         {
             InitialHeating = false;
             HeatersControlStart();
-            PrepareReady(Module_Heaters,ModuleDone);
+            HeaterReady[index] = true;
+            HeaterPrepareReady();
             HeatingTestSendResonse(0, false,GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w),GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1),
                                Temperature[0],Temperature[1],
                                HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"End Initial Heating");
@@ -305,7 +342,7 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
-    static bool InitialHeating[MAX_HEATERS_NUM] = {true};
+    static bool DCInitialHeating[MAX_HEATERS_NUM] = {true};
     /*char str[100];
     uint8_t len = 0;*/
     if (IfIndex>>8 != IfTypeHeaters)
@@ -335,7 +372,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
                            readValue,0,
                        HeaterPIDConfig[index].m_calculatedError, 0,"Standard");*/
     // check if the read value is within the proportional band
-    if (InitialHeating[index])
+    if (DCInitialHeating[index])
     {
         if (readValue > (HeaterCmd[index].targettemperatue * (1+(HeaterControl[index].outputproportionalband/100))))
                 {
@@ -349,9 +386,10 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
                 }
         else
         {
-            InitialHeating[index] = false;
+            DCInitialHeating[index] = false;
             HeatersControlStart();
-            //PrepareReady(Module_Heaters,ModuleDone);
+            HeaterReady[index] = true;
+            HeaterPrepareReady();
             HeatingTestSendResonse(0, false,GetHeaterState(index),0,
                                    readValue,0,
                                HeaterPIDConfig[index].m_calculatedError, 0,"End Initial Heating");
@@ -463,6 +501,8 @@ uint32_t HeatersControlLoop(uint32_t tick)
 
     for ( DcHeaterId = HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1; DcHeaterId<= HARDWARE_PID_CONTROL_TYPE__MixerHeater;DcHeaterId++)
     {
+        if (HeaterReady[DcHeaterId] == false)
+            continue;
         if (DCTimeSliceAllocation[DcHeaterId] > 0) //heater active
         {
             if (SliceCounter == 0)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index e957b0efc..ea75bffa3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -20,6 +20,8 @@ uint32_t ThreadInitialTestStub();
 uint32_t Winder_Init(void);
 uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails);
+uint32_t Winder_End(void);
+
 
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 1e2203abb..9a7e8cf88 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -34,6 +34,8 @@ uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
+uint32_t    SpeedControlId=0xFF;
+
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
 typedef struct
@@ -81,7 +83,8 @@ void ThreadDistanceToSpoolEnded(void);
 uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
 {
     uint32_t Time_Pass;
-  #define   MAX_COUNTER 0x3FFF  //14 bits
+//  #define   MAX_COUNTER 0x3FFF  //14 bits
+    #define   MAX_COUNTER 0x3FFFFF  //22 bits
 
 
   if (Current_Read < Previous_Read)
@@ -100,6 +103,64 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
  *
  * **************************************************************************************/
 
+void ThreadUpdateProcessLength (double length, void *Funcptr)
+{
+    CurrentRequestedLength = length*100;//Centimetres
+    CurrentProcessedLength = 0;
+    PreviousPosition = 0;
+    CurrentPosition = 0;
+    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
+}
+uint32_t MotorSentData[1000] = {0};
+uint32_t PosDif[1000] = {0};
+uint32_t tick[1000] = {0};
+uint32_t initialpos = 0xFFFF;
+
+
+int MotorDataIndex = 0;
+
+uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    uint32_t positionDiff = 0;
+    double length = 0.0;
+    int index = MAX_THREAD_MOTORS_NUM;
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+    if (index != FEEDER_MOTOR)
+    {
+        LOG_ERROR (IfIndex, "Wrong Motor");
+        return 0xFFFFFFFF;
+    }
+    CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]);
+    if (initialpos == 0xFFFF)
+        initialpos = CurrentPosition;
+    positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
+    positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+    PreviousPosition = CurrentPosition;
+
+    // total length = (position diff / full cycle) * pulley perimeter
+    //(positionDiff/pulseperround)*((2*PI*motor_Radius)
+
+    length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius);
+    totalLength+=length;
+    CurrentProcessedLength+=length;
+    PosDif[MotorDataIndex] = positionDiff;
+    MotorSentData[MotorDataIndex] = length;
+    tick[MotorDataIndex] = UsersysTickGet();
+    MotorDataIndex+=1;
+    if (MotorDataIndex == 999) MotorDataIndex = 0;
+    if (CurrentProcessedLength>=CurrentRequestedLength )
+    {
+        // segment/intersegment/distance to spool finished
+        if (ProcessedLengthFuncPtr)
+            ProcessedLengthFuncPtr();
+    }
+return OK;
+}
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
@@ -140,46 +201,6 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
  return OK;
 }
-void ThreadUpdateProcessLength (double length, void *Funcptr)
-{
-    CurrentRequestedLength = length;
-    CurrentProcessedLength = 0;
-    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
-}
-uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
-{
-    uint32_t positionDiff = 0;
-    double length = 0.0;
-    int index = MAX_THREAD_MOTORS_NUM;
-    if (IfIndex>>8 != IfTypeThread)
-    {
-        LOG_ERROR (IfIndex, "Wrong  Interface type");
-        return 0xFFFFFFFF;
-    }
-    index = IfIndex&0xFF;
-    if (index != FEEDER_MOTOR)
-    {
-        LOG_ERROR (IfIndex, "Wrong Motor");
-        return 0xFFFFFFFF;
-    }
-    CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]);
-    positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
-    PreviousPosition = CurrentPosition;
-
-    // total length = (position diff / full cycle) * pulley perimeter
-    //(positionDiff/pulseperround)*((2*PI*motor_Radius)
-
-    length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius);
-    totalLength+=length;
-    CurrentProcessedLength+=length;
-    if (CurrentProcessedLength>=CurrentRequestedLength )
-    {
-        // segment/intersegment/distance to spool finished
-        if (ProcessedLengthFuncPtr)
-            ProcessedLengthFuncPtr();
-    }
-return OK;
-}
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -217,6 +238,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         if (index == POOLER_MOTOR)
             TranslatedReadValue = (-1*TranslatedReadValue);
+        //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
@@ -295,13 +317,38 @@ bool InitialProcess = false;
             MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            {
+                if (SpeedControlId != 0xFF)
+                {
+                    RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                }
+                SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+                SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+            }
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            }
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            }
             if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
                 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,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
@@ -327,7 +374,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     JobTicket* JobTicket = JobDetails;
     int Motor_i, HW_Motor_Id;
 
-    int process_speed = JobTicket->processparameters->dyeingspeed;
+    float process_speed = JobTicket->processparameters->dyeingspeed;
 
     for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
     {
@@ -389,8 +436,16 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
+     if (SpeedControlId != 0xFF)
+     {
+         RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+     }
      for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
      {
+         if (ControlIdtoMotorId[Motor_i] != 0xFF)
+         {
+             RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+         }
         StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
      StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index 466f2eca5..cfdd0057d 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -367,7 +367,7 @@ Void jobTask(UArg arg0, UArg arg1)
                 PrintSTMMsgHandler(&Message);
                 break;
             case PrintingResultsOk:
-                CleanState(CurrentJob);
+                //CleanState(CurrentJob);
                 break;
             case PrintingResultsFail:
                 //send message data as a validation error message to host
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
index 69c4674b4..2f068e6f5 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
@@ -70,6 +70,7 @@ typedef struct
 ModuleStateEnum SegmentWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
 ModuleStateEnum PreSegmentWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
 ModuleStateEnum PrintWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
+ModuleStateEnum EndWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
 
 bool Configured[MAX_SYSTEM_MODULES] = {false};
 
@@ -258,6 +259,36 @@ static uint32_t SegmentState(void *JobDetails, int SegmentId)
 //********************************************************************************************************************
 static uint32_t EndState(void *JobDetails)
 {
+    if (Configured[Module_Winder])
+    {
+        //    EndWaiting[Module_Winder] = ModuleWaiting;
+        Winder_End();
+    }
+    if (Configured[Module_IDS])
+    {
+        //EndWaiting[Module_IDS] = ModuleWaiting;
+        //IDSEndState(JobDetails);
+    }
+    if (Configured[Module_Heaters])
+    {
+        EndWaiting[Module_Heaters] = ModuleWaiting;
+        //heaters preparation starts on process parameters handling
+        HeatersEnd();
+    }
+    if (Configured[Module_Thread])
+    {
+        EndWaiting[Module_Thread] = ModuleWaiting;
+        ThreadEndState(CurrentJob);
+    }
+
+    JobMessageStruc Message;
+    bool retcode = false;
+
+    Message.messageId = PrintingResultsOk;
+    Message.msglen = MAX_MSG_LEN;
+    if (JobmsgQ != NULL)
+        retcode = Mailbox_post(JobmsgQ , &Message, BIOS_NO_WAIT);
+
     return OK;
 }
 //********************************************************************************************************************