1. Idle task - test real time usage

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

23 files changed, 700 insertions, 194 deletions

diff --git a/Software/Embedded_SW/Embedded/.cproject b/Software/Embedded_SW/Embedded/.cproject
index b1881f6c1..5c1e4b40e 100644
--- a/Software/Embedded_SW/Embedded/.cproject
+++ b/Software/Embedded_SW/Embedded/.cproject
@@ -135,7 +135,7 @@
 									<listOptionValue builtIn="false" value="&quot;${TI_PRODUCTS_DIR}/TivaWare_C_Series-2.1.2.111&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;C:\ti\xdctools_3_32_00_06_core\include&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${COM_TI_RTSC_TIRTOSTIVAC_INSTALL_DIR}/products/TivaWare_C_Series-2.1.1.71b&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${COM_TI_RTSC_TIRTOSTIVAC_INSTALL_DIR}/products/bios_6_45_02_31/packages/ti/sysbios/posix&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${COM_TI_RTSC_TIRTOSTIVAC_INSTALL_DIR}/products/bios_6_45_01_29/packages/ti/sysbios/posix&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${CG_TOOL_ROOT}/include&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Common}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}}/Communication/PMR&quot;"/>
diff --git a/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c
new file mode 100644
index 000000000..f2caf3bcb
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.c
@@ -0,0 +1,375 @@
+/**********************************************************
+
+   File name      :Idle.c
+   Title          :Idle task for cpu load measuring
+   Project        :Hi-Mux 100
+   Subsystem      :IDLE
+   Date created   :12 November 1996
+   Revision       :Rev 0.0
+   Author         :Shay Mamon & Sharon Melamed
+   History        :
+
+   Description    :This module contains two tasks that works in the system
+                   Idle time.
+                    1) In initialization the tasks comute referance
+                       load when the system is unloaded.
+                    2) Computing the current load according to the referance load.
+
+************************************************************/
+
+/*******************  Include Files ************************/
+
+#include "include.h"
+#include <ti/sysbios/knl/Task.h>
+#include <xdc/runtime/Error.h>
+#include <xdc/runtime/System.h>
+
+#define MAX_PRIORITY 10
+#define SECOND_MAX_PRIORITY 9
+#define MIN_PRIORITY 0
+#define SECOND_MIN_PRIORITY 1
+#define TICKS_IN_SECOND 1000
+
+/**********************************************************/
+
+/*#define DEBUG*/
+
+/*******************  Function Prototypes *****************/
+
+
+void    idle_task (UArg arg0, UArg arg1);
+void    IDLE_statistics_task(UArg arg0, UArg arg1);
+static void    calculate_system_load (int current_load);
+
+/**********************************************************/
+
+/*******************   Globals  ***************************/
+
+static uint32_t idle_counter;
+static long next_interval;  /*by ticks*/
+static long idle_task_id;
+static long IDLE_statistic_task_id;
+static bool first_time_after_init;
+static int system_load;
+static int load;
+static int max_system_load;
+static int max_load;
+static uint32_t idle_load_table[101];
+static uint32_t idle_sequence_table[11];
+static uint32_t idle_max_sequence_table[11];
+/**********************************************************/
+Task_Handle IdleTaskHandle;
+Task_Handle IdleStTaskHandle;
+char IdleTaskName[11] = "timeKeeper";
+char IdleStTaskName[11] = "timeManager";
+/**********************************************************
+
+  Name            :  IDLE_package_init
+
+  Function        :  Initilize global vars
+
+  Parameters      :  None
+
+  Return value    :  None
+
+  Note            :  None
+
+  Description     :  Initilize global vars, sets the new interval and reset the samples number.
+
+***********************************************************/
+extern void IDLE_TASK_package_init(void)
+{
+    Task_Params taskParams;
+    Error_Block eb;
+
+    /* Make sure Error_Block is initialized */
+    Error_init(&eb);
+    Task_Params_init(&taskParams);
+
+   system_load=0;
+   max_load = 0;
+   max_system_load = 0;
+   first_time_after_init=TRUE;
+   next_interval = 1000 /*10 mSec*/ /*TICKS_IN_SECOND*/;
+   idle_counter=0;
+   taskParams.instance->name = IdleTaskName;
+   taskParams.priority = SECOND_MIN_PRIORITY;
+   IdleTaskHandle = Task_create((Task_FuncPtr)idle_task, &taskParams, &eb);
+   Error_init(&eb);
+   Task_Params_init(&taskParams);
+   taskParams.instance->name = IdleStTaskName;
+   taskParams.priority = MAX_PRIORITY;
+   IdleStTaskHandle = Task_create((Task_FuncPtr)IDLE_statistics_task, &taskParams, &eb);
+   //if (IdleTaskHandle == NULL) {
+   //    System_printf("netOpenHook: Failed to create tcpHandler Task\n");
+   //}
+}
+
+
+
+/**********************************************************
+
+  Name            :  idle_task
+
+  Function        :  Task that runs on idle time
+
+  Parameters      :  NONE
+
+  Return value    :  None
+
+  Note            :  None
+
+  Description     :  This task contain one loop and each pass
+                     on the loop inc counter by 1
+
+***********************************************************/
+ void idle_task (UArg arg0, UArg arg1)
+{
+   while(1)  idle_counter++;
+}
+/**********************************************************
+
+  Name            :  IDLE_statistic_task
+
+  Function        :  Calculate the presentege of idle time
+
+  Parameters      :  NONE
+
+  Return value    :  None
+
+  Note            :  None
+
+  Description     :  This task rize every x (according to user define) seconds and calculate
+                     the presentege of idle time by comparing the idle conter
+                     when the system is load and when the system is unload.
+
+***********************************************************/
+ void IDLE_statistics_task(UArg arg0, UArg arg1)
+{
+uint32_t current_load;
+uint32_t prev_load = 0;
+uint32_t old_idle_counter=0;
+uint32_t delta_idle_counter = 0;
+long time_interval;  /*by ticks*/
+uint32_t unload_max_count=0;
+
+   //taskPrioritySet (idle_task_id ,SECOND_MAX_PRIORITY);
+   Task_setPri(IdleTaskHandle, SECOND_MAX_PRIORITY);
+   Task_sleep(TICKS_IN_SECOND);                  /* stablize the idle_task */
+   old_idle_counter=idle_counter;
+   Task_sleep(TICKS_IN_SECOND);
+   //taskPrioritySet (idle_task_id ,MIN_PRIORITY);
+   Task_setPri(IdleTaskHandle, SECOND_MIN_PRIORITY);
+   unload_max_count = idle_counter - old_idle_counter + ULONG_MAX;        /* compute circular counter */
+   old_idle_counter=idle_counter;
+
+   #ifdef DEBUG
+   printf("unload max count = %lu \n",unload_max_count);
+   #endif
+
+   time_interval = next_interval;
+   while(1)
+   {
+       Task_sleep(time_interval);
+      delta_idle_counter = abs((uint32_t)(idle_counter - old_idle_counter + ULONG_MAX));   /* compute circular counter */
+
+      current_load = ((100 - (((delta_idle_counter/time_interval)*TICKS_IN_SECOND)/ (unload_max_count/100)))%101);
+      if(prev_load == (current_load / 10))
+      {
+        idle_sequence_table[prev_load]++;
+      }
+      else
+      {
+        if(idle_max_sequence_table[prev_load] < idle_sequence_table[prev_load])
+        {
+           idle_max_sequence_table[prev_load] = idle_sequence_table[prev_load];
+        }
+        idle_sequence_table[prev_load] = 0;
+        idle_sequence_table[current_load/10] = 1;
+      }
+      calculate_system_load(current_load);
+      old_idle_counter = idle_counter;
+      time_interval = next_interval;
+      load=current_load;
+      prev_load = current_load / 10;
+      idle_load_table[load]++;
+      if(load > max_load) max_load = load;
+
+      #ifdef DEBUG
+      printf("c_ld=%d\n",load);
+      #endif
+   }
+}
+
+/**********************************************************
+
+  Name            :  calculate_system_load
+
+  Function        :  calculate the average system load
+
+  Parameters      :  current load (int)
+
+  Return value    :  None
+
+  Note            :  the average resets when the number_of_load_samples = 0
+
+  Description     :  The function calculate and change the system load variable
+
+***********************************************************/
+
+static void calculate_system_load (int current_load)
+{
+static int sum_of_previous_load=0;
+static uint32_t number_of_load_samples;
+   if (first_time_after_init)
+   {
+      first_time_after_init = FALSE;
+      system_load = current_load;
+      max_system_load = system_load;
+      max_load = load;
+      number_of_load_samples = 1;
+      sum_of_previous_load = current_load;
+   }
+   else
+   {
+      sum_of_previous_load = sum_of_previous_load + current_load;
+      number_of_load_samples = number_of_load_samples + 1;
+      system_load = ( sum_of_previous_load / number_of_load_samples);
+      if(system_load > max_system_load) max_system_load = system_load;
+   }
+}
+
+/***********************************************************
+
+  Name            :  IDLE_change_parameters
+
+  Function        :  starts new statistic with new time interval
+
+  Parameters      :  new time interval
+
+  Return value    :  None
+
+  Note            :  None
+
+  Description     :  sets the new interval and reset the samples number.
+
+***********************************************************/
+extern void IDLE_change_parameters(uint32_t new_interval)
+{
+   next_interval = new_interval;
+   memset(idle_load_table,0,sizeof(idle_load_table));
+   memset(idle_max_sequence_table,0,sizeof(idle_max_sequence_table));
+   memset(idle_sequence_table,0,sizeof(idle_sequence_table));
+
+   first_time_after_init=TRUE;                  /* reset the avarage calculation */
+}
+
+/**********************************************************
+
+  Name            :  IDEL_get_load
+
+  Function        :  returns the system load
+
+  Parameters      :  None
+
+  Return value    :  system load
+
+  Note            :  External function
+
+  Description     :  Trivial
+***********************************************************/
+
+extern int IDLE_TASK_get_load(void)
+{
+   return system_load;
+}
+ /***********************************************************/
+ extern int IDLE_TASK_get_current_load(void)
+ {
+  return load;
+ }
+
+/***********************************************************/
+ /*export MN_uint32_t mn_get_load(char *pParams)
+ {
+    int i;
+
+    MN_printf("\n\r\n\rSystem load\n\r-------------");
+    for(i=0;i<10;i++)
+    MN_printf("\n\r %3d-%-3d Max Continuous Load is %4d intervals = %5d mSec",
+             i*10, (i * 10 + 9),idle_max_sequence_table[i], 10*next_interval*idle_max_sequence_table[i]);
+    MN_printf("\n\r  %3d    Max Continuous Load is %4d intervals = %5d mSec",
+              (i * 10),idle_max_sequence_table[i], 10*next_interval*idle_max_sequence_table[i]);
+    MN_printf("\n\r\n\rSystem avarege load=%d , max avarege load = %d",system_load, max_system_load);
+
+    return(RET_OK);
+ }*/
+/***********************************************************/
+ /*export MN_uint32_t mn_get_curent_load(char *pParams)
+ {
+    MN_printf("\n\rload=%d,max load=%d", load, max_load);
+    return(RET_OK);
+ }
+*/
+/***********************************************************/
+ /*export MN_uint32_t mn_get_load_table(char *pParams)
+ {
+    int i;
+    uint32_t interval_count;
+    uint32_t avarege;
+    int total_avarege;
+    interval_count = 0;
+    total_avarege = 0;
+    MN_printf("\n\ridle_load_table\n\r---------------\n\r");
+    for ( i=0;i<=100;i++)
+      {
+        interval_count+=idle_load_table[i];
+      }
+    for ( i=0;i<=100;i++)
+      {
+        avarege = (100*idle_load_table[i])/interval_count;
+        MN_printf("|%3d-%-7d(%-2d)", i, idle_load_table[i], avarege);
+        total_avarege+=(idle_load_table[i]*i);
+        taskDelay(2);
+      }
+      total_avarege = total_avarege/interval_count;
+    MN_printf("\n\rTotal avarege = %ld",total_avarege);
+
+    return(RET_OK);
+ }
+*/
+/***********************************************************/
+ /*export MN_uint32_t mn_change_interval(char *pParams)
+ {
+
+  MN_uint32_t          rc;
+  MN_WORD             new_interval;
+  MN_WORD             index;
+  index = 0;
+  rc = MN_ParseWord(pParams ,&index ,&new_interval);
+  if(rc == RET_OK)
+  {
+     IDLE_change_parameters((long)new_interval);
+  }
+  else
+  {
+    MN_printf("\r\nrc = %d Illegal parameter" ,rc);
+  }
+  return(RET_OK);
+ }*/
+/***********************************************************/
+/*extern uint32_t IDLE_TASK_monitor_init(void)
+{
+    MN_uint32_t rc=RET_OK;
+    MN_WORD idleMenuEntry;
+
+    rc|=MN_BindMenu("Idle_task menu","Monitor system load",&idleMenuEntry);
+
+    rc|=MN_BindCommand("GetCurLoad",idleMenuEntry,mn_get_curent_load,"GetCurLoad - Get Curent System CPU load\r\nGetCurLoad <cr>");
+    rc|=MN_BindCommand("GetLoad",idleMenuEntry,mn_get_load,"GetLoad - Get averege system load (since last time you changed interval or since initiation)\r\nGetLoad <cr>");
+    rc|=MN_BindCommand("IdleTbl",idleMenuEntry,mn_get_load_table,"IdleTbl - Get load table(since last time you changed interval or since initiation)\r\nGetLoad <cr>");
+    rc|=MN_BindCommand("ChangeInt",idleMenuEntry,mn_change_interval,"ChangeInt - change interval of system load chack\r\nChangeInterval <New Interval in Tics (~100 in second)>");
+
+ return(RET_OK);
+}
+*/
diff --git a/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.h b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.h
new file mode 100644
index 000000000..090b7ef8d
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Common/Utilities/idle_task.h
@@ -0,0 +1,20 @@
+/*
+ * idle_task.h
+ *
+ *  Created on: 2 april 2018
+ *      Author: shlomo
+ */
+
+#ifndef COMMON_UTILITIES_IDLE_TASK_H_
+#define COMMON_UTILITIES_IDLE_TASK_H_
+
+extern void    IDLE_TASK_package_init(void);
+extern void    IDLE_change_parameters(uint32_t new_interval);  /*time interval by ticks*/
+extern int     IDLE_TASK_get_load(void);
+extern int     IDLE_TASK_get_current_load(void);
+extern uint32_t IDLE_TASK_monitor_init(void);
+
+
+
+
+#endif /* COMMON_UTILITIES_IDLE_TASK_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
index 62e5737e5..727e3ecbe 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
@@ -159,7 +159,8 @@ uint32_t Write_Dummy_Byte()
 }
 uint32_t Control_Read_Dancer_Position(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2)
 {
-    return Read_Dancer_Position(DancerId);
+    //return Read_Dancer_Position(DancerId);
+    return SSI_enc.Position;
 }
 uint32_t Read_Dancer_Position (int DancerId)
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index e97d49094..b1c24da68 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -104,7 +104,7 @@ bool     MotorControlGetnBusyState(uint32_t _motorId, uint32_t parameter)
  * Stop Command
  * 1 - Hard_Stop/ 2 - Soft_Stop/ 3 - Hard_Hiz/ 4 - Soft_Hiz
  */
-uint32_t StopMotor(TimerMotors_t _motorId, int StopType)
+uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType)
 {
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index b7f18acf1..d06c9d4db 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -126,7 +126,14 @@ bool     MotorGetnBusyState(TimerMotors_t _motorId);
  * Stop Command
  * 1 - Hard_Stop/ 2 - Soft_Stop/ 3 - Hard_Hiz/ 4 - Soft_Hiz
  */
-uint32_t StopMotor(TimerMotors_t _motorId, int StopType);
+typedef enum
+{
+    Hard_Stop,
+    Soft_Stop,
+    Hard_Hiz,
+    Soft_Hiz,
+}STOP_TYPE_ENUM;
+uint32_t StopMotor(TimerMotors_t _motorId, STOP_TYPE_ENUM StopType);
 
 uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps);
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c b/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
index 91ab3d145..005137f73 100644
--- a/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
+++ b/Software/Embedded_SW/Embedded/Drivers/USB_Communication/USBCDCD.c
@@ -7,6 +7,7 @@
  ************************************************************************************/
 
 /* XDCtools Header files */
+#include "include.h"
 #include <xdc/std.h>
 #include <xdc/runtime/Error.h>
 #include <xdc/runtime/System.h>
@@ -263,6 +264,11 @@ static tLineCoding g_sLineCoding = {
     8                           /* 8 Bits of data. */
 };
 
+Mailbox_Handle          CommunicationRxMsgQ = NULL;
+typedef struct CommRxMessage{
+    uint16_t messageId;
+    uint16_t msgSize;
+}CommRxMessageStruc;
 
 uint32_t SendChars(char* buffer,size_t length)
 {
@@ -555,8 +561,9 @@ void handleRx(void)
     uint8_t size[4];
     int size_bar = 0;
     checkpoints[2]++;
+    CommRxMessageStruc Message;
 
-    if (expected_message_size == 0)
+if (expected_message_size == 0)
     {
         do
         {
@@ -587,6 +594,10 @@ void handleRx(void)
         if (current_message_size == expected_message_size)
         {
             g_RxCount += current_message_size;
+            Message.messageId = 1;
+            Message.msgSize = g_RxCount;
+            if (CommunicationRxMsgQ != NULL)
+                /*retcode =*/ Mailbox_post(CommunicationRxMsgQ , &Message, BIOS_NO_WAIT);
             expected_message_size = 0;
             current_message_size = 0;
             break;
@@ -753,6 +764,7 @@ void USBCDCD_init(void)
 {
     Semaphore_Params semParams;
     Error_Block eb;
+    CommunicationRxMsgQ = Mailbox_create(sizeof(CommRxMessageStruc), 5, NULL,NULL);
 
     Error_init(&eb);
  
@@ -973,6 +985,7 @@ void communicationTask(UArg arg0, UArg arg1)
 {
 
     uint32_t ui32RxCount;
+    CommRxMessageStruc Message;
 
     ui32RxCount = 0;
     g_RxCount = 0;
@@ -983,23 +996,25 @@ void communicationTask(UArg arg0, UArg arg1)
 //    Semaphore_pend(initConnectionSem, BIOS_WAIT_FOREVER);
     cpt[15]++;
 
-    // Loop forever receiving commands
-    while(true)
+    while(1)
     {
-        if(ui32RxCount != g_RxCount)
+        Mailbox_pend(CommunicationRxMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
         {
-            ui32RxCount = g_RxCount;
-            checkpoints[17]++;
+            case 1:
+                ui32RxCount += Message.msgSize;
+                if (callback != NULL)
+                {
+                    callback(inBuffer.buffer,inBuffer.used);
+                }
 
-            if (callback != NULL)
-            {
-                callback(inBuffer.buffer,inBuffer.used);
-            }
-
-            freeArray(&inBuffer);
-            initArray(&inBuffer, 1);
+                freeArray(&inBuffer);
+                initArray(&inBuffer, 1);
+                break;
+            default:
+                break;
         }
-
     }
+
 }
 
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index fc3189793..bebf71f17 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -16,6 +16,7 @@
 #include "PMR/common/MessageContainer.pb-c.h"
 #include "Common/Sys_PinOut_Config/Pin.h"
 #include "Common/Sys_PinOut_Config/Pin_config.h"
+#include "Common/utilities/idle_task.h"
 #include "Drivers/ADC_Sampling/ADC.h"
 #include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "Drivers/Peripheral_GPIO/GPIO.h"
@@ -164,6 +165,8 @@ int main(void)
     Init_stubs();
     #endif
 
+    IDLE_TASK_package_init();
+
     BIOS_start();
 
     return (0);
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();
 }
+
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index d22639eb1..b3a8c44ea 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -56,19 +56,6 @@ JobTicket *PreviousJob = (JobTicket *)PreviousJobBuffer;
 
 
 
-typedef enum
-{
-    JobSuccess = 0,
-    JobFail,
-    JobStop
-} ReturnCode;
-typedef enum
-{
-    ModuleIdle = 0,
-    ModuleWaiting,
-    ModuleDone,
-    ModuleFail
-} ModuleStateEnum;
 
 ModuleStateEnum PrepareWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
 
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
index 8c2891905..1da712ab5 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
@@ -23,30 +23,22 @@
 //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;
 
 /********************************************************************************************
 * functions describes motor operation flow and movement state during profile execution
 * used to operate in runtime correct profileflow execution
 *********************************************************************************************/
-static ReturnCode EntryState(void *JobDetails);
-static ReturnCode PrepareState(void *JobDetails);
-static ReturnCode PreSegmentState(void *JobDetails);
-static ReturnCode SegmentState(void *JobDetails);
-static ReturnCode EndState(void *JobDetails);
-static ReturnCode ExitState(void *JobDetails);
+static uint32_t EntryState(void *JobDetails);
+static uint32_t PrepareState(void *JobDetails);
+static uint32_t PreSegmentState(void *JobDetails);
+static uint32_t SegmentState(void *JobDetails);
+static uint32_t EndState(void *JobDetails);
+static uint32_t ExitState(void *JobDetails);
 
 /**********************************************************************
 * the array and enum of PrintingState_t below must be in sync order
 ***********************************************************************/
-static ReturnCode (* state[])(void *JobDetails) = { EntryState, PrepareState, PreSegmentState,  SegmentState, EndState, ExitState};
+static uint32_t (* state[])(void *JobDetails) = { EntryState, PrepareState, PreSegmentState,  SegmentState, EndState, ExitState};
 
 
 typedef enum
@@ -64,7 +56,7 @@ typedef enum
 typedef struct
 {
     PrintingState_t m_sourceState;
-    ReturnCode          m_returnCode;
+    uint32_t          m_returnCode;
     PrintingState_t     m_destinationState;
 } Transition_t;
 
@@ -101,6 +93,8 @@ static Transition_t stateTransitionTable[NUM_OF_TRANSITION] = {};
 ////////////////////////Slow Motor State////////////////////////////////////
 static PrintingState_t gPrintingState;
 ////////////////////////////////////////////////////////////////////////////
+ModuleStateEnum PreSegmentWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
+ModuleStateEnum PrintWaiting[MAX_SYSTEM_MODULES] = {ModuleIdle};
 
 
 //********************************************************************************************************************
@@ -108,107 +102,80 @@ static PrintingState_t gPrintingState;
 *function describes entry point of motor in profile execution - accelerate from stop position
 *function described above used to operate motor operation flow and movement state during profile execution
 *********************************************************************************************************************/
-static ReturnCode EntryState(void *JobDetails)
+static uint32_t EntryState(void *JobDetails)
 {
-    return NextState;
+    return OK;
 }
 
 //********************************************************************************************************************
-static ReturnCode PrepareState(void *JobDetails)
+static uint32_t PrepareState(void *JobDetails)
 {
+   PreSegmentWaiting[Module_Thread] = ModuleWaiting;
+   ThreadPreSegmentState(JobDetails);
+   PreSegmentWaiting[Module_Winder] = ModuleWaiting;
+   Winder_Presegment(JobDetails);
 
-    return NextState;
+   return OK;
 }
-
-//********************************************************************************************************************
-static ReturnCode PreSegmentState(void *JobDetails)
+uint32_t PreSegmentReady(int ModuleId, int result)
 {
+    int i;
+    bool ready = true;
+    uint32_t retcode = 0,status = OK;
+
+    JobMessageStruc Message;
+    PrintMessageStruc *PrtMessage = (PrintMessageStruc *)Message.messageData;
+
+    assert (ModuleId<MAX_SYSTEM_MODULES);
+    assert (result<=ModuleFail);
+
+
+    PreSegmentWaiting[ModuleId] = result;
+    if (result == ModuleFail) status = ERROR;
+    for (i=0;i<MAX_SYSTEM_MODULES ;i++)
+    {
+        if (PreSegmentWaiting[i] == ModuleWaiting)
+            ready = false;
+    }
+    if (ready == false) return OK;
+    else
+    {
+        Message.messageId = PrintMessage;
+        if (status == OK)    PrtMessage->messageId = PreSegmentResultsOk;
+        else PrtMessage->messageId = PreSegmentResultsOk;
+        //memcpy(Message.messageData,JobDetails,MAX_MSG_LEN);
+        Message.msglen = 10;
+        if (JobmsgQ != NULL)
+            retcode = Mailbox_post(JobmsgQ , &Message, BIOS_NO_WAIT);
+    }
+    return 0;
 
-    return NextState;
 }
 
 //********************************************************************************************************************
-static ReturnCode SegmentState(void *JobDetails)
+static uint32_t PreSegmentState(void *JobDetails)
 {
-    return Repeat;
+    ThreadPrepareState(JobDetails);
+    return OK;
 }
 
 //********************************************************************************************************************
-static ReturnCode EndState(void *JobDetails)
+static uint32_t SegmentState(void *JobDetails)
 {
-    return NextState;
+    return OK;
 }
+
 //********************************************************************************************************************
-static ReturnCode ExitState(void *JobDetails)
+static uint32_t EndState(void *JobDetails)
 {
-    return Stop;
+    return OK;
 }
-
-//***********************************************************************************************************************
-//this function is responsible for operating and transitioning between the diffrent motor state executions of the profile
-//the lower managment level
-//***********************************************************************************************************************
-static PrintingState_t LookupTransitions(PrintingState_t state,ReturnCode returnCode)
+//********************************************************************************************************************
+static uint32_t ExitState(void *JobDetails)
 {
-    char str[80];
-    uint8_t len = 0;
-    uint8_t indexInTransitionTable;
-    for (indexInTransitionTable = 0; indexInTransitionTable < NUM_OF_TRANSITION; ++indexInTransitionTable)
-    {
-        if ((stateTransitionTable[indexInTransitionTable].m_sourceState == state) && (stateTransitionTable[indexInTransitionTable].m_returnCode == returnCode))
-        {
-            //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
-            //cb_push_back (str, len);
-
-            //in normal execution flow function should not arrive here
-            //in case it did the meaning is that the entery point was wrong and a bug should be corrected
-            return stateTransitionTable[indexInTransitionTable].m_destinationState;
-        }
-    }
-    //int tick = UsersysTickGet();
-    //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
-    //cb_push_back (str, len);
-
-    //in normal execution flow function should not arrive here
-    //in case it did the meaning is that the entery point was wrong and a bug should be corrected
-    len = usnprintf(str, 80, "Internal: invalid slow motor transition state %d return code %d",state, returnCode );
-
-    return EXIT_STATE;
+    return OK;
 }
-//********************************************************************************
-//this function is used to manage and operate the motor managmant state mashine
-//the highest managment level
-//********************************************************************************
-bool PrintingIterate(void *JobDetails)
-{
-    uint32_t tick = 0;
-    char str[60];
-    uint8_t len = 0;
-    PrintingState_t keepstate  =  gPrintingState;
-    //
-    // Disable all interrupts.
-    //
-    ROM_IntMasterDisable();
-
-    ReturnCode (* state_fun)(void *JobDetails) = state[gPrintingState];
-    //if (_motorId == SCREW_MOTOR)
-    //    screw_movement[gPrintingState]++;
-    ReturnCode returnCode = state_fun(JobDetails);
 
-    gPrintingState = LookupTransitions(gPrintingState, returnCode);
-/*    if (keepstate != gPrintingState){
-        tick = UsersysTickGet();
-        len = usnprintf(str, 60, "\r\n changed state tick %d state %d retcode %d ", tick, gPrintingState,returnCode);
-        cb_push_back (str, len);
-
-    }
-*/
-    //
-    // Enable all interrupts.
-    //
-    ROM_IntMasterEnable();
-    return (gPrintingState != EXIT_STATE);
-}
 //********************************************************************************************************************
 
 void PrintingsInit(void)
@@ -242,10 +209,13 @@ void PrintSTMMsgHandler(void * msg)
     switch(PrtMessage->messageId)
     {
         case PrintRequest:
+            PreSegmentState(&CurrentJob);
             break;
         case PreSegmentResultsOk:
+            SegmentState(&CurrentJob);
             break;
         case PreSegmentResultsFail:
+            ExitState(Message->messageData);
             break;
         case SegmentResultsOk:
             break;
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
index 9af8fb4d3..7e76d6a0a 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
@@ -53,6 +53,19 @@ typedef enum
     Module_Waste,
     MAX_SYSTEM_MODULES
 }SYSTEM_MODULE_ID_ENUM;
+typedef enum
+{
+    JobSuccess = 0,
+    JobFail,
+    JobStop
+} ReturnCode;
+typedef enum
+{
+    ModuleIdle = 0,
+    ModuleWaiting,
+    ModuleDone,
+    ModuleFail
+} ModuleStateEnum;
 
 #define MAX_JOB_NAME_LEN  40
 #define MAX_SEGMENT_NUM  30
@@ -98,11 +111,16 @@ typedef struct PrintMessage{
     uint8_t messageData[MAX_MSG_LEN];
 }PrintMessageStruc;
 
+extern JobTicket *CurrentJob;
+extern JobTicket *PreviousJob;
+extern Mailbox_Handle JobmsgQ;
+
 void PrintSTMMsgHandler(void * msg);
 void JobInit(void);
 void PrintingsInit(void);
 
 void JobRequestFunc(MessageContainer* requestContainer);
 uint32_t PrepareReady(int ModuleId, int result);
+uint32_t PreSegmentReady(int ModuleId, int result);
 
 #endif /* STATEMACHINES_PRINTSTM_H_ */