1. Idle task - test real time usage

2. improve and fix heaters test
3. motor/dancer test preparations

12 files changed, 180 insertions, 70 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index c242401fd..e0abf6c2b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -24,6 +24,7 @@
 #include "control.h"
 
 #include "drivers/Motors/Motor.h"
+#include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 
 /******************** Definitions  ********************************************/
@@ -49,6 +50,7 @@ uint32_t  MotorSpeed_Data[MOTOR_SPARE1_1] = {0};
 uint32_t  MotorStatus_Data[MOTOR_SPARE1_1] = {0};
 uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
 bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
+uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
@@ -58,7 +60,7 @@ static GateMutex_Handle gateMillisecDB;
 uint32_t                MillisecDatalog[MAX_TANGO_CONTROL_DEVICES];
 uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
 /******************** Functions  ********************************************/
-
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
@@ -154,6 +156,9 @@ uint32_t MillisecLoop(uint32_t tick)
     MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
     MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);
     //gather Dancer data from FPGA
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+    //Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
+    //Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     //gather data from FPGA
     if (Ten_msTick)
     {
@@ -240,3 +245,29 @@ uint32_t getADCData(int DeviceId)
     return ADC_Data[DeviceId];
 }
 
+/********************************************************************
+*
+*    Name        : GTIME_Delta_Time_Pass
+*
+*    Parameters  : start_time.
+*
+*    Return      : time pass from start time
+*
+*    Description :
+*
+*********************************************************************/
+
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
+{
+    uint32_t Time_Pass;
+  #define   MAX_COUNTER 0x3FFF  //14 bits
+
+
+  if (Current_Read < Previous_Read)
+    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
+  else
+    Time_Pass = Current_Read - Previous_Read;
+
+  return (Time_Pass);
+}
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index c0f4c3a65..bfbdec3bb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -110,6 +110,7 @@ void ControlInit(void)
     int Device_i;
     Error_Block eb;
 
+    //Mailbox_Params_init(&ControlMsgQ);
     ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
 
     ControlRestart = false;
@@ -148,11 +149,14 @@ void ControlStop(void)
 
 void ControlStart(void)
 {
-    ControlRestart = true;
-    ROM_TimerLoadSet(Control_timerBase, TIMER_A,120000+(ControlPhaseDelay*120)/*one millisecond*/);
-    TimerEnable(Control_timerBase, TIMER_A);
+    if (ControlRestart == false)
+    {
+        ControlRestart = true;
+        ROM_TimerLoadSet(Control_timerBase, TIMER_A,120000+(ControlPhaseDelay*120)/*one millisecond*/);
+        TimerEnable(Control_timerBase, TIMER_A);
 
-    ADCAcquireStart(0,1);
+        ADCAcquireStart(0,1);
+    }
 }
 
 /************************************************************************************************************************************************
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
index fb9486c66..ba23ad972 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
@@ -62,6 +62,6 @@ extern uint32_t DeviceId2Heater[MAX_HEATERS_NUM];
 extern uint32_t  Heater_timerBase;
 
 uint32_t HeaterRecalculateSharedHeatersParams(uint32_t deviceId, uint32_t new_outputproportionalpowerlimit);
-void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage);
+void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message);
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index 19953ade4..70522b016 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -107,7 +107,7 @@ void HeatingTestRequest(MessageContainer* requestContainer)
 
     if (status==OK)
         {
-            HeatersControlStart();
+            //HeatersControlStart();
             ControlStart();
         }
     StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT;
@@ -163,11 +163,11 @@ void HeatingTestPollRequest(MessageContainer* requestContainer)
     free(request);
 }
 
-void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage)
+void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message)
 {
     MessageContainer responseContainer;
-//    uint8_t* container_buffer;
-    uint8_t container_buffer[50];
+    uint8_t* container_buffer;
+//    uint8_t container_buffer[50];
 
     if (stubToken[0] == 0)
         return;
@@ -202,11 +202,11 @@ response.has_heater2percentage = true;
 response.heater2percentage = Heater2Percentage;
 response.has_zone2temp = true;
 response.zone2temp = temperature2;
-
+response.infomessage = Message;
 responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, stubToken, last, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size);
 //setContainerContinuous
 responseContainer.continuous = true;
-//container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+container_buffer = malloc(message_container__get_packed_size(&responseContainer));
 
     if (status)
     {
@@ -217,8 +217,8 @@ responseContainer.continuous = true;
     free(responseContainer.data.data);
     //USBCDCD_sendData(container_buffer, container_size,10);
     SendChars(container_buffer, container_size);
-//    free(container_buffer);
-    stubToken[0] = 0;
+    free(container_buffer);
+//    stubToken[0] = 0;
 }
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index c13d6b5b3..82a3d362c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -98,6 +98,7 @@ void HeatersStartControlTimer (void)
         return;
     TimerActivated = true;
     ROM_TimerConfigure(Heater_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    ROM_TimerLoadSet(Heater_timerBase, TIMER_A,OutputProportionalSingleStep/*twelve millisecond???*/);
     ROM_IntEnable(INT_TIMER2A);
     ROM_TimerIntEnable(Heater_timerBase, TIMER_TIMA_TIMEOUT);
     TimerEnable(Heater_timerBase, TIMER_A);
@@ -214,7 +215,7 @@ uint32_t HeaterControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
         Temperature[deviceID] =  ReadValue;
     HeatingTestSendResonse(0, false,GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w),GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1),
                            Temperature[0],Temperature[1],
-                           HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError);
+                           HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"Standard");
 
     // check if the read value is within the proportional band
     if (InitialHeating)
@@ -243,7 +244,11 @@ uint32_t HeaterControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
         else
         {
             InitialHeating = false;
+            HeatersControlStart();
             PrepareReady(Module_Heaters,OK);
+            HeatingTestSendResonse(0, false,GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w),GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1),
+                               Temperature[0],Temperature[1],
+                               HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"End Initial Heating");
 
         }
     }
@@ -308,13 +313,21 @@ void EightMilliSecondHeatersInterrupt(UArg arg0)
 
 uint32_t HeatersControlLoop(uint32_t tick)
 {
-    char str[100];
-    uint8_t len = 0;
+    //char str[100];
+    //uint8_t len = 0;
 
     /*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]);
 */
+    static bool first = true;
+    if (first == true)
+    {
+        first = false;
+        HeatingTestSendResonse(0, false,GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w),GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1),
+                           0,0, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"First Control");
+
+    }
     if (TimeSliceAllocation[SliceCounter] == HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w)
     {
         //If HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w should be active
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
index 0be3d5fa9..96a2dbc3a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
@@ -27,6 +27,7 @@
 #include "Drivers/USB_Communication/usb_serial_buffer.h"
 #include "StateMachines/Printing/PrintingSTM.h"
 #include "Modules/General/process.h"
+#include "Modules/Thread/Thread_ex.h"
 #include "Common/report/report.h"
 
 
@@ -156,6 +157,9 @@ void receive_callback(char* buffer, size_t length)
          case MESSAGE_TYPE__JobRequest:
              JobRequestFunc(requestContainer);
              break;
+         case MESSAGE_TYPE__UploadHardwareConfigurationRequest:
+             HWConfigurationFunc(requestContainer);
+             break;
          case MESSAGE_TYPE__UploadProcessParametersRequest:
              ProcessRequestFunc(requestContainer);
              break;
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 c16e33a99..0c9a11723 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -14,6 +14,10 @@
 #include <PMR/Stubs/StubMotorMovRequest.pb-c.h>
 #include <PMR/Stubs/StubMotorMovResponse.pb-c.h>
 
+
+#include "PMR/Hardware/HardwareMotor.pb-c.h"
+#include "PMR/Hardware/HardwareMotorType.pb-c.h"
+
 #include <stdbool.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -37,6 +41,7 @@
 #include "drivers/FPGA/Moters_Driver/L6470.h"
 
 #include "driverlib/ssi.h"
+#include "Modules/Thread/Thread_ex.h"
 
 
 extern unsigned long Run_Value ;
@@ -85,6 +90,20 @@ void Stub_MotorInitRequest(MessageContainer* requestContainer)
     //temp_init_spi2();
 
 
+    HardwareMotor MotorConfig;
+    MotorConfig.hardwaremotortype = request->motor_id;
+    MotorConfig.minfrequency = 0;
+    MotorConfig.maxfrequency = request->max_speed;
+    MotorConfig.kp = 5000;
+    MotorConfig.ki = 100;
+    MotorConfig.kd = 0;
+    MotorConfig.minmicrostep = request->micro_steps;
+    MotorConfig.maxmicrostep = request->micro_steps;
+    MotorConfig.ratiotodryerspeed = 1;
+    MotorsConfigMessage(&MotorConfig);
+
+
+
     StubMotorInitResponse response =   STUB_MOTOR_INIT_RESPONSE__INIT;
 
    // status_response(status,&response.status, &response.statusword ,&response.has_statusword);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index fca12d50e..913b38df8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -69,6 +69,7 @@ extern InternalWinderConfigStruc InternalWinderCfg;
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
 uint32_t MotorsConfigMessage(HardwareMotor * request);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
+uint32_t ThreadInitialTestStub(HardwareMotor * request);
 
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 9474ee44d..eacdd2ed9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -53,7 +53,12 @@ uint32_t Winder_PrepareStage2(void)
         return status;
 
 }
-uint32_t Winder_Print(void *JobDetails)
+
+uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
+{
+    return PreSegmentReady(Module_Winder,OK);
+}
+uint32_t Winder_Presegment(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
     int process_speed = JobTicket->processparameters->dyeingspeed;
@@ -87,6 +92,7 @@ uint32_t Winder_Print(void *JobDetails)
 //     * 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
 
     //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back.
+     MotorSetSpeedWithCallback (MOTOR_SCREW, screw_speed, MotorsCfg[SCREW_MOTOR].minmicrostep,WinderPresegmentReady);
     //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
 
     return OK;
@@ -94,7 +100,7 @@ uint32_t Winder_Print(void *JobDetails)
 uint32_t Winder_End(void)
 {
     //stop screw
-    StopMotor (MOTOR_SCREW,1);
+    return StopMotor (MOTOR_SCREW,1);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index e69de29bb..9b371ae07 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -0,0 +1,11 @@
+
+#ifndef MODULES_THREAD_THREAD_EX_H_
+#define MODULES_THREAD_THREAD_EX_H_
+uint32_t ThreadPreSegmentState(void *JobDetails);
+uint32_t ThreadSegmentState(void *JobDetails);
+uint32_t ThreadEndState(void *JobDetails);
+uint32_t ThreadInitialTestStub();
+
+
+
+#endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index db32b7ba7..5b0f034f4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -12,9 +12,9 @@
 
 #include "thread.h"
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
-MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM];
-InternalWinderConfigStruc InternalWinderCfg;
-DancerConfigStruc DancersCfg[MAX_SYSTEM_DANCERS];
+MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM]={0};
+InternalWinderConfigStruc InternalWinderCfg = {0};
+DancerConfigStruc DancersCfg[MAX_SYSTEM_DANCERS] = {0};
 
 
 
@@ -47,7 +47,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     int Motor_i;
 
     Motor_i = request->hardwaremotortype;
-    if ((Motor_i)&&(Motor_i< MAX_THREAD_MOTORS_NUM))
+    if (Motor_i< MAX_THREAD_MOTORS_NUM)
     {
         MotorsCfg[Motor_i].id               = request->hardwaremotortype;
         MotorsCfg[Motor_i].minfreq          = request->minfrequency;
@@ -63,6 +63,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
         MotorsCfg[Motor_i].kd               = request->kd;
         MotorsCfg[Motor_i].changeslope      = request->changeslope;
         MotorsCfg[Motor_i].hightimeoutusec  = request->highlengthmicrosecond;
+        ThreadInitialTestStub(request);
         return status;
     }
     else return Motor_i;
@@ -74,7 +75,7 @@ uint32_t DancerConfigMessage(HardwareDancer * request)
     int Dancer_i;
 
     Dancer_i = request->hardwaredancertype;
-    if ((Dancer_i)&&(Dancer_i<MAX_SYSTEM_DANCERS ))
+    if (Dancer_i<MAX_SYSTEM_DANCERS )
     {
         DancersCfg[Dancer_i].id = Dancer_i;
         DancersCfg[Dancer_i].fixorgradual = request->gradual;
@@ -90,8 +91,8 @@ uint32_t DancerConfigMessage(HardwareDancer * request)
 
 uint32_t thread_init(void)
 {
-    memset (MotorsCfg,0,sizeof(MotorsCfg));
-    memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc));
+    //memset (MotorsCfg,0,sizeof(MotorsCfg));
+    //memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc));
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index e850ad776..366ce827b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -13,6 +13,8 @@
 #include "PMR/Printing/JobSegment.pb-c.h"
 #include "PMR/Printing/JobTicket.pb-c.h"
 
+#include "StateMachines/Printing/PrintingSTM.h"
+
 #include "drivers/Motors/Motor.h"
 #include "drivers/Danser_SSI/ssi_comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
@@ -21,14 +23,6 @@
 //the state machine operation is used to operate in runtime correct profile flow execution
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
-typedef enum
-{
-    NextState = 0,
-    Repeat,
-    Inter,
-    Home,
-    Stop
-} ReturnCode;
 
 
 uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
@@ -49,6 +43,7 @@ typedef struct
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 ////////////////////////Slow Motor State////////////////////////////////////
+uint32_t ThreadPreSegmentState(void *JobDetails);
 
 ////////////////////////////////////////////////////////////////////////////
 
@@ -57,7 +52,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (DeviceId2Motor[i] == deviceID)
+        if (ThreadMotorIdToDancerId[i] == deviceID)
         {
             index = i;
             break;
@@ -90,8 +85,9 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 {
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
+    uint32_t TranslatedReadValue = ReadValue - 8740;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (DeviceId2Motor[i] == deviceID)
+        if (ThreadMotorIdToDancerId[i] == deviceID)
         {
             index = i;
             break;
@@ -101,9 +97,9 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
         LOG_ERROR (deviceID, "No motor  for device");
         return 0xFFFFFFFF;
     }
-    if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
+    if(MotorControlConfig[index].m_isEnabled )
     {
-        MotorControlConfig[index].m_mesuredParam = ReadValue;
+        MotorControlConfig[index].m_mesuredParam = TranslatedReadValue;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
@@ -131,71 +127,94 @@ uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t
     return (rand() % (103 + 1 + 103) - 103);
 }
 #endif
+uint32_t ThreadInitialTestStub(HardwareMotor * request)
+{
+
+
+    //MotorsConfigMessage(request);
+     ThreadPrepareState(request);
+     ThreadPreSegmentState(request);
+    return OK;
+}
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
-    int Motor_i;
+    int Motor_i, HW_Motor_Id;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
-        MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[Motor_i].maxfreq;
-        MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[Motor_i].minfreq;
-        MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[Motor_i].kd;
-        MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[Motor_i].kp;
-        MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[Motor_i].ki;
-        MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
-        MotorControlConfig[Motor_i].m_calculatedError = 0;
-        MotorControlConfig[Motor_i].m_integral = 0;
-        MotorControlConfig[Motor_i].m_isEnabled = true;
-        MotorControlConfig[Motor_i].m_isReady = true;
-        MotorControlConfig[Motor_i].m_mesuredParam = 0;
-        MotorControlConfig[Motor_i].m_preError = 0;
-        MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
-#ifdef DEBUG_TEST_FUNCTIONS
-        if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-            AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
-        else
-            AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Debug_Get_Dancer_Read,ThreadMotorIdToDancerId[Motor_i],0);
-#else
-        if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-            continue;
-            //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
-        else
-            AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],0);
-#endif
+        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
+            MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq;
+            MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq;
+            MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd;
+            MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp;
+            MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].ki;
+            MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
+            MotorControlConfig[Motor_i].m_calculatedError = 0;
+            MotorControlConfig[Motor_i].m_integral = 0;
+            MotorControlConfig[Motor_i].m_isEnabled = true;
+            MotorControlConfig[Motor_i].m_isReady = true;
+            MotorControlConfig[Motor_i].m_mesuredParam = 0;
+            MotorControlConfig[Motor_i].m_preError = 0;
+            MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
+    #ifdef DEBUG_TEST_FUNCTIONS
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
+    //            continue;
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+    #else
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                continue;
+                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i);
+            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+    #endif
     }
     Winder_Prepare();
     //set 3 dancers to the profile positions
+    ControlStart();
     return OK;
 }
 
 //********************************************************************************************************************
-ReturnCode PreSegmentState(void *JobDetails)
+uint32_t ThreadPreSegmentState(void *JobDetails)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-    int process_speed = JobTicket->processparameters->dyeingspeed;
-    int dryer_speed = process_speed * MotorsCfg[DRYER_MOTOR].ratio2dryerspeed; //set dryer_speed_translation_here
+//    int process_speed = JobTicket->processparameters->dyeingspeed;
+    int process_speed = 500;
+
+    int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here
 
     // set the new speed in the dryer motor to the speed of the new segment
-    MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed,  MotorsCfg[DRYER_MOTOR].minmicrostep);
+    MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed,  MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep);
 
     // 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_Thread,OK);
+
     return OK;
 }
 
 //********************************************************************************************************************
- ReturnCode SegmentState(void *JobDetails)
+ uint32_t ThreadSegmentState(void *JobDetails)
 {
     return OK;
 }
 
 //********************************************************************************************************************
- ReturnCode EndState(void *JobDetails)
+ uint32_t ThreadEndState(void *JobDetails)
 {
+     int Motor_i;
+     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
+     {
+        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+     }
+
     return OK;
 }
 
@@ -215,3 +234,4 @@ void ThreadStopPrinting(void)
 {
     //PrintingIterate();
 }
+