handle heater off command, improves screw speed, improve diagnostics

9 files changed, 219 insertions, 274 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 83c20f769..28cdc3702 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -12,7 +12,7 @@
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
-//by recieved esign flow of the user from the UI
+//by recieved design flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
@@ -269,11 +269,12 @@ 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,O900Millisecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98;
     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;
+    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
 
@@ -386,10 +387,23 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         Speed_Data = Read_Speed_Sensor();
     }
+    if (Tick98)
+    {
+        //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+        //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+        //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+        //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+        //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    }
     if (Hundred_msTick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+        //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+        //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+        //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+        //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
 
     if (O900Millisecond_Tick)
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 49be765cc..ead95388a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -37,6 +37,7 @@
 #include  <PMR/Diagnostics/EventType.pb-c.h>
 #include "PMR/debugging/DebugLogCategory.pb-c.h"
 
+#include "StateMachines/Printing/PrintingSTM.h"
 
 char DiagnosticsToken[36+1] = {0};
 bool DiagnosticsActive = false;
@@ -77,10 +78,10 @@ double dryerzone2temperature[DIAGNOSTICS_LIMIT];
 double dryerzone3temperature[DIAGNOSTICS_LIMIT];
 
 
-double dispensermotorfrequency[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS];
-double dispenserspressure[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS];
+double dispensermotorfrequency[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT];
+double dispenserspressure[MAX_SYSTEM_DISPENSERS][DIAGNOSTICS_LIMIT];
 
-double dispenser1motorfrequency[DIAGNOSTICS_LIMIT];
+/*double dispenser1motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser2motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser3motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser4motorfrequency[DIAGNOSTICS_LIMIT];
@@ -88,7 +89,7 @@ double dispenser5motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser6motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser7motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser8motorfrequency[DIAGNOSTICS_LIMIT];
-
+*/
 Task_Handle Diagnostics_Task_Handle;
 Mailbox_Handle          DiagnosticsMsgQ = NULL;
 
@@ -122,13 +123,13 @@ void DiagnosticsInit(void)
     return;
 }
 uint8_t HeaterCounterIndex[MAX_HEATERS_NUM]= {0,0,0,0,0,0,0,0,0,0};
-Double HeaterTemperature[MAX_HEATERS_NUM][DIAGNOSTICS_LIMIT];
+double HeaterTemperature[MAX_HEATERS_NUM][DIAGNOSTICS_LIMIT];
 uint8_t  DancerCounterIndex[NUM_OF_DANCERS]= {0,0,0};
-uint16_t DancerValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT];
+double DancerValue[NUM_OF_DANCERS][DIAGNOSTICS_DANCER_LIMIT];
 uint8_t  MotorCounterIndex[MAX_THREAD_MOTORS_NUM]= {0,0,0};
-uint16_t MotorValue[MAX_THREAD_MOTORS_NUM][DIAGNOSTICS_LIMIT];
+double MotorValue[MAX_THREAD_MOTORS_NUM][DIAGNOSTICS_LIMIT];
 uint8_t  SpeedCounterIndex= 0;
-uint32_t SpeedValue[DIAGNOSTICS_DANCER_LIMIT];
+double SpeedValue[DIAGNOSTICS_DANCER_LIMIT];
 
 void DiagnosticLoadMotor(int MotorId, int frequency)
 {
@@ -180,7 +181,8 @@ void DiagnosticTenMsecCollection(void)
 {
     if (DiagnosticsActive == false)
         return;
-
+    if (JobIsActive()== false)
+        return;
     DiagnosticLoadDancer(WINDER_DANCER,Control_Read_Dancer_Position(WINDER_DANCER, 0,0));
     DiagnosticLoadDancer(POOLER_DANCER,Control_Read_Dancer_Position(POOLER_DANCER, 0,0));
     DiagnosticLoadDancer(FEEDER_DANCER,Control_Read_Dancer_Position(FEEDER_DANCER, 0,0));
@@ -197,11 +199,14 @@ void Diagnostic100msecCollection(void)
 //    dancer2angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(POOLER_DANCER, 0,0);
 //    dancer3angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(FEEDER_DANCER, 0,0);
 
-    DiagnosticLoadMotor(FEEDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING));
-    DiagnosticLoadMotor(DRYER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING));
-    DiagnosticLoadMotor(POOLER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING));
-    DiagnosticLoadMotor(WINDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER));
-    DiagnosticLoadMotor(SCREW_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW));
+    if (JobIsActive())
+    {
+        DiagnosticLoadMotor(FEEDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING));
+        DiagnosticLoadMotor(DRYER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING));
+        DiagnosticLoadMotor(POOLER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING));
+        DiagnosticLoadMotor(WINDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER));
+        DiagnosticLoadMotor(SCREW_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW));
+    }
 /*
     feedermotorfrequency[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
     dryermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
@@ -233,10 +238,10 @@ void Diagnostic100msecCollection(void)
 */
     for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
     {
-        dispensermotorfrequency[DiagnosticsIndex][i] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1+i);
-        dispenserspressure[DiagnosticsIndex][i] = GetDispenserPressure(i);
+        dispensermotorfrequency[i][DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1+i);
+        dispenserspressure[i][DiagnosticsIndex] = GetDispenserPressure(i);
     }
-     dispenser1motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
+/*     dispenser1motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
      dispenser2motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2);
      dispenser3motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3);
      dispenser4motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4);
@@ -244,7 +249,7 @@ void Diagnostic100msecCollection(void)
      dispenser6motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6);
      dispenser7motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7);
      dispenser8motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8);
-
+*/
 /*    for (i=0;i<1;i++)
     {
         DigitalPinArray[i][DiagnosticsIndex] = DIGITAL_PIN__INIT;
@@ -280,16 +285,27 @@ void SendDiagnostics(void)
         return;
 
 
-    /*
-    DiagnosticsMonitor.n_dispenser1motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser2motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser3motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser4motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser5motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser6motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser7motorfrequency = DiagnosticsIndex;
-    DiagnosticsMonitor.n_dispenser8motorfrequency = DiagnosticsIndex;
 
+    if (JobIsActive())
+    {
+        DiagnosticsMonitor.n_dispenser1motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser2motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser3motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser4motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser5motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser6motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser7motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.n_dispenser8motorfrequency = DiagnosticsIndex;
+        DiagnosticsMonitor.dispenser1motorfrequency =   dispensermotorfrequency[0];
+        DiagnosticsMonitor.dispenser2motorfrequency =   dispensermotorfrequency[1];
+        DiagnosticsMonitor.dispenser3motorfrequency =   dispensermotorfrequency[2];
+        DiagnosticsMonitor.dispenser4motorfrequency =   dispensermotorfrequency[3];
+        DiagnosticsMonitor.dispenser5motorfrequency =   dispensermotorfrequency[4];
+        DiagnosticsMonitor.dispenser6motorfrequency =   dispensermotorfrequency[5];
+        DiagnosticsMonitor.dispenser7motorfrequency =   dispensermotorfrequency[6];
+        DiagnosticsMonitor.dispenser8motorfrequency =   dispensermotorfrequency[7];
+    }
+/*
     DiagnosticsMonitor.n_dancer1angle =  DiagnosticsIndex;
     DiagnosticsMonitor.n_dancer2angle =  DiagnosticsIndex;
     DiagnosticsMonitor.n_dancer3angle =  DiagnosticsIndex;
@@ -297,37 +313,39 @@ void SendDiagnostics(void)
     DiagnosticsMonitor.dancer2angle = dancer2angle;
     DiagnosticsMonitor.dancer3angle = dancer3angle;
 */
+    if (JobIsActive())
+    {
+        DiagnosticsMonitor.n_dancer1angle =  DancerCounterIndex[0];
+        DiagnosticsMonitor.n_dancer2angle =  DancerCounterIndex[1];
+        DiagnosticsMonitor.n_dancer3angle =  DancerCounterIndex[2];
+        DiagnosticsMonitor.dancer1angle = DancerValue[0];
+        DiagnosticsMonitor.dancer2angle = DancerValue[1];
+        DiagnosticsMonitor.dancer3angle = DancerValue[2];
 
-    DiagnosticsMonitor.n_dancer1angle =  DancerCounterIndex[0];
-    DiagnosticsMonitor.n_dancer2angle =  DancerCounterIndex[1];
-    DiagnosticsMonitor.n_dancer3angle =  DancerCounterIndex[2];
-    DiagnosticsMonitor.dancer1angle = &DancerValue[0];
-    DiagnosticsMonitor.dancer2angle = &DancerValue[1];
-    DiagnosticsMonitor.dancer3angle = &DancerValue[2];
+        DiagnosticsMonitor.feedermotorfrequency = MotorValue[FEEDER_MOTOR];
+        DiagnosticsMonitor.dryermotor = MotorValue[DRYER_MOTOR];
+        DiagnosticsMonitor.pollermotor = MotorValue[POOLER_MOTOR];
+        DiagnosticsMonitor.windermotor = MotorValue[WINDER_MOTOR];
+        DiagnosticsMonitor.screwmotor = MotorValue[SCREW_MOTOR];
+        DiagnosticsMonitor.n_feedermotorfrequency =  MotorCounterIndex[FEEDER_MOTOR];
+        DiagnosticsMonitor.n_dryermotor =  MotorCounterIndex[DRYER_MOTOR];
+        DiagnosticsMonitor.n_pollermotor =  MotorCounterIndex[POOLER_MOTOR];
+        DiagnosticsMonitor.n_windermotor =  MotorCounterIndex[WINDER_MOTOR];
+        DiagnosticsMonitor.n_screwmotor = MotorCounterIndex[SCREW_MOTOR];
+    }
     memset(DancerCounterIndex,0,sizeof(DancerCounterIndex));
-
-    DiagnosticsMonitor.feedermotorfrequency = &MotorValue[FEEDER_MOTOR];
-    DiagnosticsMonitor.dryermotor = &MotorValue[DRYER_MOTOR];
-    DiagnosticsMonitor.pollermotor = &MotorValue[POOLER_MOTOR];
-    DiagnosticsMonitor.windermotor = &MotorValue[WINDER_MOTOR];
-    DiagnosticsMonitor.screwmotor = &MotorValue[SCREW_MOTOR];
-    DiagnosticsMonitor.n_feedermotorfrequency =  MotorCounterIndex[FEEDER_MOTOR];
-    DiagnosticsMonitor.n_dryermotor =  MotorCounterIndex[DRYER_MOTOR];
-    DiagnosticsMonitor.n_pollermotor =  MotorCounterIndex[POOLER_MOTOR];
-    DiagnosticsMonitor.n_windermotor =  MotorCounterIndex[WINDER_MOTOR];
-    DiagnosticsMonitor.n_screwmotor = MotorCounterIndex[SCREW_MOTOR];
     memset(MotorCounterIndex,0,sizeof(MotorCounterIndex));
 
-    DiagnosticsMonitor.mixertemperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
-    DiagnosticsMonitor.headzone1temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
-    DiagnosticsMonitor.headzone2temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2];
-    DiagnosticsMonitor.headzone3temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3];
-    DiagnosticsMonitor.headzone4temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
-    DiagnosticsMonitor.headzone5temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
-    DiagnosticsMonitor.headzone6temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
-    DiagnosticsMonitor.dryerzone1temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w];
-    DiagnosticsMonitor.dryerzone2temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1];
-    DiagnosticsMonitor.dryerzone3temperature = &HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2];
+    DiagnosticsMonitor.mixertemperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
+    DiagnosticsMonitor.headzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
+    DiagnosticsMonitor.headzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2];
+    DiagnosticsMonitor.headzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3];
+    DiagnosticsMonitor.headzone4temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
+    DiagnosticsMonitor.headzone5temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
+    DiagnosticsMonitor.headzone6temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
+    DiagnosticsMonitor.dryerzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w];
+    DiagnosticsMonitor.dryerzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1];
+    DiagnosticsMonitor.dryerzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2];
 
     DiagnosticsMonitor.n_mixertemperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
     DiagnosticsMonitor.n_headzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
@@ -342,47 +360,29 @@ void SendDiagnostics(void)
 
     memset(HeaterCounterIndex,0,sizeof(HeaterCounterIndex));
 
-    //double dispensermotorfrequency[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS];
-    //double dispenserspressure[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS];
+    if (JobIsActive())
+    {
 
-    DiagnosticsMonitor.n_dispensersmotorsfrequency = MAX_SYSTEM_DISPENSERS;
-    DiagnosticsMonitor.dispensersmotorsfrequency = (DoubleArray**)malloc(sizeof(DoubleArray)*8);
+        DiagnosticsMonitor.n_dispensersmotorsfrequency = MAX_SYSTEM_DISPENSERS;
+        DiagnosticsMonitor.dispensersmotorsfrequency = (DoubleArray**)malloc(sizeof(DoubleArray)*8);
 
-    for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
-    {
-        //double_array__init(&DispenserFreq[i]);
-        //DispenserFreq[i] = dispensermotorfrequency[i];
-        DispenserFreq[i].data = &dispensermotorfrequency[i];
-        DispenserFreq[i].n_data = DiagnosticsIndex;
-        DiagnosticsMonitor.dispensersmotorsfrequency[i] = &DispenserFreq[i];
+        for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
+        {
+            //double_array__init(&DispenserFreq[i]);
+            //DispenserFreq[i] = dispensermotorfrequency[i];
+            DispenserFreq[i].data = dispensermotorfrequency[i];
+            DispenserFreq[i].n_data = DiagnosticsIndex;
+            DiagnosticsMonitor.dispensersmotorsfrequency[i] = &DispenserFreq[i];
+        }
     }
 
-/*
-    DispenserFreq[0].data = (double *)dispenser1motorfrequency;
-    DispenserFreq[0].n_data = DiagnosticsIndex;
-    DispenserFreq[1].data = (double *)dispenser2motorfrequency;
-    DispenserFreq[1].n_data = DiagnosticsIndex;
-    DispenserFreq[2].data = (double *)dispenser3motorfrequency;
-    DispenserFreq[2].n_data = DiagnosticsIndex;
-    DispenserFreq[3].data = (double *)dispenser4motorfrequency;
-    DispenserFreq[3].n_data = DiagnosticsIndex;
-    DispenserFreq[4].data = (double *)dispenser5motorfrequency;
-    DispenserFreq[4].n_data = DiagnosticsIndex;
-    DispenserFreq[5].data = (double *)dispenser6motorfrequency;
-    DispenserFreq[5].n_data = DiagnosticsIndex;
-    DispenserFreq[6].data = (double *)dispenser7motorfrequency;
-    DispenserFreq[6].n_data = DiagnosticsIndex;
-    DispenserFreq[7].data = (double *)dispenser8motorfrequency;
-    DispenserFreq[7].n_data = DiagnosticsIndex;
-*/
-
     DiagnosticsMonitor.n_dispenserspressure = MAX_SYSTEM_DISPENSERS;
     DiagnosticsMonitor.dispenserspressure = (DoubleArray**)malloc(sizeof(DoubleArray)*8);
     for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
     {
         //DiagnosticsDispenserPressure[i] = dispenserspressure[i];
         //double_array__init(&DiagnosticsDispenserPressure[i]);
-        DiagnosticsDispenserPressure[i].data = &dispenserspressure[i];
+        DiagnosticsDispenserPressure[i].data = dispenserspressure[i];
         DiagnosticsDispenserPressure[i].n_data = DiagnosticsIndex;
         DiagnosticsMonitor.dispenserspressure[i] = &DiagnosticsDispenserPressure[i];
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 08940899e..b70c5a744 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -254,15 +254,20 @@ uint32_t DispenserJoggingRequestFunc(MessageContainer* requestContainer)
     {
         if (JobIsActive() == false)
         {
-            //if ((request->has_direction)&&(request->has_speed))
+            if (request->has_direction)
             {
+                switch (request->direction)
+                {
+                case MOTOR_DIRECTION__Forward:
+                    Control3WayValvesWithCallback ((Valves_t) request->index, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
+                    break;
+                case MOTOR_DIRECTION__Backward:
+                    Control3WayValvesWithCallback ((Valves_t) request->index, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
+                    break;
+                }
                 MotorSetDirection(MotorId,request->direction);
                 MotorSetSpeed(MotorId, /*request->speed*/150);
             }
-            /*else
-            {
-                status = ERROR;
-            }*/
         }
         else
         {
@@ -366,6 +371,7 @@ uint32_t DispenserAbortJoggingRequestFunc(MessageContainer* requestContainer)
 
     TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
     MotorStop(MotorId,Hard_Hiz);
+    Control3WayValvesWithCallback ((Valves_t) request->index, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
 
     responseContainer = createContainer(MESSAGE_TYPE__DispenserAbortJoggingResponse,  requestContainer->token, false, &response, &dispenser_abort_jogging_response__pack, &dispenser_abort_jogging_response__get_packed_size);
     responseContainer.continuous = false;
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index dd2c6b17c..e254c9ff1 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -35,199 +35,96 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams)
     {
         ProcessParametersKeep = ProcessParams;
     }
-    if (ProcessParams->has_mixertemp)
-    {
-        if (ProcessParams->mixertemp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__MixerHeater, true,
-                    ProcessParams->mixertemp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__MixerHeater, false,
-                    ProcessParams->mixertemp);
-    }
+    if (ProcessParams->mixertemp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__MixerHeater, true,
+                ProcessParams->mixertemp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__MixerHeater, false,0);
-    }
-    if (ProcessParams->has_headzone1temp)
-    {
-        if (ProcessParams->headzone1temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, true,
-                    ProcessParams->headzone1temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, false,
-                    ProcessParams->headzone1temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__MixerHeater, false,
+                ProcessParams->mixertemp);
+    if (ProcessParams->headzone1temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, true,
+                ProcessParams->headzone1temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, false,0);
-    }
-    if (ProcessParams->has_headzone2temp)
-    {
-        if (ProcessParams->headzone2temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, true,
-                    ProcessParams->headzone2temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, false,
-                    ProcessParams->headzone2temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1, false,
+                ProcessParams->headzone1temp);
+    if (ProcessParams->headzone2temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, true,
+                ProcessParams->headzone2temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, false,0);
-    }
-    if (ProcessParams->has_headzone3temp)
-    {
-        if (ProcessParams->headzone3temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, true,
-                    ProcessParams->headzone3temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, false,
-                    ProcessParams->headzone3temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ2, false,
+                ProcessParams->headzone2temp);
+    if (ProcessParams->headzone3temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, true,
+                ProcessParams->headzone3temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, false,0);
-    }
-    if (ProcessParams->has_headzone4temp)
-    {
-        if (ProcessParams->headzone4temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, true,
-                    ProcessParams->headzone4temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, false,
-                    ProcessParams->headzone4temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ3, false,
+                ProcessParams->headzone3temp);
+    if (ProcessParams->headzone4temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, true,
+                ProcessParams->headzone4temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, false,0);
-    }
-    if (ProcessParams->has_headzone5temp)
-    {
-        if (ProcessParams->headzone5temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, true,
-                    ProcessParams->headzone5temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, false,
-                    ProcessParams->headzone5temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, false,
+                ProcessParams->headzone4temp);
+    if (ProcessParams->headzone5temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, true,
+                ProcessParams->headzone5temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, false,0);
-    }
-    if (ProcessParams->has_headzone6temp)
-    {
-        if (ProcessParams->headzone6temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, true,
-                    ProcessParams->headzone6temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, false,
-                    ProcessParams->headzone6temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, false,
+                ProcessParams->headzone5temp);
+    if (ProcessParams->headzone6temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, true,
+                ProcessParams->headzone6temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, false,0);
-    }
-    if (ProcessParams->has_dryerzone1temp)
-    {
-        if (ProcessParams->dryerzone1temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, true,
-                    ProcessParams->dryerzone1temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, false,
-                    ProcessParams->dryerzone1temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, false,
+                ProcessParams->headzone6temp);
+    if (ProcessParams->dryerzone1temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, true,
+                ProcessParams->dryerzone1temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, false,0);
-    }
-    if (ProcessParams->has_dryerzone2temp)
-    {
-        if (ProcessParams->dryerzone2temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, true,
-                    ProcessParams->dryerzone2temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, false,
-                    ProcessParams->dryerzone2temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, false,
+                ProcessParams->dryerzone1temp);
+    if (ProcessParams->dryerzone2temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, true,
+                ProcessParams->dryerzone2temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, false,0);
-    }
-    if (ProcessParams->has_dryerzone3temp)
-    {
-        if (ProcessParams->dryerzone3temp)
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, true,
-                    ProcessParams->dryerzone3temp);
-        else
-            status |= HeaterCommandRequestMessage(
-                    HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, false,
-                    ProcessParams->dryerzone3temp);
-    }
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, false,
+                ProcessParams->dryerzone2temp);
+    if (ProcessParams->dryerzone3temp)
+        status |= HeaterCommandRequestMessage(
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, true,
+                ProcessParams->dryerzone3temp);
     else
-    {
         status |= HeaterCommandRequestMessage(
-                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, false,0);
-    }
-    if (ProcessParams->has_dyeingspeed)
-    {
-        dyeingspeed = ProcessParams->dyeingspeed;
-    }
-    if (ProcessParams->has_dryerbufferlength)
-    {
-        dryerbufferlength = ProcessParams->dryerbufferlength;
-    }
-    if (ProcessParams->has_mininkuptake)
-    {
-        mininkuptake = ProcessParams->mininkuptake;
-    }
-    if (ProcessParams->has_feedertension)
-    {
-        feedertension = ProcessParams->feedertension;
-    }
-    if (ProcessParams->has_pullertension)
-    {
-        pullertension = ProcessParams->pullertension;
-    }
-    if (ProcessParams->has_windertension)
-    {
-        windertension = ProcessParams->windertension;
-    }
-    if (ProcessParams->has_headairflow)
-    {
-        headairflow = ProcessParams->headairflow;
-    }
-    if (ProcessParams->has_dryerairflow)
-    {
-        dryerairflow = ProcessParams->dryerairflow;
-    }
-    return OK;//status;
+                HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, false,
+                ProcessParams->dryerzone3temp);
+    dyeingspeed = ProcessParams->dyeingspeed;
+    dryerbufferlength = ProcessParams->dryerbufferlength;
+    mininkuptake = ProcessParams->mininkuptake;
+    feedertension = ProcessParams->feedertension;
+    pullertension = ProcessParams->pullertension;
+    windertension = ProcessParams->windertension;
+    headairflow = ProcessParams->headairflow;
+    dryerairflow = ProcessParams->dryerairflow;
+
+   return OK;//status;
 }
 
 void ProcessRequestFunc(MessageContainer* requestContainer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index c0f0fae1f..d8cfdb846 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -238,11 +238,11 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
         //stop the control
         HeaterPIDConfig[HeaterId].m_SetParam  = 0;
     }
-    else
+    /*else
     {
         LOG_ERROR (OnOff, "unsupported command id");
         return ERROR;
-    }
+    }*/
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index bfdb7b5d7..d1e23b657 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -299,7 +299,7 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
  //********************************************************************************************************************
  uint32_t IDSDistanceToSpoolState(void)
  {
-     int Dispenser_i,DispenserId;
+     int Dispenser_i;
      TimerMotors_t HW_Motor_Id;
 
          for (Dispenser_i = 0;Dispenser_i < MAX_SYSTEM_DISPENSERS;Dispenser_i++)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 5084114c6..3a0b25ba8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -40,6 +40,7 @@ extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
 extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
 //extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
+extern uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
 
 #define MAX_CONTROL_SAMPLES 10
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 0680e97a4..30cbd4db3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -168,6 +168,8 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__
 uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 {
     uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+    float screw_speed = 0;
+    float RotationsPerSecond;
 
     if (Counter)
     {
@@ -176,6 +178,15 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
             NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
             REPORT_MSG(ScrewNumberOfSteps, "Head Backing");
             ScrewNumberOfSteps--;
+            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 = dyeingspeed / (InternalWinderCfg.diameter * PI);
+            RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
+            // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
+            ScrewSpeed = screw_speed*RotationsPerSecond;
+
+            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+
         }
         if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
         {
@@ -193,6 +204,9 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 }
 uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
+    uint32_t Steps;
+
+
     if (BusyfirstCall)
     {
         BusyfirstCall = false;
@@ -202,6 +216,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     if (BusyFlag == NOTBUSY)
     {
         CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
+        Steps = ScrewNumberOfSteps;
         if (ScrewCurrentDirection == false)
         {
             ScrewCurrentDirection = true;
@@ -210,7 +225,18 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         {
             ScrewCurrentDirection = false;
         }
-        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
+        if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
+        {
+            ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
+            Steps += InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+            REPORT_MSG(LIMIT, "Winder at  right limit");
+        }
+        else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT)
+        {
+            ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
+            REPORT_MSG(LIMIT, "Winder at left limit");
+        }
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
     }
 /*
  * calculate new ScrewSpeed and call     MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
@@ -234,7 +260,6 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     float screw_speed = 0;
     float RotationsPerSecond;
 
-    int process_speed = dyeingspeed;
     if (dyeingspeed == 0)
     {
         LOG_ERROR (dyeingspeed," job speed zero");
@@ -251,7 +276,8 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         //     * 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 * PI);
+        RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI);
+        RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
         // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
         ScrewSpeed = screw_speed*RotationsPerSecond;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index b6c1fea57..75d54ef02 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -360,7 +360,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 if (ReadBreakSensor()==ERROR)
                 {
                     //LOG_ERROR(index, "ReadBreakSensor Error");
-                    JobEndReason = JOB_DANCER_FAIL;
+                    JobEndReason = JOB_THREAD_BREAK;
                     //SendJobProgress(0.0,0,false, "ReadBreakSensor Error");
                    EndState(CurrentJob,"ReadBreakSensor Error" );
                 }
@@ -372,7 +372,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
             JobAbortedByUser = true;
-            JobEndReason = JOB_THREAD_BREAK;
+            JobEndReason = JOB_DANCER_FAIL;
             EndState(CurrentJob,Message );
         }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
@@ -404,7 +404,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 KeepReadValue = TranslatedReadValue;
             }
         }*/
-        if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
@@ -533,6 +533,7 @@ void SetOriginMotorSpeed(float process_speed)
                 / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius);
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
+        CurrentControlledSpeed[Motor_i] = (int) motor_speed;
     }
 }