conflict merge

28 files changed, 610 insertions, 198 deletions

diff --git a/Software/Embedded_SW/Embedded/Communication/Container.c b/Software/Embedded_SW/Embedded/Communication/Container.c
index 16da014af..cf1cb3ef5 100644
--- a/Software/Embedded_SW/Embedded/Communication/Container.c
+++ b/Software/Embedded_SW/Embedded/Communication/Container.c
@@ -1,5 +1,4 @@
 #include <PMR/Common/MessageContainer.pb-c.h>
-#include <Stubs_Handler/Stub_I2C.h>
 #include "include.h"
 #include "driverlib/gpio.h"
 
diff --git a/Software/Embedded_SW/Embedded/DataDef.h b/Software/Embedded_SW/Embedded/DataDef.h
index c4db06fb1..7e06853af 100644
--- a/Software/Embedded_SW/Embedded/DataDef.h
+++ b/Software/Embedded_SW/Embedded/DataDef.h
@@ -13,7 +13,6 @@
 
 #define ON  1
 #define OFF 0
-#define test
 
 enum
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c
index f07bd790e..9659e5ff5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c
+++ b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.c
@@ -154,6 +154,7 @@ uint32_t g_pui32ADCSeq[MAX_ADC_DEVICES] =
 
 //#define NUM_ADC_CHANNELS        (sizeof(g_pui32ADCSeq) /                      \
                                  sizeof(g_pui32ADCSeq[0]))
+int DispenserIdToPressureSensorId[MAX_DISPENSERS] = {CHAN_DISPENSE_PRESSURE_1, CHAN_DISPENSE_PRESSURE_2 , CHAN_DISPENSE_PRESSURE_3 , CHAN_DISPENSE_PRESSURE_4 , CHAN_DISPENSE_PRESSURE_5 , CHAN_DISPENSE_PRESSURE_6 , CHAN_DISPENSE_PRESSURE_7 , CHAN_DISPENSE_PRESSURE_8 };
 
 //const uint8_t Num_ADC_Channels = ( sizeof(g_pui32ADCSeq) / sizeof(g_pui32ADCSeq[0]) );
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h
index 87f776c89..b667025e5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h
+++ b/Software/Embedded_SW/Embedded/Drivers/ADC_Sampling/ADC.h
@@ -12,10 +12,13 @@
 //*****************************************************************************
 
 #define  MAX_ADC_DEVICES 20
+#define MAX_DISPENSERS
+
 //#define MAX_ADC_DEVICES NUM_ADC_CHANNELS
 
 #define MAX_DIGITAL_POTENTIOMETER_READ		0x660D
 
+extern int DispenserIdToPressureSensorId[MAX_DISPENSERS];
 typedef void (*ProcessCallback)(uint32_t*  adcData);
 
 void ADCAcquireInit(void);
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 6bfeb4896..cec8b74af 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -383,6 +383,18 @@ uint32_t FPGA_MotorConfig_callback(TimerMotors_t _motorId, uint32_t ReadValue)
         temp |= speed_calc<<8;
         MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
         break;
+    case MOTOR_CONFIG_MIN_SPEED:
+        ConfigStages[_motorId]++;
+        if(MotorsCfg[_motorId].has_lowspeedoptimization)
+        {
+            temp = x_SET_PARAM | x_MIN_SPEED;
+            temp = temp << 24;
+            speed_calc = MaxSpdCalc(MotorsCfg[_motorId].minfrequency);
+            speed_calc|= 0x800;  //LSPD_OPT
+            temp |= speed_calc<<8;
+            MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
+            break;
+        }
     case MOTOR_CONFIG_MAX_SPEED:
         ConfigStages[_motorId]++;
         if(MotorsCfg[_motorId].has_microstep)
@@ -570,6 +582,44 @@ void FPGA_SetMotSpeed(TimerMotors_t _motorId)//Run
 //    FPGA_SPI_Transnit(_motorId);
 //-----------------------------------------------------
 }
+void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId)//Mov
+{
+    uint32_t temp;
+
+
+    temp = x_SET_PARAM | x_MAX_SPEED;
+    temp = temp << 24;
+    uint16_t speed_calc = MaxSpdCalc(MotorsCfg[_motorId].maxfrequency);
+    if (speed_calc > 0x3FF) //TODO to check if it is needed here
+        speed_calc = 0x3FF;
+    temp |= speed_calc<<8;
+
+    MillisecWriteToMotor(_motorId, temp, 4, NULL);
+}
+
+
+void FPGA_SetMotMicroStep(TimerMotors_t _motorId)//Mov
+{
+    char MultiStep[8] = {1,2,4,8,16,32,64,128};
+    char i;
+    char good = 0xFF;
+    uint32_t temp;
+    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, NULL);
+    }
+}
 
 void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov
 {
@@ -585,7 +635,8 @@ void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov
     Fpga_Spi[_motorId].TX_MOSI = temp;
     //Fpga_Spi[_motorId].AMT_OF_Words = 4;
 
-    FPGA_SPI_Transnit(_motorId);
+    MillisecWriteToMotor(_motorId, temp, 4, NULL);
+//   FPGA_SPI_Transnit(_motorId);
 }
 
 void FPGA_SetGoMotHome(TimerMotors_t _motorId)
@@ -613,6 +664,7 @@ void FPGA_SetMotHome(TimerMotors_t _motorId)
 
 void FPGA_SetMotStop(TimerMotors_t _motorId)
 {
+    uint32_t temp;
 
     switch(MotorDriverRequest[_motorId].Stop)
     {
@@ -636,7 +688,9 @@ void FPGA_SetMotStop(TimerMotors_t _motorId)
     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;//TODO ?
 
-    FPGA_SPI_Transnit(_motorId);
+    temp = Fpga_Spi[_motorId].TX_MOSI;
+  if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ;
+//    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 ca3477e7c..046d3e5d9 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -20,6 +20,8 @@ void FPGA_GetBusy();
 void FPGA_SetMotStop(TimerMotors_t _motorId);
 void FPGA_GetMotMicroSteps_Cmd(TimerMotors_t _motorId);
 void FPGA_GetMotPosition_Cmd(TimerMotors_t _motorId);
+void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId);
+void FPGA_SetMotMicroStep(TimerMotors_t _motorId);
 void FPGA_SetMotPosition(TimerMotors_t _motorId);
 void FPGA_SetMotSpeed(TimerMotors_t _motorId);
 void FPGA_SetMotorsInit();
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index fe334c53d..2f4afed73 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -48,21 +48,22 @@ MotorDriverRequestStruct MotorDriverRequest[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;
-callback_fptr MotorMovetoLimitSwitchCallback = 0;
+callback_fptr MotorMoveModuleCallback[NUM_OF_MOTORS];
+callback_fptr MotorSetSpeedModuleCallback[NUM_OF_MOTORS];
+callback_fptr MotorMovetoLimitSwitchCallback[NUM_OF_MOTORS];
 
 uint32_t ControlCallBackFunction(uint32_t deviceID, uint32_t ReadValue);
 uint32_t MotorMoveCallBackFunction(uint32_t deviceId, uint32_t BusyFlag);
 uint32_t MotorSetSpeedCallBackFunction(uint32_t deviceId, uint32_t BusyFlag);
-uint32_t MotorMoveControlId = 0;
-uint32_t MotorSetSpeedControlId = 0;
-uint32_t MotorMovetoLimitSwitchControlId[NUM_OF_MOTORS] = {0};
+uint32_t MotorMoveControlId[NUM_OF_MOTORS];
+uint32_t MotorSetSpeedControlId[NUM_OF_MOTORS];
+uint32_t MotorMovetoLimitSwitchControlId[NUM_OF_MOTORS];
 
 
 //initialize general motors driver database
 uint32_t MotorsInit(void)
 {
+    int i = 0;
     #ifdef EVALUATION_BOARD
         //MOTOR Driver L6470 SPI
         SPI2_Init();
@@ -76,6 +77,16 @@ uint32_t MotorsInit(void)
     #else
         FPGA_SetMotorsInit();
     #endif
+        for (i=0;i<NUM_OF_MOTORS;i++)
+        {
+            MotorMoveControlId[i] = 0xFF;
+            MotorSetSpeedControlId[i] = 0xFF;
+            MotorMovetoLimitSwitchControlId[i] = 0xFF;
+            MotorMoveModuleCallback[i] = 0;
+            MotorSetSpeedModuleCallback[i] = 0;
+            MotorMovetoLimitSwitchCallback[i] = 0;
+        }
+
 
     return OK;
 }
@@ -287,7 +298,7 @@ bool     MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter)
 /*
  * Stop Command
   */
-uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType)
+uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType)
 {
     #ifdef EVALUATION_BOARD
         Stop_Command = StopType;
@@ -311,6 +322,21 @@ uint32_t SetMotHome(TimerMotors_t _motorId)
     return OK;
 }
 
+uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed)
+{
+    MotorsCfg[_motorId].maxfrequency = MaxSpeed;
+    FPGA_SetMotMaxSpeed(_motorId);
+
+    return OK;
+}
+
+uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep)
+{
+    MotorsCfg[_motorId].microstep = microstep;
+    FPGA_SetMotMicroStep(_motorId);
+
+    return OK;
+}
 
 uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps)
 {
@@ -332,22 +358,23 @@ uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t
 
     //call driver action to device id with the parameter
     //SetMotorSpeed (deviceId, parameter);
-    MotorMove(_motorId,direction,Steps );
-    MotorMoveModuleCallback = callback;
+    MotorMoveModuleCallback[_motorId] = callback;
 
-    MotorMoveControlId = AddControlCallback( MotorMoveCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
-    return MotorMoveControlId;
+    MotorStop(_motorId,2 );
+    MotorMove(_motorId,direction,Steps );
+    MotorMoveControlId[_motorId] = AddControlCallback( MotorMoveCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
+    return MotorMoveControlId[_motorId];
 }
 
 uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, callback_fptr callback) //TODO
 {
     assert (callback);
-    MotorSetSpeedModuleCallback = callback;
+    MotorSetSpeedModuleCallback[_motorId] = callback;
 
-    MotorSetSpeedControlId = AddControlCallback( MotorMoveCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
+    MotorSetSpeedControlId[_motorId] = AddControlCallback( MotorSetSpeedCallBackFunction,  eTenMilliSecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
     MotorSetSpeed(_motorId, _freq);
 
-    return MotorSetSpeedControlId;
+    return MotorSetSpeedControlId[_motorId];
 }
 
 uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO
@@ -360,15 +387,16 @@ uint32_t MotorMoveCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO
     }
     MotorId = IfIndex&0xFF;
 
-       if (BusyFlag == 1)
+       if (BusyFlag == NOTBUSY)
        {
            //stop this control loop
-           RemoveControlCallback(MotorMoveControlId, MotorMoveCallBackFunction );
+           RemoveControlCallback(MotorMoveControlId[MotorId], MotorMoveCallBackFunction );
            //possibly: start regular control (speed etc)
            //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter );
 
            //call the module callback
-           MotorMoveModuleCallback(MotorId,BusyFlag);
+           if (MotorMoveModuleCallback[MotorId])
+               MotorMoveModuleCallback[MotorId](MotorId,BusyFlag);
 
        }
    return OK;
@@ -387,19 +415,20 @@ uint32_t MotorSetSpeedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TO
        if (BusyFlag == 1)
        {
            //stop this control loop
-           RemoveControlCallback(MotorSetSpeedControlId, MotorSetSpeedCallBackFunction );
+           RemoveControlCallback(MotorSetSpeedControlId[MotorId], MotorSetSpeedCallBackFunction );
            //possibly: start regular control (speed etc)
            //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter );
 
            //call the module callback
-           MotorMoveModuleCallback(MotorId,BusyFlag);
+           if (MotorSetSpeedModuleCallback[MotorId])
+               MotorSetSpeedModuleCallback[MotorId](MotorId,BusyFlag);
 
        }
    return OK;
 }
 uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwitch) //TODO
 {
-    uint32_t MotorId;
+    TimerMotors_t MotorId;
     if (IfIndex>>8 != IfTypeMotors)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -407,19 +436,23 @@ uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwit
     }
     MotorId = IfIndex&0xFF;
 
-       if (LimitSwitch == LIMIT)
-       {
-           StopMotor((TimerMotors_t)MotorId,Hard_Hiz);
-           //stop this control loop
-           RemoveControlCallback(MotorMovetoLimitSwitchControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction );
-           //possibly: start regular control (speed etc)
-           //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter );
+    if (MotorMovetoLimitSwitchControlId[MotorId] == 0xFF)
+        return ERROR;
+   if (LimitSwitch == LIMIT)
+   {
+       //stop this control loop
+       RemoveControlCallback(MotorMovetoLimitSwitchControlId[MotorId], MotorMoveToLimitSwitchCallBackFunction );
+       MotorMovetoLimitSwitchControlId[MotorId] = 0xFF;
+       MotorStop(MotorId,Hard_Hiz);  //TODO in run time limit switch just reverse direction
 
-           //call the module callback
-           if (MotorMovetoLimitSwitchCallback)
-               MotorMovetoLimitSwitchCallback(MotorId,LimitSwitch);
+       //possibly: start regular control (speed etc)
+       //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, (IfTypeMotors*0x100+_motorId), deviceId, Parameter );
 
-       }
+       //call the module callback
+       if (MotorMovetoLimitSwitchCallback[MotorId])
+           MotorMovetoLimitSwitchCallback[MotorId](MotorId,LimitSwitch);
+
+   }
    return OK;
 }
 uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback) //TODO
@@ -432,8 +465,10 @@ uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t
 
     MotorSetDirection( _motorId, direction);
     MotorSetSpeed(_motorId, Freq);
+    if (MotorMovetoLimitSwitchControlId[_motorId] != 0xFF)
+        return ERROR;
     MotorMovetoLimitSwitchControlId[_motorId] = AddControlCallback( MotorMoveToLimitSwitchCallBackFunction, eOneMillisecond , FPGA_Read_limit_Switches,(IfTypeMotors*0x100+_motorId), LimitSwitchId, 0 );
-    MotorMovetoLimitSwitchCallback = callback;
+    MotorMovetoLimitSwitchCallback[_motorId] = callback;
 
     return MotorMoveControlId;
 }
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index 5fc5b6cc5..e7ebbf0da 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -122,6 +122,10 @@ uint32_t MotorSetDirection(TimerMotors_t _motorId,bool _direction);
 
 uint32_t MotorSetSpeed(TimerMotors_t _motorId, uint32_t _freq);
 
+uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed);
+
+uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep);
+
 uint32_t MotorGetSpeed(TimerMotors_t _motorId);
 
 uint32_t MotorGetPosition(TimerMotors_t _motorId);
@@ -158,12 +162,14 @@ typedef enum
     Soft_Hiz,
 }STOP_TYPE_ENUM;
 
-uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType);
+uint32_t MotorStop(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType);
 
 uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps);
 
 uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t Steps, callback_fptr callback);
 
+bool     MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter);
+
 uint32_t MotorSetSpeedWithCallback (TimerMotors_t _motorId, uint32_t _freq, callback_fptr callback);
 
 uint32_t SetMotHome(TimerMotors_t _motorId);
@@ -173,7 +179,5 @@ uint32_t MotorGetStatusFromFPGA_Callback(TimerMotors_t _motorId,uint32_t Data);
 
 uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t Freq, uint32_t LimitSwitchId, callback_fptr callback);
 
-uint32_t MotorSetSpeedCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag);
-uint32_t MotorMoveToLimitSwitchCallBackFunction(uint32_t IfIndex, bool LimitSwitch);
 
 #endif /* DRIVERS_MOTOR_H_ */
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index 429c2792a..8ec2008a7 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -158,6 +158,7 @@ int main(void)
 
 	CommunicationTaskInit();
 	utilsInit(ui32SysClock);
+	MotorsInit();
 //    init_graphics(ui32SysClock);
 //    clear();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 9dc03601c..e8779d265 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -23,10 +23,17 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "modules/thread/thread_ex.h"
+#include  <PMR/Diagnostics/StartDiagnosticsRequest.pb-c.h>
+#include  <PMR/Diagnostics/StartDiagnosticsResponse.pb-c.h>
+#include  <PMR/Diagnostics/StopDiagnosticsRequest.pb-c.h>
+#include  <PMR/Diagnostics/StopDiagnosticsResponse.pb-c.h>
+
+char DiagnosticsToken[36] = {0};
+bool DiagnosticsActive = false;
+
 
 Task_Handle AlarmHandling_Task_Handle;
 Mailbox_Handle          AlarmHandlingMsgQ = NULL;
-static GateMutex_Handle gateAlarmHandlingDB;
 
 /******************** Functions  ********************************************/
 //uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
@@ -34,6 +41,7 @@ static GateMutex_Handle gateAlarmHandlingDB;
 typedef enum
 {
     AlarmHandlingTrigger,
+    AlarmHandlingSendDiagnostics
 }AlarmHandlingMessages;
 
 typedef struct AlarmHandlingMessage{
@@ -45,6 +53,42 @@ typedef struct AlarmHandlingMessage{
 
 /******************** CODE  ********************************************/
 //**********************************************************************
+uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer)
+{
+    uint32_t status = NOT_SUPPORTED;
+    //MessageContainer responseContainer;
+
+//    ReportInitParams InitParams;
+
+    StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    strcpy (DiagnosticsToken, requestContainer->token);
+    DiagnosticsActive = true;
+
+    start_diagnostics_request__free_unpacked(request,NULL);
+    return status;
+}
+uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT;
+
+    DiagnosticsActive = false;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse,  requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    //USBCDCD_sendData(container_buffer, container_size,10);
+    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+return OK;
+}
 
 void AlarmHandlingInit(void)
 {
@@ -57,13 +101,6 @@ void AlarmHandlingInit(void)
 
     //memset(AlarmHandlingDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
 
-    /*gateAlarmHandlingDB = GateMutex_create(NULL, &eb);
-    if (gateAlarmHandlingDB == NULL)
-    {
-        System_abort("Could not create USB Wait gate");
-    }*/
-
-
     return;
 }
 void AlarmHandlingIterate(UArg arg0)
@@ -80,11 +117,58 @@ void AlarmHandlingIterate(UArg arg0)
 
     return ;
 }
+void AlarmHandler_SendDiagnostics(void)
+{
+    AlarmHandlingMessageStruc Message;
 
+    //send message to the Millisec task
+    Message.messageId = AlarmHandlingSendDiagnostics;
+    Message.tick = UsersysTickGet();
+    Message.msglen = sizeof(AlarmHandlingMessageStruc);
+    if (AlarmHandlingMsgQ != NULL)
+        Mailbox_post(AlarmHandlingMsgQ , &Message, BIOS_NO_WAIT);
+
+
+    return ;
+}
 uint32_t AlarmHandlingLoop(uint32_t tick)
 {
     return OK;
 }
+void SendDiagnostics(void)
+{
+    MessageContainer responseContainer;
+
+    StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT;
+
+    if (DiagnosticsActive == false)
+        return;
+
+    /*DiagnosticsMonitors *monitors;
+    size_t n_digitalpins;
+    DigitalPin **digitalpins;
+    extern bool FPGA_Gpi_Buf[MAX_GPI];
+
+void Read_FPGA_GPI_Rgisters();
+bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch);
+
+    size_t n_componentsstates;
+    ValueComponentState **componentsstates;
+    size_t n_events;
+    Event **events;*/
+
+
+    responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
+    responseContainer.continuous = true;
+
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    //USBCDCD_sendData(container_buffer, container_size,10);
+//    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+}
 /******************************************************************************
  *  ======== messageTsk ========
  *  Task for this function is created statically. See the project's .cfg file.
@@ -107,6 +191,8 @@ void AlarmHandlingTask(UArg arg0, UArg arg1)
             case AlarmHandlingTrigger:
                 AlarmHandlingLoop(Message.tick);
                 break;
+            case AlarmHandlingSendDiagnostics:
+                SendDiagnostics();
             default:
                 break;
         }
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
index c5eb88660..9e1be07e2 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
@@ -9,7 +9,7 @@
 #define MODULES_ALARMHANDLING_ALARMHANDLING_H_
 
 
-
+void AlarmHandler_SendDiagnostics(void);
 
 
 #endif /* MODULES_ALARMHANDLING_ALARMHANDLING_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index b9079919c..67de0271f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -16,6 +16,7 @@
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
+#include "Modules/AlarmHandling/AlarmHandling.h"
 
 #include "MillisecTask.h"
 #include <driverlib/timer.h>
@@ -29,11 +30,9 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA.h"
-#include "drivers/Valves/Valve.h"
 
 #include "modules/thread/thread_ex.h"
-#include "Drivers/Peripheral_GPIO/GPIO.h"
-#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "modules/ids/ids_ex.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -137,7 +136,7 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-
+uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -151,7 +150,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 
     //send message to the Millisec task
     Message.messageId = OneMillisec;
-    Message.tick = UsersysTickGet();
+    Message.tick = msec_millisecondCounter++;;
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
         Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
@@ -229,17 +228,18 @@ uint32_t MillisecLoop(uint32_t tick)
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     //gather Motor data from FPGA
 #ifndef EVALUATION_BOARD
         FPGA_GetBusy();  //load the busy motor information to all motors
         FPGA_Read_limit_Switches_Registers();
         //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
-        //FPGA_GetDispenserValveBusyOCD();
 #endif
+
         temp += MotorActivity;
    if (MotorActivity)
    {
@@ -341,15 +341,20 @@ uint32_t MillisecLoop(uint32_t tick)
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     if (Hundred_msTick)
     {
-        ACTIVITY_RED_LED_CHANGE_MODE;
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        AlarmHandler_SendDiagnostics();
     }
 
-    if (Onesecond_Tick)
+    if (O900Millisecond_Tick)
     {
         ADC_TriggerCollection();
     }
+    if (Onesecond_Tick)
+    {
+        for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
+            CalculateDispenserPressure(Sensor_i);
+    }
 
     /*        OLD CODE
     MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index a0ba50078..8e999a668 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -51,6 +51,7 @@
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "control.h"
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
@@ -93,6 +94,8 @@ static GateMutex_Handle gateControlDB;
 Task_Handle Control_Task_Handle;
 ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
+uint16_t ControlBacklog[1000]={0};
+uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
 /******************** Functions  ********************************************/
 void OneMilliSecondFunction(UArg arg0);
@@ -148,6 +151,25 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+uint32_t ControlActivityLed( uint32_t Parameter1, uint32_t Parameter2)
+{
+    static bool flag = false;
+    if (flag==true)
+    {
+        COMM_RED_LED_ON;
+        flag = false;
+    }
+    else
+    {
+        COMM_RED_LED_OFF;
+        flag = true;
+    }
+return OK;
+}
+uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
 
 void ControlStart(void)
 {
@@ -159,6 +181,8 @@ void ControlStart(void)
         TimerEnable(Control_timerBase, TIMER_A);
 
         ADCAcquireStart(0,1);
+        AddControlCallback( ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );
+
     }
 }
 
@@ -260,45 +284,72 @@ uint32_t ControlLoop(uint32_t tick)
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
-    Tick998         =   (tick%eOneSecond == 998)         ?true:false;
+    Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
         if (ControlArray[Device_i].ControlActive)
         {
+            ControlBacklog[backlogindex]=Device_i;
+            if ( ++backlogindex >= 999)
+                backlogindex = 0;
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
+                    if(ControlArray[Device_i].ControlDataReadPtr)
                         ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                    else
+                        LOG_ERROR (Device_i, "Invalid callback ptr");
+                    if(ControlArray[Device_i].ControlCallbackPtr)
                         ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                    else
+                        LOG_ERROR (Device_i, "Invalid callback ptr");
                     break;
                 case eTenMilliSecond:
                     if (Ten_msTick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 case eHundredMillisecond:
                     if (Tick98)
                     {
                         //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     if (Hundred_msTick)
                     {
                         //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 case eOneSecond:
                     if (Tick998)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     if (Onesecond_Tick)
                     {
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 default:
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index ff9acdb94..69ff06880 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -486,7 +486,7 @@ uint32_t HeatersControlLoop(uint32_t tick)
 {
     //char str[100];
     //uint8_t len = 0;
-    int DcHeaterId,HeaterSliceCounter;
+    int DcHeaterId;
     /*len = usnprintf(str, 100, "\r\n EightMilliSecondHeatersInterrupt SliceCounter %d Owner %d H1000 %d  H2000 %d"
                     ,SliceCounter,TimeSliceAllocation[SliceCounter],HeatersRestart,NumberOFSlicesInUse);
     Report(str, __FILE__,__LINE__,0, RpMessage, SliceCounter, TimeSliceAllocation[SliceCounter]);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
index 2c3910140..c3499454d 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
@@ -6,6 +6,7 @@
 #include "ids_ex.h"
 
 extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS];
+extern float DispenserPressure[MAX_SYSTEM_DISPENSERS];
 uint32_t DispenserConfigMessage(HardwareDispenser * request);
 
 extern HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS];
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index feb38000f..bc9e11749 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -17,5 +17,6 @@ uint32_t IDSSegmentState(void *JobDetails, int SegmentId);
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request);
 
+float CalculateDispenserPressure (int DispenserId);
 
 #endif /* MODULES_IDS_IDS_EX_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
index d53ab3d60..81efa5662 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
@@ -12,6 +12,7 @@
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
 #include "drivers/Motors/Motor.h"
+#include "drivers/ADC_Sampling/adc.h"
 
 #define MAX_CONTROL_SAMPLES 10
 
@@ -19,6 +20,22 @@ HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS];
 
 uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8};
 
+float DispenserPressure[MAX_SYSTEM_DISPENSERS] = {0};
+
+float CalculateDispenserPressure (int DispenserId)
+{
+    unsigned short data = 0;
+    float temp= 0.0;
+
+    data = ADC_GetReading(DispenserIdToPressureSensorId[DispenserId]);
+    temp = 4096 - data;
+    temp *= 3;
+    temp *= 1000; //move to mv
+    temp /= 4096;
+    DispenserPressure[DispenserId] = 3000 - temp;
+    return( DispenserPressure[DispenserId]);
+
+}
 
 
 uint32_t DispenserConfigMessage(HardwareDispenser * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 6739619a7..f4ddc0fbd 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -35,6 +35,7 @@ double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0};
 
 HardwarePidControlType ThreadDispenserIdToControlId[MAX_SYSTEM_DISPENSERS] = {  HARDWARE_PID_CONTROL_TYPE__Dispenser1,HARDWARE_PID_CONTROL_TYPE__Dispenser2,HARDWARE_PID_CONTROL_TYPE__Dispenser3,HARDWARE_PID_CONTROL_TYPE__Dispenser4,HARDWARE_PID_CONTROL_TYPE__Dispenser5,HARDWARE_PID_CONTROL_TYPE__Dispenser6,HARDWARE_PID_CONTROL_TYPE__Dispenser7,HARDWARE_PID_CONTROL_TYPE__Dispenser8};
 
+bool    DispenserReady[MAX_SYSTEM_DISPENSERS] = {true};
 
 /******************** STRUCTURES AND ENUMs  ********************************************/
 /******************** GLOBAL PARAMETERS  ********************************************/
@@ -83,7 +84,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 //callback - calls printing stm with the result
 
 // registration - 10 msec, dispenser pressure sensor
-//    AddControlCallback(DeviceId2Heater[HeaterId], HeaterControlCBFunction, eHundredMillisecond);
+//    AddControlCallback(DeviceId2Dispenser[DispenserId], DispenserControlCBFunction, eHundredMillisecond);
 // start the dispenser pressure building - move up in a TBD speed, valve closed
 //Dispenser Speed control
 //callback - handles speed
@@ -94,7 +95,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 //********************************************************************************************************************
  uint32_t IDSPrepareState(void *JobDetails)
 {
-    int Motor_i, HW_Motor_Id, Pid_Id;
+    int Motor_i, HW_Motor_Id, Pid_Id,i;
     //start IDS control for all motors
     for (Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++)
     {
@@ -117,6 +118,11 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 
             MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); //set the dispenser to the
 #warning activate valve
+            /*Start the dispensers to build initial pressure
+             * check different handling for dispensers that participate in the first segment and idle dispensers
+             * start control for initial pressure
+             *
+             */
             //ValveCommand (Enable,MixerDirection);
     #ifdef DEBUG_TEST_FUNCTIONS
 // add control here
@@ -124,36 +130,51 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
     #endif
     }
     //set 3 dancers to the profile positions
+    for (i = 0; i < MAX_SYSTEM_DISPENSERS; i++)
+    {
+        DispenserReady[i] = true;
+    }
 
     return OK;
 }
+ void DispenserPrepareReady(void)
+ {
+     int i;
+     for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
+     {
+         if (DispenserReady[i] == false)
+         {
+             return; //not all configured Dispensers are ready
+         }
+     }
+     PrepareReady(Module_IDS,ModuleDone);
+ }
 
 //********************************************************************************************************************
 uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-    int Dispenser_i, HW_Motor_Id;
+    int Dispenser_i;
+    TimerMotors_t HW_Motor_Id;
     int segmentfirst_speed;
-int CurrentSegment = 0;
+    int CurrentSegment = 0;
+
     JobBrushStopId = 0;
 
+    // activate control fr all motors
+    /* wait for all dispensers  to get to the required pressure
+     * move the presegment ready when all dispensers are ready.
+     */
     for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
     {
         HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
-        //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
-        segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->pulsepersecond;
-        double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
-        //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
-        OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
+        MotorStop(HW_Motor_Id,Hard_Stop);
+        if (JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->has_dispenserstepdivision)
+            MotorSetMicroStep(HW_Motor_Id,JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->dispenserstepdivision);
+        else
+            MotorSetMicroStep(HW_Motor_Id,MotorsCfg[HW_Motor_Id].microstep);
     }
-    //ControlStart();
-    // set the new speed in the dryer Dispenser to the speed of the new segment
-    MotorSetSpeed(HW_Motor_Id, OriginalDispenserSpd_2PPS[HW_Motor_Id]);
-
-    // activate control fr all motors
-    //set speed for both rocker motors
-    //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
     PreSegmentReady(Module_IDS,ModuleDone);
 
@@ -163,6 +184,23 @@ int CurrentSegment = 0;
 //********************************************************************************************************************
  uint32_t IDSSegmentState(void *JobDetails, int SegmentId)
 {
+     JobTicket* JobTicket = JobDetails;
+     int Dispenser_i;
+     TimerMotors_t HW_Motor_Id;
+     double segmentfirst_speed;
+     int CurrentSegment = 0;
+
+
+     for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+     {
+         HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
+         //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
+         segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanolitterpersecond/JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanoliterperpulse;
+//         double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
+         //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
+//         OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
+         MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
+     }
     return OK;
 }
 
@@ -170,12 +208,10 @@ int CurrentSegment = 0;
  uint32_t IDSEndState(void *JobDetails)
 {
      int Motor_i;
-     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
+     for ( Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++)
      {
-        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+        MotorStop(DispenserIdToMotorId[Motor_i],Hard_Hiz);
      }
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
index 669df84a5..49c7d0d3b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
@@ -16,8 +16,6 @@
 
 #include "Stub_Status.h"
 
-#include "drivers/Motors/Motor.h"
-
 void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 {
     uint32_t status = NOT_SUPPORTED;
@@ -25,12 +23,7 @@ void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 
     StubCartridgeReadRequest* request = stub_cartridge_read_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
-    MillisecStart();
-    ControlStart();
-
-    //MotorMovetoLimitSwitch (14,1, 1000, 32, MotorSetSpeedCallBackFunction);
-    MotorSetSpeed( 15,  1000);
-    //request->cartridgeid
+	//request->cartridgeid
 
     writeLine("cartridge Request: ");
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
index 9584ee39b..9202d9936 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
@@ -119,7 +119,7 @@ void Stub_I2CWriteBytesRequest(MessageContainer* requestContainer)
 
     //----------------------
 
-    status = I2C_Write(request->i2cid, request->slaveaddress, I2C_W_buf, request->n_bytestwrite);
+    status = Write_I2C(request->i2cid, request->slaveaddress, I2C_W_buf, request->n_bytestwrite);
 
     //--------------------------
 
@@ -159,7 +159,7 @@ void Stub_I2CReadBytesRequest(MessageContainer* requestContainer)
     response.slaveaddress = request->slaveaddress;
     response.has_slaveaddress = true;
 
-    status = I2C_Read(request->i2cid, request->slaveaddress, I2C_R_buf, request->numberofbytestoread);
+    status = Read_I2C(request->i2cid, request->slaveaddress, I2C_R_buf, request->numberofbytestoread);
 
     //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytetowrite );
     //response.has_readbyte= true;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index 3b9cfe9a8..ff545b516 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -294,7 +294,7 @@ void Stub_MotorStopRequest(MessageContainer* requestContainer)
     Mot_Stop();
 #else
     TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning
-    StopMotor(Motor_Id, Stop_Command);
+    MotorStop(Motor_Id, Stop_Command);
 #endif
 	
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index db3bfc46e..dad7a1db6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -55,6 +55,7 @@ extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 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];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
@@ -65,8 +66,5 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
-uint32_t ThreadPrepareState(void *JobDetails);
-uint32_t Winder_Prepare(void);
-
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index b61247fe4..3505ff3c6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -9,53 +9,132 @@
 #include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "drivers/Motors/Motor.h"
 #include "StateMachines/Printing/PrintingSTM.h"
+#include "Modules/Control/Control.h"
+
+#include "drivers/FPGA/FPGA.h"
+
+#include "drivers/FPGA/FPGA_SPI_Comm.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+
 bool Winder_ScrewHoming = false;
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
-uint32_t Winder_PrepareStage2(void);
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
+uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
 
-double ScrewSpeed = 0;
+bool ScrewCurrentDirection = false;  //holds current screw direction
+bool BusyfirstCall = true;           //Ignores first call after activating the screw - too early
+double ScrewSpeed = 0;               //ScrewSpeed
+uint32_t ScrewControlId = 0xFF;
+uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
+uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw  - will be used to build the cone
 
 uint32_t Winder_Init(void)
 {
     return OK;
 }
+/*
+ *uint32_t Winder_Prepare(void *JobDetails)
+
+ * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
+ * report ready to the job STM
+ */
 uint32_t Winder_Prepare(void)
 {
     uint32_t status = 0;
-//    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    //JobTicket* JobTicket = JobDetails;
+
+    //float process_speed = JobTicket->processparameters->dyeingspeed;
+    double ScrewSpeed = 500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
 
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
      * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
      * report ready to the job STM
      */
-    if (PollGPIO(GPI113_LS_SCREW_RIGHT))
+    if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
     {
-        //go to prepare stage 2
-        Winder_PrepareStage2();
+        Winder_PrepareStage2(0,0);
     }
     else
     {
         Winder_ScrewHoming = true;
-        status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1);//make sur to move the cart home
-        status |=  MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
     }
     return status;
 }
-uint32_t Winder_PrepareStage2(void)
+/*
+ * uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
+ *  2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
+ * report ready to the job STM
+ */
+
+uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 {
-    uint32_t status;
-    uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses;
+    uint32_t status=OK;
+    uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+
+    status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
+     //set motor location 0 here
+        return status;
+
+}
+
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
+{
+    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+
+    ScrewCurrentDirection = false;
+    BusyfirstCall = true;
+    ScrewSpeed = 0;
+    ScrewControlId = 0xFF;
+    ScrewNumberOfSteps = 0;
+    DirectionChangeCounter = 0;
+
+    PrepareReady(Module_Winder, ModuleDone);
+    return OK;
+}
+
+/*
+ * this is the main operational function of the screw - run back and forth until stopped
+ */
+/*
+
+bool ScrewCurrentDirection = false;
+bool BusyfirstCall = true;
+double ScrewSpeed = 0;
+uint32_t ScrewControlId = 0xFF;
+uint32_t ScrewNumberOfSteps = 0;
+uint32_t DirectionChangeCounter = 0;
+*/
+
+uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
+{
+    if (BusyfirstCall)
+    {
+        BusyfirstCall = false;
+        return OK;
+    }
+    //ScrewCurrentDirection: false moves out, true moves home
+    if (BusyFlag == NOTBUSY)
+    {
+        if (ScrewCurrentDirection == false)
+        {
+            ScrewCurrentDirection = true;
+        }
+        else
+        {
+            ScrewCurrentDirection = false;
+        }
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
+        DirectionChangeCounter++;
+    }
+/*
+ * calculate new ScrewSpeed and call     MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+ *
+ */
     /*
-     * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
-     * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
-     * report ready to the job STM
+     * calculate cone shape according to DirectionChangeCounter and update ScrewNumberOfSteps
      */
-        status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0);//make sur to move the cart out
-        //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
-        //set motor location 0 here
-        return status;
+    return OK;
 
 }
 
@@ -63,6 +142,7 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
 {
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
+
 uint32_t Winder_Presegment(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
@@ -70,72 +150,54 @@ uint32_t Winder_Presegment(void *JobDetails)
     float screw_speed = 0;
     float RotationsPerSecond;
 
-//    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
 
-/*typedef struct
-{
-    uint32_t startoffsetpulses;
-    uint32_t spoolbackingrate;
-    uint32_t segmentoffsetpulses;
-    uint32_t milimetersperrotation;
-    int32 SpoolBottomBackingRate;// the angle of the bottom of the spool
-    double NumberOfRotationPerPassage;  // how many rotations per spool passage
-}InternalWinderConfigStruc;
- * */
-//     * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
-//     * calculate
-//     * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
-    // calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
+    //     * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
+    //     * calculate
+    //     * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
+    //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
     screw_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
     // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
-    RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * 3.1416);
+    RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI);
     // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
-    screw_speed = screw_speed*RotationsPerSecond;
+    ScrewSpeed = screw_speed*RotationsPerSecond;
+
+    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
     //screw_speed = InternalWinderCfg.milimetersperrotation
 //     * 2. determine optimal micro-step setting
 //     * 3. calculate cart travel length from winding parameters
 //     * 4. start move of travel length
 //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
+    ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+    MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
+    ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
 
-    //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back.
-     MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
+    // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
     //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
+    PreSegmentReady(Module_Winder,ModuleDone);
 
     return OK;
 }
 uint32_t Winder_End(void)
 {
     //stop screw
-    return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+    ScrewNumberOfSteps = 0;
+    if (ScrewControlId != 0xFF)
+        RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
+    return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
     }
-    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
+    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
 void Winder_ScrewOutLimitSwitchInterrupt(void)
 {
     uint32_t status;
-    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
-}
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
-{
-    if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses)
-    {
-        PrepareReady(Module_Winder, ModuleDone);
-        return OK;
-    }
-    else
-    {
-        //do we want to do something?
-        PrepareReady(Module_Winder, ModuleFail);
-        return ERROR;
-    }
-
+    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index ea75bffa3..b7401c77f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -2,6 +2,8 @@
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
 
+#include "PMR/Printing/JobSpool.pb-c.h"
+
 //typedef enum
 //{
 #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer
@@ -10,6 +12,8 @@
 #define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 //} DANCER_ENUM;
 
+uint32_t InternalWindingConfigMessage(JobSpool* request);
+
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 0e276fe92..3c9adbc31 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -17,6 +17,7 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
+int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
 HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
 
@@ -97,10 +98,12 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
-    temp=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
+    temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
-
+    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2);
+    DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
 uint32_t DancerConfigMessage(HardwareDancer * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 92bfa9c92..e9cc3ddde 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -22,6 +22,7 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
 #include "drivers/Motors/Motor.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
@@ -36,8 +37,9 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 uint32_t    SpeedControlId=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;
@@ -66,6 +68,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
 void ThreadSegmentEnded(void);
 void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
+
+double KeepNormalizedError = 0;
 ////////////////////////Slow Motor State////////////////////////////////////
 //uint32_t ThreadPreSegmentState(void *JobDetails);
 
@@ -118,7 +122,6 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
 }
 double MotorSentData[1000] = {0};
 uint32_t PosDif[1000] = {0};
-uint32_t tick[1000] = {0};
 
 
 int MotorDataIndex = 0;
@@ -163,13 +166,12 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         totalLength+=length;
     }
 
-#warning control disabled
+//#warning control disabled
         CurrentProcessedLength+=length;
-#warning control disabled
+
     PosDif[MotorDataIndex] = CurrentPosition;
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
-    tick[MotorDataIndex] = UsersysTickGet();
     MotorDataIndex+=1;
     if (MotorDataIndex == 999) MotorDataIndex = 0;
     static int pooler_counter = 0;
@@ -177,6 +179,8 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     if (pooler_counter%10 == 0)
     {
         SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+        //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false);
+
     }
     if (pooler_counter>=100)
     {
@@ -187,8 +191,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     if (CurrentProcessedLength>=CurrentRequestedLength )
     {
-        SendJobProgress(100,0,true);
-		#warning handle job wit several segments!!!
         // segment/intersegment/distance to spool finished
         if (ProcessedLengthFuncPtr)
             ProcessedLengthFuncPtr();
@@ -280,10 +282,14 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
-            return;
+            return OK;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-        if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite
+        if (index == POOLER_MOTOR)
+        {
+			//pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
+            JobCounter++;
+        }
         //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
@@ -291,12 +297,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+        //Stop Execution if the dancer moves too much
+
+        if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
+        {
+            EndState(CurrentJob);
+        }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
+        DancerError[DancerId] = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((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
+        if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
             MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        else
+            KeepNormalizedError = NormalizedError;
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
@@ -309,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         pooler_counter++;
         if (pooler_counter>=1000)
         {
-            float error_integered = MotorControlConfig[index].m_calculatedError*1000;
+            //float error_integered = MotorControlConfig[index].m_calculatedError*1000;
             /*{
   "HeaterGroupId": 0,
   "Zone1Temp": 80,
@@ -324,7 +339,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 }*/
             //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
 
-            HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed");
+            HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed");
                 pooler_counter = 0;
         }
     }
@@ -350,6 +365,8 @@ bool InitialProcess = false;
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
     CurrentSegmentId = 0;
+
+    JobCounter = 0;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
@@ -469,12 +486,13 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 //#warning rocker disabled
     if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize)
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5);
     if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize)
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5);
 //#warning rocker disabled
 
+//    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO
 
     // activate control fr all motors
     //set speed for both rocker motors
@@ -486,6 +504,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     }
     else
     {
+        ThreadUpdateProcessLength (0,(void *)NULL);
         PreSegmentReady(Module_Thread,ModuleDone);
         InitialProcess = false;
     }
@@ -527,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
          {
              RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
          }
-        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+        MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index e748d3ef4..e3405689e 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -54,8 +54,9 @@ Mailbox_Handle JobmsgQ = NULL;
 
 
 #define MAX_TICKET_SIZE  10000
-char CurrentJobBuffer[MAX_TICKET_SIZE];
-JobTicket *CurrentJob = (JobTicket *)CurrentJobBuffer;
+//char CurrentJobBuffer[MAX_TICKET_SIZE];
+JobTicket *CurrentJob = NULL;//(JobTicket *)CurrentJobBuffer;
+JobRequest *CurrentJobRequest  = NULL;
 //char PreviousJobBuffer[MAX_TICKET_SIZE];
 //JobTicket *PreviousJob = (JobTicket *)PreviousJobBuffer;
 
@@ -133,30 +134,41 @@ static ReturnCode PrepareState(void *JobDetails)
     ReturnCode retcode;
     retcode = JobSuccess;
     //start (fast??) heating
-    //wait for fast heating to end
-    //start other peripheral systems: chiller, waist handling
-    //check thread type
-    if (Configured[Module_Winder])
-    {
-        //    PrepareWaiting[Module_Winder] = ModuleWaiting;
-        Winder_Prepare();
-    }
     if (Configured[Module_IDS])
     {
         //PrepareWaiting[Module_IDS] = ModuleWaiting;
-        //IDSPrepareState(JobDetails);
     }
     if (Configured[Module_Heaters])
     {
         PrepareWaiting[Module_Heaters] = ModuleWaiting;
-        //heaters preparation starts on process parameters handling
-        //IDSPrepareState(JobDetails);
     }
     if (Configured[Module_Thread])
     {
         PrepareWaiting[Module_Thread] = ModuleWaiting;
+    }
+    if (Configured[Module_Winder])
+    {
+        PrepareWaiting[Module_Winder] = ModuleWaiting;
+    }
+    //wait for fast heating to end
+    //start other peripheral systems: chiller, waist handling
+    //check thread type
+    if (Configured[Module_IDS])
+    {
+        //IDSPrepareState(JobDetails);
+    }
+/*    if (Configured[Module_Heaters])
+    {
+        //heaters preparation starts on process parameters handling
+    }*/
+    if (Configured[Module_Thread])
+    {
         ThreadPrepareState(CurrentJob);
     }
+    if (Configured[Module_Winder])
+    {
+        Winder_Prepare();
+    }
 
 	return retcode;
 }
@@ -255,7 +267,7 @@ void JobAbortFunc(MessageContainer* requestContainer)
     AbortJobRequest* request = abort_job_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     EndState(CurrentJob);
 
-    AbortJob();
+    //AbortJob();
     AbortJobResponse response = ABORT_JOB_RESPONSE__INIT;
     responseContainer = createContainer(MESSAGE_TYPE__AbortJobResponse, JobToken, false, &response, &abort_job_response__pack, &abort_job_response__get_packed_size);
     container_buffer = malloc(message_container__get_packed_size(&responseContainer));
@@ -264,6 +276,11 @@ void JobAbortFunc(MessageContainer* requestContainer)
     free(responseContainer.data.data);
     SendChars((char*)container_buffer, container_size);
     abort_job_request__free_unpacked(request,NULL);
+    //We keep the job request until it is done
+    job_request__free_unpacked(CurrentJobRequest,NULL);
+    CurrentJob = NULL;
+    CurrentJobRequest = NULL;
+
 
 }
 
@@ -275,6 +292,9 @@ void JobRequestFunc(MessageContainer* requestContainer)
     uint8_t* container_buffer;
 
 
+    if (CurrentJobRequest!= NULL)
+        job_request__free_unpacked(CurrentJobRequest,NULL);
+
     JobRequest* request = job_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
     strcpy (JobToken, requestContainer->token);
@@ -287,9 +307,14 @@ void JobRequestFunc(MessageContainer* requestContainer)
     }
     else
     {
-        memcpy(CurrentJob, Ticket,TicketSize);
+        //memcpy(CurrentJob, Ticket,TicketSize);
+        CurrentJob = Ticket;
+        CurrentJobRequest = request;
+
         status  = PASSED;
         HandleProcessParameters(Ticket->processparameters);
+        InternalWindingConfigMessage(Ticket->spool);
+
 //        PrepareWaiting[Module_Heaters] = ModuleWaiting;
     }
     if (status == PASSED)
@@ -333,7 +358,8 @@ void JobRequestFunc(MessageContainer* requestContainer)
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     free(responseContainer.data.data);
     SendChars((char*)container_buffer, container_size);
-    job_request__free_unpacked(request,NULL);
+    //We keep the job request until it is done
+    //    job_request__free_unpacked(request,NULL);
 
     //free(container_buffer);
     //free(requestContainer);
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
index c93c85df0..c6ea98ba3 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
@@ -19,6 +19,7 @@
 #include "include.h"
 #include "./printingSTM.h"
 #include "modules/thread/thread_ex.h"
+#include "modules/Heaters/Heaters_ex.h"
 #include "modules/ids/ids_ex.h"
 #include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
@@ -173,16 +174,25 @@ static uint32_t PreSegmentState(void *JobDetails, int SegmentId)
     if (Configured[Module_Thread])
     {
         PreSegmentWaiting[Module_Thread] = ModuleWaiting;
-        ThreadPreSegmentState(JobDetails);
     }
     if (Configured[Module_Winder])
     {
         PreSegmentWaiting[Module_Winder] = ModuleWaiting;
-        Winder_Presegment(JobDetails);
     }
     if (Configured[Module_IDS])
     {
         PreSegmentWaiting[Module_IDS] = ModuleWaiting;
+    }
+    if (Configured[Module_Thread])
+    {
+        ThreadPreSegmentState(JobDetails);
+    }
+    if (Configured[Module_Winder])
+    {
+        Winder_Presegment(JobDetails);
+    }
+    if (Configured[Module_IDS])
+    {
         IDSPreSegmentState(JobDetails,SegmentId);
     }
     return OK;
@@ -247,6 +257,8 @@ static uint32_t SegmentState(void *JobDetails, int SegmentId)
 //********************************************************************************************************************
 uint32_t EndState(void *JobDetails)
 {
+    SendJobProgress(100,0,true);
+
     if (Configured[Module_Winder])
     {
         //    EndWaiting[Module_Winder] = ModuleWaiting;
@@ -255,17 +267,17 @@ uint32_t EndState(void *JobDetails)
     if (Configured[Module_IDS])
     {
         //EndWaiting[Module_IDS] = ModuleWaiting;
-        //IDSEndState(JobDetails);
+        IDSEndState(JobDetails);
     }
     if (Configured[Module_Heaters])
     {
-        EndWaiting[Module_Heaters] = ModuleWaiting;
+        //EndWaiting[Module_Heaters] = ModuleWaiting;
         //heaters preparation starts on process parameters handling
         HeatersEnd();
     }
     if (Configured[Module_Thread])
     {
-        EndWaiting[Module_Thread] = ModuleWaiting;
+        //EndWaiting[Module_Thread] = ModuleWaiting;
         ThreadEndState(CurrentJob);
     }