Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

26 files changed, 1579 insertions, 71 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c b/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
index 216779f8b..1de8ac0de 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
@@ -1,7 +1,53 @@
 /*
  * DriverWithCallbackExample.c
  *
- *  Created on: 11 במרץ 2018
+ *  Created on: 11 march 2018
  *      Author: shlomo
  */
-uint32_t
+#include "include.h"
+#include "control.h"
+
+
+uint32_t KeepParameter = 0;
+callback_fptr ModuleCallback = 0;
+bool isValid (uint32_t deviceID);
+uint32_t ControlCallBackFunction(uint32_t deviceID, uint32_t ReadValue);
+
+
+
+uint32_t DriverActionWithCallback (uint32_t deviceId, uint32_t parameter, callback_fptr callback)
+{
+    assert (callback);
+    assert (isValid(deviceId));
+
+    //call driver action to device id with the parameter
+    //SetMotorSpeed (deviceId, parameter);
+    KeepParameter = parameter;
+    ModuleCallback = callback;
+    //start control:
+
+    uint32_t ControlId = AddControlCallback( callback,  eOneMillisecond, NULL, deviceId, parameter );
+    return ControlId;
+
+}
+
+uint32_t ControlCallBackFunction(uint32_t deviceId, uint32_t ReadValue)
+{
+       if (ReadValue == KeepParameter)
+       {
+           //stop this control loop
+           RemoveControlCallback(deviceId, ControlCallBackFunction );
+           //possibly: start regular control (speed etc)
+           //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, NULL, deviceId, Parameter );
+
+           //call the module callback
+           ModuleCallback(deviceId,ReadValue);
+
+       }
+   return OK;
+}
+
+bool isValid (uint32_t deviceID)
+{
+    return true;
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 6f3a59852..ded4c9f9e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -22,6 +22,10 @@
 
 #include "drivers/adc_sampling/adc.h"
 #include "control.h"
+
+#include "drivers/Motors/Motor.h"
+#include "drivers/Heater/TemperatureSensor.h"
+
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
@@ -30,7 +34,7 @@
 typedef enum
 {
     OneMillisec,
-}nillisecMessages;
+}MillisecMessages;
 
 typedef struct MillisecMessage{
     uint16_t messageId;
@@ -38,6 +42,13 @@ typedef struct MillisecMessage{
     uint32_t tick;
     uint8_t messageData[20];
 }MillisecMessageStruc;
+
+uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
+uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
+uint32_t  MotorSpeed_Data[MOTOR_SPARE1_1] = {0};
+uint32_t  MotorStatus_Data[MOTOR_SPARE1_1] = {0};
+bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
+
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
 bool                    MillisecRestart;
@@ -105,9 +116,6 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     else
         ROM_TimerDisable(Millisec_timerBase,TIMER_A);
 
-    //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
-    //we might want to call it from the task, afetr execution of other taks!!!
-    ADC_TriggerCollection();
     //send message to the Millisec task
     Message.messageId = OneMillisec;
     Message.tick = UsersysTickGet();
@@ -133,7 +141,46 @@ uint32_t MillisecLoop(uint32_t tick)
     Hundred_msTick  =   (tick%eHunderdMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
 
+    //gather Motor data from FPGA
+    MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
+    MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
+    MotorSpeed_Data[MOTOR_SCREW] = MotorGetSpeedFromFPGA(MOTOR_SCREW);
+    MotorStatus_Data[MOTOR_SCREW] = MotorGetStatusFromFPGA(MOTOR_SCREW);
+    MotorSpeed_Data[MOTOR_WINDER] = MotorGetSpeedFromFPGA(MOTOR_WINDER);
+    MotorStatus_Data[MOTOR_WINDER] = MotorGetStatusFromFPGA(MOTOR_WINDER);
+    MotorSpeed_Data[MOTOR_LDRIVING] = MotorGetSpeedFromFPGA(MOTOR_LDRIVING);
+    MotorStatus_Data[MOTOR_LDRIVING] = MotorGetStatusFromFPGA(MOTOR_LDRIVING);
+    MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
+    MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);
+    //gather Dancer data from FPGA
     //gather data from FPGA
+
+    if (Hundred_msTick)
+    {
+        int adc_i;
+        for (adc_i = 0; adc_i < MAX_ADC_DEVICES ; adc_i++)
+            ADC_Data[adc_i] = ADC_GetReading(adc_i);
+        //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
+        //we might want to call it from the task, after execution of other tasks!!!
+        ADC_TriggerCollection();
+
+        TEMPERATURE_SENSOR_ID_ENUM pt100_i;
+        for (pt100_i = 0; pt100_i < (int)MAX_TEMPERATURE_SENSOR_ID ; pt100_i++)
+            TemperatureSensor_Data[pt100_i] = TemperatureSensorReadFromFPGA(pt100_i);
+        MotorGetnBusyFromFPGA(); // get all motors nBusy bit status from the FPGAs
+        TimerMotors_t motor_i;
+        for (motor_i = 0; motor_i < MOTOR_SPARE1_1 ; motor_i++)
+        {
+            MotorBusy_Data[motor_i] = MotorGetnBusyState(motor_i);
+            if (MotorBusy_Data[motor_i] == false) //can get data
+            {
+                MotorSpeed_Data[motor_i] = MotorGetSpeedFromFPGA(motor_i);
+                MotorStatus_Data[motor_i] = MotorGetStatusFromFPGA(motor_i);
+            }
+        }
+    }
+    //gather data from FPGA
+
     return OK;
 }
 /******************************************************************************
@@ -163,3 +210,28 @@ void MillisecTask(UArg arg0, UArg arg1)
         }
     }
 }
+
+
+uint32_t getMotorStatusData(int MotorId)
+{
+    assert (MotorId < MOTOR_SPARE1_1);
+    return MotorStatus_Data[MotorId];
+}
+uint32_t getMotorSpeedData(int MotorId)
+{
+    assert (MotorId < MOTOR_SPARE1_1);
+    return MotorSpeed_Data[MotorId];
+}
+
+uint32_t getTemperatureSensorData(int SensorId)
+{
+    assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
+    return TemperatureSensor_Data[SensorId];
+}
+
+uint32_t getADCData(int DeviceId)
+{
+    assert (DeviceId < MAX_ADC_DEVICES);
+    return ADC_Data[DeviceId];
+}
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
new file mode 100644
index 000000000..0c9d66201
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -0,0 +1,25 @@
+/*
+ * MillisecTask.h
+ *
+ *  Created on: 15 במרץ 2018
+ *      Author: shlomo
+ */
+
+#ifndef MODULES_CONTROL_MILLISECTASK_H_
+#define MODULES_CONTROL_MILLISECTASK_H_
+
+#include "drivers/adc_sampling/adc.h"
+#include "control.h"
+#include "drivers/Motors/Motor.h"
+#include "drivers/Heater/TemperatureSensor.h"
+
+
+uint32_t getMotorStatusData(int MotorId);
+uint32_t getMotorSpeedData(int MotorId);
+uint32_t getTemperatureSensorData(int SensorId);
+uint32_t getADCData(int DeviceId);
+
+
+
+
+#endif /* MODULES_CONTROL_MILLISECTASK_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index abc61972d..1d9686124 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -62,7 +62,8 @@ typedef struct
 {
   uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
   bool ControlActive;
-  uint32_t Parameter;
+  uint32_t Parameter1;
+  uint32_t Parameter2;
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
   CTRL_TIMING_ENUM ControlTiming;
@@ -97,7 +98,7 @@ void OneMilliSecondFunction(UArg arg0);
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
-uint32_t TemplateDataReadCBFunction (uint32_t deviceID, uint32_t Parameter)
+uint32_t TemplateDataReadCBFunction (uint32_t deviceID, uint32_t Parameter1, uint32_t Parameter2)
 {
     return 0;
 }
@@ -160,23 +161,34 @@ void ControlStart(void)
  * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
  *
  ***************************************************************************************************************************************************/
-int  AddControlCallback(uint32_t deviceId, ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter )
+uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter1, uint32_t Parameter2 )
 {
-    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
     assert(Callback);
     assert(DriverfPtr);
     unsigned int key;
 
-
+    uint32_t device_i;
+    uint32_t deviceId = 0xFFFFFFFF;
+    for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
+    {
+        if (ControlArray[device_i].ControlActive == false)
+        {
+            deviceId = device_i;
+            break;
+        }
+    }
+    if (deviceId == 0xFFFFFFFF)
+        return 0xFFFFFFFF;
     key = GateMutex_enter(gateControlDB);
     ControlArray[deviceId].ControlTiming = CtrlFrequency;
     ControlArray[deviceId].ControlCallbackPtr = Callback;
     ControlArray[deviceId].ControlActive = true;
     ControlArray[deviceId].ControlDataReadPtr = DriverfPtr;
-    ControlArray[deviceId].Parameter = Parameter;
+    ControlArray[deviceId].Parameter1 = Parameter1;
+    ControlArray[deviceId].Parameter2 = Parameter2;
     GateMutex_leave(gateControlDB, key);
 
-    return OK;
+    return deviceId;
 
 }
 int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
@@ -212,9 +224,6 @@ void OneMilliSecondControlInterrupt(UArg arg0)
     else
         ROM_TimerDisable(Control_timerBase, TIMER_A);
 
-    //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
-    //we might want to call it from the task, afetr execution of other taks!!!
-    ADC_TriggerCollection();
     //send message to the control task
     Message.messageId = OneMillisec;
     Message.tick = UsersysTickGet();
@@ -243,33 +252,41 @@ uint32_t ControlLoop(uint32_t tick)
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
-        if (ControlArray[Device_i].ControlDataReadPtr)
-            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter);
         if (ControlArray[Device_i].ControlActive)
         {
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     break;
                 case eTenMilliSecond:
                     if (Ten_msTick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 case eHunderdMillisecond:
                     if (Hundred_msTick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 case eOneSecond:
                     if (Onesecond_Tick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 default:
                     LOG_ERROR (ControlArray[Device_i].ControlTiming, "Invalid control timing value");
                     break;
-            }
-        }
-    }
+            } //switch
+        } //if control active
+    }  //for
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 292bbeb82..f443a10d7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -10,7 +10,7 @@
 
 //typedef  uint32_t (* DeviceDataFunction)(uint32_t deviceID, uint32_t *Value);
 typedef  uint32_t (* ControlCBFunction)(uint32_t deviceID, uint32_t ReadValue);
-typedef  uint32_t (* DataReadCBFunction)(uint32_t deviceID, uint32_t Parameter);
+typedef  uint32_t (* DataReadCBFunction)(uint32_t deviceID, uint32_t Parameter1, uint32_t Parameter2);
 typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
@@ -20,10 +20,11 @@ typedef enum {
 }CTRL_TIMING_ENUM;
 
 
-void    ControlInit(void);
-void    ControlStop(void);
-void    ControlStart(void);
-int     AddControlCallback(uint32_t deviceId, ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter );
-int     RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
-uint32_t TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter);
+void        ControlInit(void);
+void        ControlStop(void);
+void        ControlStart(void);
+uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter1, uint32_t Parameter2 );
+int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
+uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1, uint32_t Parameter2);
+
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index 1162ff11e..b835cf8f1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -23,6 +23,7 @@
 #include "include.h"
 #include <driverlib/timer.h>
 
+#include "heaters_ex.h"
 #include "heaters.h"
 #include "Drivers/Heater/Heater.h"
 
@@ -50,6 +51,7 @@ HeaterControlConfig_t  HeaterPIDConfig[MAX_HEATERS_NUM] = {0};
 uint32_t DeviceId2Heater[MAX_HEATERS_NUM] = {0};
 bool AcHeaterConfigured[MAX_AC_HEATERS] = {0};
 
+bool FastHeating = 1;
 /******************** CODE  ********************************************/
 /*
  * HeatersInit
@@ -107,6 +109,10 @@ uint32_t HeaterConfigRequestMessage(HardwarePidControl* request)
                     LOG_ERROR (ValidationError, "Validation Error in Heaters Control");
                     return ERROR;
                 }
+            else
+            {
+                HeaterConfigSetSharedHeatersParams(1000,120000*12); // initial setting 50Mhz
+            }
         }
         return OK;
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index f1920f68f..614250a38 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -23,8 +23,22 @@
 
 #include "heaters.h"
 #include "Drivers/Heater/Heater.h"
+#include "Drivers/Heater/TemperatureSensor.h"
 
 
+/******************** Data Structures  ********************************************/
+typedef enum
+{
+    HeatersControl,
+}HeatersControlMessages;
+
+typedef struct HeatersControlMessage{
+    uint16_t messageId;
+    uint16_t msglen;
+    uint32_t tick;
+    uint8_t messageData[20];
+}HeatersControlMessageStruc;
+
 /******************** GLOBAL PARAMETERS  ********************************************/
 HeaterCommand          HeaterCmd[MAX_HEATERS_NUM] = {};
 int      NumberOFSlicesInUse = 0;
@@ -35,16 +49,64 @@ uint32_t  Heater_timerBase = TIMER2_BASE;        //Timer handle
 uint32_t OutputProportionalCycleTime = 0; //A/C Heaters Cycle time in milliseconds - one for all heaters
 char     TimeSliceAllocation[MAX_TIMESLICES] = {0xFF};
 
-bool InitialHeatingState = false;
+bool InitialHeatingState = true;
+bool TimerActivated = false;
+
+Mailbox_Handle          HeatersControlMsgQ = NULL;
+
 /******************** FUNCTIONS  ********************************************/
 uint32_t HeaterControlCBFunction(uint32_t deviceID, uint32_t ReadValue);
 uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue);
+void HeatersStartControlTimer (void);
 
-/*
+//**********************************************************************
+//******************** CODE  *******************************************/
+//**********************************************************************
+
+void HeatersControlInit(void)
+{
+
+    HeatersControlMsgQ = Mailbox_create(sizeof(HeatersControlMessageStruc), 20, NULL,NULL);
+
+    ROM_TimerConfigure(Heater_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    ROM_IntEnable(INT_TIMER2A);
+    ROM_TimerIntEnable(Heater_timerBase, TIMER_TIMA_TIMEOUT);
+
+    HeatersRestart = false;
+
+    return;
+}
+void HeatersControlStop(void)
+{
+    Report("HeatersControlStop ", __FILE__,__LINE__,0, RpMessage, 0, 0);
+    TimerActivated = false;
+    HeatersRestart = false;
+}
+
+void HeatersControlStart(void)
+{
+    Report("HeatersControlStart ", __FILE__,__LINE__,0, RpMessage, 0, 0);
+    HeatersRestart = true;
+    HeatersStartControlTimer();
+}
+
+void HeatersStartControlTimer (void)
+{
+    if (TimerActivated == true)
+        return;
+    TimerActivated = true;
+    ROM_TimerConfigure(Heater_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    ROM_IntEnable(INT_TIMER2A);
+    ROM_TimerIntEnable(Heater_timerBase, TIMER_TIMA_TIMEOUT);
+    Report("HeatersStartControlTimer ", __FILE__,__LINE__,0, RpMessage, 0, 0);
+    return;
+}
+
+/**************************************************************************************
  * HeaterCommandRequestMessage
  * called by: Communication from host
  * initialized all global data
- */
+ *************************************************************************************/
 uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
 {
     //uint32_t status = NOT_SUPPORTED;
@@ -69,9 +131,6 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
         //start the control
         PrepareHeater(HeaterId,Temperature);  //prepare the heaters control info
         //set the power balance handler (if not set yet)
-        ROM_TimerConfigure(Heater_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
-        ROM_IntEnable(INT_TIMER2A);
-        ROM_TimerIntEnable(Heater_timerBase, TIMER_TIMA_TIMEOUT);
         // if the heater is off (?) start it.
         ActivateHeater(HeaterId);
         //set the heater operation mode to fast heating - depended on the current temperature
@@ -123,7 +182,7 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
     HeaterPIDConfig[HeaterId].m_mesuredParam = 0;
     HeaterPIDConfig[HeaterId].m_preError = 0;
     HeaterPIDConfig[HeaterId].m_SetParam = SetTemperatue;//need to update SetParams on presegment stage
-    AddControlCallback(DeviceId2Heater[HeaterId], HeaterControlCBFunction, eHunderdMillisecond,TemplateDataReadCBFunction,HeaterId);
+    AddControlCallback( HeaterControlCBFunction, eHunderdMillisecond,TemperatureSensorRead,HeaterId,0);
 
     return OK;
 }
@@ -196,6 +255,37 @@ uint32_t HeaterControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 int  SliceCounter = 0;
 void EightMilliSecondHeatersInterrupt(UArg arg0)
 {
+
+    ROM_TimerIntClear(Heater_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
+
+    ROM_IntMasterDisable();
+    HeatersControlMessageStruc Message;
+    bool retcode = false;
+
+    //send message to the HeatersControl task
+    Message.messageId = HeatersControl;
+    Message.tick = UsersysTickGet();
+    Message.msglen = sizeof(HeatersControlMessageStruc);
+    if (HeatersControlMsgQ != NULL)
+        retcode = Mailbox_post(HeatersControlMsgQ , &Message, BIOS_NO_WAIT);
+
+    if (HeatersRestart == true)
+    {
+        ROM_TimerLoadSet(Heater_timerBase, TIMER_A,OutputProportionalSingleStep/*twelve millisecond???*/);
+    }
+    else
+        TimerDisable(Heater_timerBase, TIMER_A);
+
+    //
+    // Enable all interrupts.
+    //
+    ROM_IntMasterEnable();
+    return ;
+}
+
+
+uint32_t HeatersControlLoop(uint32_t tick)
+{
     char str[100];
     uint8_t len = 0;
 
@@ -203,7 +293,6 @@ void EightMilliSecondHeatersInterrupt(UArg arg0)
     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]);
-    ROM_IntMasterDisable();
 
     if (TimeSliceAllocation[SliceCounter] == HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w)
     {
@@ -236,16 +325,32 @@ void EightMilliSecondHeatersInterrupt(UArg arg0)
     if (SliceCounter >= NumberOFSlicesInUse)
         SliceCounter = 0;
 
-    if (HeatersRestart == true)
+    return OK;
+}
+/******************************************************************************
+ *  ======== messageTsk ========
+ *  Task for this function is created statically. See the project's .cfg file.
+ *  this message task is created statically in system initialization,
+ ******************************************************************************/
+void HeatersControlTask(UArg arg0, UArg arg1)
+{
+    HeatersControlMessageStruc Message;
+    //char str[60];
+    //uint16_t length;
+    //Clock_setTimeout(HostKAClock, 1000);
+    //Clock_start(HostKAClock);
+    HeatersControlInit();
+
+    while(1)
     {
-        ROM_TimerLoadSet(Heater_timerBase, TIMER_A,OutputProportionalSingleStep/*twelve millisecond???*/);
+        Mailbox_pend(HeatersControlMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
+        {
+            case HeatersControl:
+                HeatersControlLoop(Message.tick);
+                break;
+            default:
+                break;
+        }
     }
-    else
-        TimerDisable(Heater_timerBase, TIMER_A);
-
-    //
-    // Enable all interrupts.
-    //
-    ROM_IntMasterEnable();
-    return ;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
index 1c9fee547..074923d79 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
@@ -1,6 +1,8 @@
+#ifndef MODULES_IDS_IDS_H_
+#define MODULES_IDS_IDS_H_
 #include "PMR/common/MessageContainer.pb-c.h"
 #include "PMR/Hardware/HardwareDispenser.pb-c.h"
-
+#include "drivers/motors/motor.h"
 #define MAX_SYSTEM_DISPENSERS 8
 typedef struct
 {
@@ -22,4 +24,9 @@ typedef struct
       int32_t id;
 }DispenserConfigStruc;
 
+
+extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS];
 uint32_t DispenserConfigMessage(HardwareDispenser * request);
+
+
+#endif  //MODULES_IDS_IDS_H_
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
index 0294615b6..db4dbb1f0 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
@@ -12,6 +12,8 @@
 
 DispenserConfigStruc DispensersCfg[ MAX_SYSTEM_DISPENSERS];
 
+uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {MOTOR_DISPENSER_1,MOTOR_DISPENSER_2,MOTOR_DISPENSER_3,MOTOR_DISPENSER_4,MOTOR_DISPENSER_5,MOTOR_DISPENSER_6,MOTOR_DISPENSER_7,MOTOR_DISPENSER_8};
+
 
 
 uint32_t DispenserConfigMessage(HardwareDispenser * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 32b147569..13ea3d77e 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -11,6 +11,7 @@
 #include "PMR/Hardware/Hardwaremotor.pb-c.h"
 #include "PMR/Hardware/HardwareDispenser.pb-c.h"
 #include "StateMachines/Printing/printingSTM.h"
+#include "drivers/motors/motor.h"
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
index 1014c08fc..49ddb757c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Container.c
@@ -15,6 +15,12 @@
 #include "Stub_SteperMotor.h"
 #include "Stub_Valve.h"
 #include "Stub_ExtFlash.h"
+#include "Stub_FPGAReadBackReg.h"
+#include "Stub_FPGAReadVersion.h"
+#include "Stub_L6470.h"
+#include "Stub_Motor.h"
+#include "Stub_HW_Version.h"
+
 #include "General/GeneralHardware.h"
 #include "heaters/heaters_ex.h"
 #include "Drivers/USB_Communication/USBCDCD.h"
@@ -111,6 +117,39 @@ void receive_callback(char* buffer, size_t length)
          case MESSAGE_TYPE__StubExtFlashWriteRequest:
              Stub_ExtFlashWriteRequest(requestContainer);
              break;
+		case MESSAGE_TYPE__StubFPGAReadBackRegRequest:
+             Stub_FPGAReadBackRegRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubFPGAReadVersionRequest:
+             Stub_FPGAReadVersionRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubL6470DriverRequest:
+             Stub_L6470DriverRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorInitRequest:
+             Stub_MotorInitRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorRunRequest:
+             Stub_MotorRunRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorStopRequest:
+             Stub_MotorStopRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorStatusRequest:
+             Stub_MotorStatusRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorPositionRequest:
+             Stub_MotorPositionRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorSpeedRequest:
+             Stub_MotorSpeedRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubMotorMovRequest:
+             Stub_MotorMovRequest(requestContainer);
+             break;
+         case MESSAGE_TYPE__StubHWVersionRequest:
+             Stub_HWVersionRequest(requestContainer);
+             break;
          case MESSAGE_TYPE__DebugLogRequest:
              ReportInitMessage(requestContainer);
              break;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/DataDef.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/DataDef.h
index 08da87c09..67c509d33 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/DataDef.h
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/DataDef.h
@@ -25,18 +25,23 @@ enum
 #define NEW  0
 #define USED 1
 
-#define START 1
-#define STOP  0
+#define START 0
+#define STOP  1
+#define NOSTOP     2
+#define CONTINUE   2
 
+#define NOTBUSY 0
+#define BUSY 1
+//
 // motor direction
-#define CW  1 // clockwise
-#define CCW 0 // counter clockwise
+#define CW  0 // clockwise
+#define CCW 1 // counter clockwise
 
 #define TRUE  1
 #define FALSE 0
 
-#define INPUT  1
-#define OUTPUT 0
+#define INPUT 0x0
+#define OUTPUT 0x1
 
 #define PULLDOWN 0
 #define PULLUP   1
@@ -44,6 +49,41 @@ enum
 #define TX 0
 #define RX 1
 #define TXRX 2
+#define ODD         0
+#define EVEN        1
+
+#define READY       0
+#define NOTREADY    1
+#define WAIT        2
+
+#define LOW  0x0
+#define HIGH 0x1
+
+#define WR          0
+#define RD          1
+
+#define FLAGSTART   1
+#define FLAGSET     1
+#define FLAGCLEAR   0
+
+#define IN              2
+#define OUT             3
+
+#define INIT 1
+#define SKIP_INIT 0
+
+#define byte uint8_t//char
+#define boolean bool
+
+#define SERIAL  0x0
+#define DISPLAY 0x1
+
+#define LSBFIRST 0
+#define MSBFIRST 1
+
+#define CHANGE 1
+#define FALLING 2
+#define RISING 3
 
 //---------------------------------------------------------------------
 #pragma diag_suppress 169 /*  Avoid warnings on VOID * conversions (protobuf issue)
@@ -56,7 +96,10 @@ The warning:
 or in the environment add command line:
 
 Project -> properties ->
-
+Press on show advanced settings
+CCS Build ->
+Advanced Options ->
+Diagnistic Options ->
 Add the warning number in
 Supports diagnostic <id> (--diag_suppress, - pds)
 169
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c
new file mode 100644
index 000000000..13df055d6
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.c
@@ -0,0 +1,69 @@
+
+#include <PMR/Stubs/StubFPGAReadBackRegRequest.pb-c.h>
+#include <PMR/Stubs/StubFPGAReadBackRegResponse.pb-c.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <Stubs_Handler/DataDef.h>
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Container.h"
+#include "Stub_Status.h"
+#include "drivers/FPGA/FPGA.h"
+
+void Stub_FPGAReadBackRegRequest(MessageContainer* requestContainer)
+{
+    uint32_t status = FAILED;
+
+    unsigned short ReadBack_Value;
+
+    MessageContainer responseContainer;
+
+    StubFPGAReadBackRegRequest* request = stub_fpgaread_back_reg_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    writeLine("ReadBack Request: ");
+
+    writeFloat(request->fpgaid);
+    writeString(", ");
+    writeFloat(request->value);
+
+    status = Test_FPGA_ReadBack((unsigned char) request->fpgaid, (unsigned short) request->value, &ReadBack_Value);
+
+    StubFPGAReadBackRegResponse response =   STUB_FPGAREAD_BACK_REG_RESPONSE__INIT;
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    response.fpgaid = request->fpgaid;
+    response.has_fpgaid = true;
+    response.readbackvalue = ReadBack_Value;
+    response.has_readbackvalue = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubFPGAReadBackRegResponse, requestContainer->token, true, &response, &stub_fpgaread_back_reg_response__pack, &stub_fpgaread_back_reg_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.fpgaid);
+    writeString(", ");
+    writeFloat(response.readbackvalue);
+    writeString(", ");
+
+    Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.h
new file mode 100644
index 000000000..14e245225
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadBackReg.h
@@ -0,0 +1,11 @@
+#ifndef STUB_READBACK_H
+#define STUB_READBACK_H
+
+void Stub_FPGAReadBackRegRequest(MessageContainer* requestContainer);
+
+#endif //STUB_READBACK_H
+
+
+
+
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.c
new file mode 100644
index 000000000..71ec6fc0a
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.c
@@ -0,0 +1,81 @@
+
+#include <PMR/Stubs/StubFPGAReadVersionResponse.pb-c.h>
+#include <PMR/Stubs/StubFPGAReadVersionRequest.pb-c.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <Stubs_Handler/DataDef.h>
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Container.h"
+#include "Stub_Status.h"
+#include "drivers/FPGA/FPGA.h"
+
+void Stub_FPGAReadVersionRequest(MessageContainer* requestContainer)
+{
+    uint32_t status = FAILED;
+    unsigned char Version = 0;
+    unsigned char Year = 0 ;
+    unsigned char Month = 0;
+    unsigned char Day = 0;
+
+    MessageContainer responseContainer;
+
+    StubFPGAReadVersionRequest* request = stub_fpgaread_version_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    writeLine("ReadBack Request: ");
+
+    writeFloat(request->fpgaid);
+
+    status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
+
+    StubFPGAReadVersionResponse response =   STUB_FPGAREAD_VERSION_RESPONSE__INIT;
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    response.fpgaid = request->fpgaid;
+    response.has_fpgaid = true;
+    response.day = Day;
+    response.has_day = true;
+    response.month = Month;
+    response.has_month = true;
+    response.year = Year;
+    response.has_year = true;
+    response.ver_num = Version;
+    response.has_ver_num = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubFPGAReadVersionResponse, requestContainer->token, true, &response, &stub_fpgaread_version_response__pack, &stub_fpgaread_version_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.fpgaid);
+    writeString(", ");
+    writeFloat(response.day);
+    writeString(", ");
+    writeFloat(response.month);
+    writeString(", ");
+    writeFloat(response.year);
+    writeString(", ");
+    writeFloat(response.ver_num);
+    writeString(", ");
+
+    Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.h
new file mode 100644
index 000000000..d9c33b8b8
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGAReadVersion.h
@@ -0,0 +1,6 @@
+#ifndef STUB_FPGAVER_H
+#define STUB_FPGAVER_H
+
+void Stub_FPGAReadVersionRequest(MessageContainer* requestContainer);
+
+#endif //STUB_FPGAVER_H
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.c
new file mode 100644
index 000000000..72d935b83
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.c
@@ -0,0 +1,95 @@
+
+#include <PMR/Stubs/StubHWVersionResponse.pb-c.h>
+#include <PMR/Stubs/StubHWVersionRequest.pb-c.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <Stubs_Handler/DataDef.h>
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+#include "driverlib/gpio.h"
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Container.h"
+#include "Stub_Status.h"
+#include "drivers/FPGA/FPGA.h"
+#include "driverlib/rom_map.h"
+#include "driverlib/rom.h"
+
+
+char Read_HW_Version(unsigned char *Brd_ID, unsigned char *Assy_ID)
+{
+    //TODO Move in GPIO Initialisation
+
+    // -----------  Set HW Version GPIO as Input -----------
+    MAP_GPIOPinTypeGPIOInput(GPIO_PORTS_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1);
+    MAP_GPIOPinTypeGPIOInput(GPIO_PORTJ_BASE, GPIO_PIN_5 | GPIO_PIN_4 | GPIO_PIN_7);
+    MAP_GPIOPinTypeGPIOInput(GPIO_PORTP_BASE, GPIO_PIN_3 | GPIO_PIN_5);
+
+    //Set HW Version GPIO to Pull down
+    GPIOPadConfigSet(GPIO_PORTS_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPD );
+    GPIOPadConfigSet(GPIO_PORTJ_BASE, GPIO_PIN_5 | GPIO_PIN_4 | GPIO_PIN_7, GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPD );
+    GPIOPadConfigSet(GPIO_PORTP_BASE, GPIO_PIN_3 | GPIO_PIN_5,              GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPD );
+    // ------------------------------------------------------
+
+    if (ROM_GPIOPinRead(GPIO_PORTS_BASE, GPIO_PIN_3) == GPIO_PIN_3)
+        *Brd_ID |= 0x08;
+    if (ROM_GPIOPinRead(GPIO_PORTS_BASE, GPIO_PIN_2) == GPIO_PIN_2)
+        *Brd_ID |= 0x04;
+    if (ROM_GPIOPinRead(GPIO_PORTJ_BASE, GPIO_PIN_5) == GPIO_PIN_5)
+        *Brd_ID |= 0x02;
+    if (ROM_GPIOPinRead(GPIO_PORTJ_BASE, GPIO_PIN_4) == GPIO_PIN_4)
+        *Brd_ID |= 0x01;
+
+    if (ROM_GPIOPinRead(GPIO_PORTP_BASE, GPIO_PIN_3) == GPIO_PIN_3)
+        *Assy_ID |= 0x08;
+    if (ROM_GPIOPinRead(GPIO_PORTP_BASE, GPIO_PIN_5) == GPIO_PIN_5)
+        *Assy_ID |= 0x04;
+    if (ROM_GPIOPinRead(GPIO_PORTS_BASE, GPIO_PIN_1) == GPIO_PIN_1)
+        *Assy_ID |= 0x02;
+    if (ROM_GPIOPinRead(GPIO_PORTJ_BASE, GPIO_PIN_7) == GPIO_PIN_7)
+        *Assy_ID |= 0x01;
+
+    return PASSED;
+}
+
+void Stub_HWVersionRequest(MessageContainer* requestContainer)
+{
+    uint32_t status = FAILED;
+    unsigned char Brd_ID  = 0;
+    unsigned char Assy_ID = 0;
+
+    MessageContainer responseContainer;
+
+    StubHWVersionRequest* request = stub_hwversion_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    status = Read_HW_Version(&Brd_ID, &Assy_ID);
+
+    StubHWVersionResponse response =   STUB_HWVERSION_RESPONSE__INIT;
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    response.brd_id = Brd_ID ;
+    response.has_brd_id = true;
+    response.assy_id = Assy_ID;
+    response.has_assy_id = true;
+    
+    responseContainer = createContainer(MESSAGE_TYPE__StubHWVersionResponse, requestContainer->token, true, &response, &stub_hwversion_response__pack, &stub_hwversion_response__get_packed_size);
+
+    Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.h
new file mode 100644
index 000000000..b1bc504a3
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_HW_Version.h
@@ -0,0 +1,6 @@
+#ifndef STUB_HWVER_H
+#define STUB_HWVER_H
+
+void Stub_HWVersionRequest(MessageContainer* requestContainer);
+
+#endif //STUB_HWVER_H
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.c
new file mode 100644
index 000000000..4b9f016de
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.c
@@ -0,0 +1,133 @@
+
+#include <PMR/Stubs/StubL6470DriverResponse.pb-c.h>
+#include <PMR/Stubs/StubL6470DriverRequest.pb-c.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <Stubs_Handler/DataDef.h>
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Container.h"
+#include "Stub_Status.h"
+//#include "drivers/FPGA/FPGA.h"
+#include "drivers/SPI/SPI_Comm.h"
+
+#include "drivers/FPGA/Moters_Driver/L6470.h"
+
+#include "driverlib/ssi.h"
+
+    unsigned long Run_Value = 136902 ;
+    unsigned long Pos_Value;
+    unsigned long Mov_Value ;
+    bool Direction ;
+    unsigned long Time_2_Change_Direction ;
+    bool Display_Tx_ON_LCD;
+    bool Display_Rx_on_LCD;
+    unsigned long Init_MicroStep ;
+    unsigned long Init_Acc;
+    unsigned long Init_Dec;
+    bool TestBool_1;
+    bool TestBool_2;
+    bool TestBool_3;
+    bool TestBool_4;
+    bool TestBool_5;
+    unsigned long TestUint32_1;
+    unsigned long TestUint32_2;
+    unsigned long TestUint32_3;
+    unsigned long TestUint32_4;
+    unsigned long TestUint32_5;
+    unsigned long TestUint32_6;
+
+void Stub_L6470DriverRequest(MessageContainer* requestContainer)
+{
+    uint32_t status = PASSED;
+
+    MessageContainer responseContainer;
+
+    StubL6470DriverRequest* request = stub_l6470_driver_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    Run_Value = request->run_value;
+    Mov_Value = request->mov_value;
+    Direction = request->direction;
+    Time_2_Change_Direction = request->time_2_change_direction;
+    Display_Tx_ON_LCD = request->display_tx_on_lcd;
+    Display_Rx_on_LCD = request->display_rx_on_lcd;
+    Init_MicroStep = request->init_microstep;
+    Init_Acc = request->init_acc;
+    Init_Dec = request->init_dec;
+    TestBool_1 = request->testbool_1;
+    TestBool_2 = request->testbool_2;
+    TestBool_3 = request->testbool_3;
+    TestBool_4 = request->testbool_4;
+    TestBool_5 = request->testbool_5;
+    TestUint32_1 = request->testuint32_1;
+    TestUint32_2 = request->testuint32_2;
+    TestUint32_3 = request->testuint32_3;
+    TestUint32_4 = request->testuint32_4;
+    TestUint32_5 = request->testuint32_5;
+    TestUint32_6 = request->testuint32_6;
+
+    writeLine("L6470 Request: ");
+
+    writeFloat(request->run_value);
+
+    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
+    SPI_Control(INIT);
+
+    //temp_init_spi2();
+
+
+    StubL6470DriverResponse response =   STUB_L6470_DRIVER_RESPONSE__INIT;
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    //response.run_value = Get_Param(x_SPEED);
+    /*
+    response.mov_value = ;
+    response.day = Day;
+    response.has_day = true;
+    response.month = Month;
+    response.has_month = true;
+    response.year = Year;
+    response.has_year = true;
+    response.ver_num = Version;
+    response.has_ver_num = true;
+*/
+    responseContainer = createContainer(MESSAGE_TYPE__StubL6470DriverResponse, requestContainer->token, true, &response, &stub_l6470_driver_response__pack, &stub_l6470_driver_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.run_value);
+    /*
+    writeString(", ");
+    writeFloat(response.day);
+    writeString(", ");
+    writeFloat(response.month);
+    writeString(", ");
+    writeFloat(response.year);
+    writeString(", ");
+    writeFloat(response.ver_num);
+    writeString(", ");
+*/
+    Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+
+
+    //StartUSB(ui32SysClock); //todo  to test the USB comm problem after using 6470
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.h
new file mode 100644
index 000000000..0b4702861
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_L6470.h
@@ -0,0 +1,3 @@
+void Stub_L6470DriverRequest(MessageContainer* requestContainer);
+
+void temp_init_spi2();
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
new file mode 100644
index 000000000..c16e33a99
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -0,0 +1,453 @@
+
+#include <PMR/Stubs/StubMotorInitRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorInitResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorRunRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorRunResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorStopRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorStopResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorStatusRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorStatusResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorSpeedResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorSpeedRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorPositionRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorPositionResponse.pb-c.h>
+#include <PMR/Stubs/StubMotorMovRequest.pb-c.h>
+#include <PMR/Stubs/StubMotorMovResponse.pb-c.h>
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <Stubs_Handler/DataDef.h>
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "inc/hw_uart.h"
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+#include "MessageContainer.pb-c.h"
+
+#include "Container.h"
+#include "Stub_Status.h"
+#include "drivers/FPGA/FPGA.h"
+#include "drivers/SPI/SPI_Comm.h"
+
+#include "drivers/FPGA/Moters_Driver/L6470.h"
+
+#include "driverlib/ssi.h"
+
+
+extern unsigned long Run_Value ;
+extern unsigned long Pos_Value ;
+extern bool Direction ;
+unsigned char Stop_Command = 0 ;
+
+
+
+void Stub_MotorInitRequest(MessageContainer* requestContainer)
+{
+    //uint32_t status = FAILED;
+
+    MessageContainer responseContainer;
+
+    StubMotorInitRequest* request = stub_motor_init_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    //SPI_Control(INIT);
+
+    SPI2_Init();
+    init_BUSY_Pin();
+    //while(SSIBusy(SSI2_BASE)){};
+    //init_BUSY_Pin();
+
+
+    setup(request);
+    //setup();
+
+    //init_BUSY_Pin();
+    //while(SSIBusy(SSI2_BASE)){};
+    //ResetPos();
+    //while(SSIBusy(SSI2_BASE)){};
+
+    //Run_Value = request->run_value;
+    //Mov_Value = request->mov_value;
+    //Direction = request->direction;
+
+
+    //writeLine("L6470 Request: ");
+
+    //writeFloat(request->run_value);
+
+    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
+    //SPI_Control();
+
+    //temp_init_spi2();
+
+
+    StubMotorInitResponse response =   STUB_MOTOR_INIT_RESPONSE__INIT;
+
+   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+    response.status = 0;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorInitResponse, requestContainer->token, true, &response, &stub_motor_init_response__pack, &stub_motor_init_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.motor_id);
+    writeString(", ");
+	writeFloat(response.status);
+  
+    //writeString(", ");
+    //
+    //Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+void Stub_MotorRunRequest(MessageContainer* requestContainer)
+{
+    //uint32_t status = FAILED;
+
+    uint32_t read_status;
+
+    MessageContainer responseContainer;
+
+    StubMotorRunRequest* request = stub_motor_run_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+   float  temp =  (float)request->speed;
+    Run_Value = SpdCalc(temp);
+    Direction = request->direction;
+	
+	//SPI_Control(SKIP_INIT);
+    Mot_Run();
+
+    //writeLine("L6470 Request: ");
+
+    //writeFloat(request->run_value);
+
+    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
+
+
+
+    StubMotorRunResponse response =   STUB_MOTOR_RUN_RESPONSE__INIT;
+
+   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+    read_status = Get_and_Clear_Status();
+
+    response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
+    response.has_speed = true;
+
+    response.status_reg = read_status;
+    response.has_status_reg = true;
+
+    response.direction = ((read_status & x_STATUS_DIR)>>4);
+    response.has_direction = true;
+
+    response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
+    response.has_step_loss_a = true;
+    response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
+    response.has_step_loss_b = true;
+
+
+    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
+    response.has_mot_status = true;
+
+    response.busy = ((read_status & x_STATUS_BUSY)>>1);
+    response.has_busy = true;
+
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorRunResponse, requestContainer->token, true, &response, &stub_motor_run_response__pack, &stub_motor_run_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.motor_id);
+  
+    //writeString(", ");
+    //writeFloat(response.day);
+    
+    //Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+void Stub_MotorStopRequest(MessageContainer* requestContainer)
+{
+    uint32_t read_status;
+    //uint32_t status = FAILED;
+
+    MessageContainer responseContainer;
+
+    StubMotorStopRequest* request = stub_motor_stop_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    
+    Stop_Command = request->stop_cmd;
+	
+	//SPI_Control(SKIP_INIT);
+    Mot_Stop();
+
+    //writeLine("L6470 Request: ");
+
+    //writeFloat(request->run_value);
+
+    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
+
+
+
+    StubMotorStopResponse response =   STUB_MOTOR_STOP_RESPONSE__INIT;
+
+   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+    read_status = Get_and_Clear_Status();
+
+    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
+    response.has_mot_status = true;
+
+	response.hiz = (read_status & x_STATUS_HIZ);
+	response.has_hiz = true;
+
+    response.busy = ((read_status & x_STATUS_BUSY)>>1);
+    response.has_busy = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorStopResponse, requestContainer->token, true, &response, &stub_motor_stop_response__pack, &stub_motor_stop_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.motor_id);
+  
+    //writeString(", ");
+    //writeFloat(response.day);
+    
+    //Write_status_response(status);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+
+void Stub_MotorStatusRequest(MessageContainer* requestContainer)
+{
+    uint32_t read_status;
+
+    MessageContainer responseContainer;
+
+    StubMotorStatusRequest* request = stub_motor_status_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    
+    if(request->clearstatus)
+	{
+		read_status = Get_and_Clear_Status();
+	}
+	else
+	{
+		read_status = Get_Param(x_STATUS);
+	}
+
+    StubMotorStatusResponse response =   STUB_MOTOR_STATUS_RESPONSE__INIT;
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+    response.sck_mod = ((read_status & x_STATUS_SCK_MOD)>>15);
+    response.has_sck_mod = true;
+
+    response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
+    response.has_step_loss_b = true;
+
+    response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
+    response.has_step_loss_a = true;
+
+    response.ocd = ((read_status & x_STATUS_OCD)>>12);
+    response.has_ocd = true;
+
+    response.th_sd = ((read_status & x_STATUS_TH_SD)>>11);
+    response.has_th_sd = true;
+
+    response.th_wrn = ((read_status & x_STATUS_TH_WRN)>>10);
+    response.has_th_wrn = true;
+
+    response.uvlo = ((read_status & x_STATUS_UVLO)>>9);
+    response.has_uvlo = true;
+
+    response.wrong_cmd = ((read_status & x_STATUS_WRONG_CMD)>>8);
+    response.has_wrong_cmd = true;
+
+    response.notperf_cmd = ((read_status & x_STATUS_NOTPERF_CMD)>>7);
+    response.has_notperf_cmd = true;
+
+    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
+    response.has_mot_status = true;
+
+    response.dir = ((read_status & x_STATUS_DIR)>>4);
+    response.has_dir = true;
+
+    response.sw_evn = ((read_status & x_STATUS_SW_EVN)>>3);
+    response.has_sw_evn = true;
+
+    response.sw_f = ((read_status & x_STATUS_SW_F)>>2);
+    response.has_sw_f = true;
+
+    response.busy = ((read_status & x_STATUS_BUSY)>>1);
+    response.has_busy = true;
+
+	response.hiz = (read_status & x_STATUS_HIZ);
+	response.has_hiz = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorStatusResponse, requestContainer->token, true, &response, &stub_motor_status_response__pack, &stub_motor_status_response__get_packed_size);
+
+    writeLine("Sending Response: ");
+
+    writeFloat(response.motor_id);
+ 
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+void Stub_MotorSpeedRequest(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    StubMotorSpeedRequest* request = stub_motor_speed_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubMotorSpeedResponse response =   STUB_MOTOR_SPEED_RESPONSE__INIT;
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+    //response.speed = Get_Param(x_SPEED);
+    response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
+    response.has_speed = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorSpeedResponse, requestContainer->token, true, &response, &stub_motor_speed_response__pack, &stub_motor_speed_response__get_packed_size);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+void Stub_MotorPositionRequest(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    StubMotorPositionRequest* request = stub_motor_position_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubMotorPositionResponse response =   STUB_MOTOR_POSITION_RESPONSE__INIT;
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+    response.position = Get_Param(x_ABS_POS);
+    response.has_position = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorPositionResponse, requestContainer->token, true, &response, &stub_motor_position_response__pack, &stub_motor_position_response__get_packed_size);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
+
+void Stub_MotorMovRequest(MessageContainer* requestContainer)
+{
+    uint32_t read_status;
+
+    MessageContainer responseContainer;
+
+    StubMotorMovRequest* request = stub_motor_mov_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    Pos_Value = request->position;
+    Direction = request->direction;
+
+    Mot_Mov();
+
+    StubMotorMovResponse response =   STUB_MOTOR_MOV_RESPONSE__INIT;
+
+    response.motor_id = request->motor_id;
+    response.has_motor_id = true;
+
+
+    response.position = Get_Param(x_ABS_POS);
+    response.has_position = true;
+
+    while(SSIBusy(SSI2_BASE)){};
+
+    read_status = Get_and_Clear_Status();
+
+
+    response.status_reg = read_status;
+    response.has_status_reg = true;
+
+    response.direction = ((read_status & x_STATUS_DIR)>>4);
+    response.has_direction = true;
+
+    response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
+    response.has_step_loss_a = true;
+    response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
+    response.has_step_loss_b = true;
+
+
+    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
+    response.has_mot_status = true;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubMotorMovResponse, requestContainer->token, true, &response, &stub_motor_mov_response__pack, &stub_motor_mov_response__get_packed_size);
+
+    free(request);
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    SendChars((char*)container_buffer, container_size);
+    free(container_buffer);
+    free(requestContainer);
+}
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.h
new file mode 100644
index 000000000..95f82353f
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.h
@@ -0,0 +1,7 @@
+void Stub_MotorInitRequest(MessageContainer* requestContainer);
+void Stub_MotorRunRequest(MessageContainer* requestContainer);
+void Stub_MotorStopRequest(MessageContainer* requestContainer);
+void Stub_MotorStatusRequest(MessageContainer* requestContainer);
+void Stub_MotorSpeedRequest(MessageContainer* requestContainer);
+void Stub_MotorPositionRequest(MessageContainer* requestContainer);
+void Stub_MotorMovRequest(MessageContainer* requestContainer);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.c
new file mode 100644
index 000000000..994f6d84f
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.c
@@ -0,0 +1,264 @@
+//*****************************************************************************
+//
+// temperature_sensor.c - Example demonstrating the internal ADC temperature
+//                        sensor.
+//
+// Copyright (c) 2010-2014 Texas Instruments Incorporated.  All rights reserved.
+// Software License Agreement
+//
+//   Redistribution and use in source and binary forms, with or without
+//   modification, are permitted provided that the following conditions
+//   are met:
+//
+//   Redistributions of source code must retain the above copyright
+//   notice, this list of conditions and the following disclaimer.
+//
+//   Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the
+//   distribution.
+//
+//   Neither the name of Texas Instruments Incorporated nor the names of
+//   its contributors may be used to endorse or promote products derived
+//   from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// This is part of revision 2.1.0.12573 of the Tiva Firmware Development Package.
+//
+//*****************************************************************************
+/*
+ *  This code was made to show a simple ADC read.
+ *
+ *  It was made from the example provided by TivaWare but it was a some modifications
+ * like the math
+ *
+ *
+ * Lu?s Afonso
+ * https://sites.google.com/site/luiselectronicprojects/tutorials/tiva-tutorials/tiva-adc/internal-temperature-sensor
+ *
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "stdlib.h"
+#include "inc/hw_ints.h"
+#include "inc/hw_memmap.h"
+#include "inc/hw_uart.h"
+#include "inc/hw_gpio.h"
+#include "inc/hw_pwm.h"
+#include "inc/hw_types.h"
+#include "driverlib/adc.h"
+#include "driverlib/timer.h"
+#include "driverlib/gpio.h"
+#include "driverlib/interrupt.h"
+#include "driverlib/pin_map.h"
+#include "driverlib/rom.h"
+#include "driverlib/rom_map.h"
+#include "driverlib/sysctl.h"
+#include "driverlib/uart.h"
+#include "driverlib/udma.h"
+#include "driverlib/pwm.h"
+#include "driverlib/ssi.h"
+#include "driverlib/systick.h"
+#include "driverlib/adc.h"
+#include "utils/uartstdio.h"
+#include "utils/uartstdio.c"
+#include <string.h>
+
+#include "Drivers/USB_Communication/USBCDCD.h"
+#include "drivers/twine_graphicslib/graphics_adapter.h"
+
+//*****************************************************************************
+//
+// This function sets up UART0 to be used for a console to display information
+// as the example is running.
+//
+//*****************************************************************************
+void
+InitConsole(void)
+{
+    //
+    // Enable GPIO port A which is used for UART0 pins.
+    // TODO: change this to whichever GPIO port you are using.
+    //
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
+
+    //
+    // Configure the pin muxing for UART0 functions on port A0 and A1.
+    // This step is not necessary if your part does not support pin muxing.
+    // TODO: change this to select the port/pin you are using.
+    //
+    GPIOPinConfigure(GPIO_PA0_U0RX);
+    GPIOPinConfigure(GPIO_PA1_U0TX);
+
+    //
+    // Enable UART0 so that we can configure the clock.
+    //
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
+
+    //
+    // Use the internal 16MHz oscillator as the UART clock source.
+    //
+    UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
+
+    //
+    // Select the alternate (UART) function for these pins.
+    // TODO: change this to select the port/pin you are using.
+    //
+    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
+
+    //
+    // Initialize the UART for console I/O.
+    //
+    UARTStdioConfig(0, 115200, 16000000);
+}
+
+int temperature_main(){
+      SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_OSC_MAIN|SYSCTL_XTAL_16MHZ);
+      InitConsole();
+        //
+        // This array is used for storing the data read from the ADC FIFO. It
+        // must be as large as the FIFO for the sequencer in use.  This example
+        // uses sequence 3 which has a FIFO depth of 1.  If another sequence
+        // was used with a deeper FIFO, then the array size must be changed.
+        //
+        uint32_t ADCValues[1];
+
+        //
+        // These variables are used to store the temperature conversions for
+        // Celsius and Fahrenheit.
+        //
+        uint32_t TempValueC ;
+        uint32_t TempValueF ;
+
+
+        //
+        // Display the setup on the console.
+        //
+        UARTprintf("ADC ->\n");
+        UARTprintf("  Type: Internal Temperature Sensor\n");
+        UARTprintf("  Samples: One\n");
+        UARTprintf("  Update Rate: 250ms\n");
+        UARTprintf("  Input Pin: Internal temperature sensor\n\n");
+
+        writeLine("ADC ->\n");
+        writeLine("  Type: Internal Temperature Sensor\n");
+        writeLine("  Samples: One\n");
+        writeLine("  Update Rate: 250ms\n");
+        writeLine("  Input Pin: Internal temperature sensor\n\n");
+
+        //
+        // The ADC0 peripheral must be enabled for use.
+        //
+        SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
+        SysCtlDelay(3);//3 wait cycles after enabling a clock on a peripheral
+
+        //
+        // Enable sample sequence 3 with a processor signal trigger.  Sequence 3
+        // will do a single sample when the processor sends a singal to start the
+        // conversion.  Each ADC module has 4 programmable sequences, sequence 0
+        // to sequence 3.  This example is arbitrarily using sequence 3.
+        //
+        ADCSequenceConfigure(ADC0_BASE, 3, ADC_TRIGGER_PROCESSOR, 0);
+
+        //
+        // Configure step 0 on sequence 3.  Sample the temperature sensor
+        // (ADC_CTL_TS) and configure the interrupt flag (ADC_CTL_IE) to be set
+        // when the sample is done.  Tell the ADC logic that this is the last
+        // conversion on sequence 3 (ADC_CTL_END).  Sequence 3 has only one
+        // programmable step.  Sequence 1 and 2 have 4 steps, and sequence 0 has
+        // 8 programmable steps.  Since we are only doing a single conversion using
+        // sequence 3 we will only configure step 0.  For more information on the
+        // ADC sequences and steps, reference the datasheet.
+        //
+        ADCSequenceStepConfigure(ADC0_BASE, 3, 0, ADC_CTL_TS | ADC_CTL_IE |
+                                 ADC_CTL_END);
+
+        //ADCHardwareOversampleConfigure(ADC0_BASE,64);//This will configure to do averages of 64 values (powers of 2, from 2 to 64 )
+
+        //
+        // Since sample sequence 3 is now configured, it must be enabled.
+        //
+        ADCSequenceEnable(ADC0_BASE, 3);
+
+        //
+        // Clear the interrupt status flag.  This is done to make sure the
+        // interrupt flag is cleared before we sample.
+        //
+        ADCIntClear(ADC0_BASE, 3);
+
+        //
+        // Sample the temperature sensor forever.  Display the value on the
+        // console.
+        //
+        while(1)
+        {
+            //
+            // Trigger the ADC conversion.
+            //
+            ADCProcessorTrigger(ADC0_BASE, 3);
+
+            //
+            // Wait for conversion to be completed.
+            //
+            while(!ADCIntStatus(ADC0_BASE, 3, false))
+            {
+            }
+
+            //
+            // Clear the ADC interrupt flag.
+            //
+            ADCIntClear(ADC0_BASE, 3);
+
+            //
+            // Read ADC Value.
+            //
+            ADCSequenceDataGet(ADC0_BASE, 3, ADCValues);
+
+            //
+            // Use non-calibrated conversion provided in the data sheet. I use floats in intermediate
+            // math but you could use intergers with multiplied by powers of 10 and divide on the end
+            // Make sure you divide last to avoid dropout.
+            //
+            TempValueC = (uint32_t)(147.5 - ((75.0*3.3 *(float)ADCValues[0])) / 4096.0);
+
+            //float Temaraure_C = 147.5 - ((75.0*3.3 *(float)ADCValues[0])) / 4096.0;
+
+            //
+            // Get Fahrenheit value.  Make sure you divide last to avoid dropout.
+            //
+            TempValueF = ((TempValueC * 9) + 160) / 5;
+
+            //
+            // Display the temperature value on the console.
+            //
+            UARTprintf("Temperature = %3d*C or %3d*F\r", TempValueC,
+                    TempValueF);
+
+            writeLine("Temperature(Celsius): ");
+            writeFloat(TempValueC);
+
+            //
+            // This function provides a means of generating a constant length
+            // delay.  The function delay (in cycles) = 3 * parameter.  Delay
+            // 250ms arbitrarily.
+            //
+            SysCtlDelay(80000000 / 12);
+
+            ///SysCtlDelay(80000000);
+        }
+
+
+
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.h
new file mode 100644
index 000000000..6368cf744
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/temperature_sensor.h
@@ -0,0 +1 @@
+int temperature_main();
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 475f63795..f568f9816 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -1,3 +1,7 @@
+
+#ifndef MODULES_THREAD_THREAD_H_
+#define MODULES_THREAD_THREAD_H_
+
 #include "PMR/common/MessageContainer.pb-c.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
@@ -41,7 +45,18 @@ typedef struct
     int32_t pulsepermmspring;
     int id;  //HardwareDancerType
 }DancerConfigStruc;
-#define MAX_THREAD_MOTORS_NUM  HARDWARE_MOTOR_TYPE__WinderMotor +1
+
+typedef enum threadMotorsEnum
+{
+    FEEDER_MOTOR,
+    DRYER_MOTOR,
+    POOLER_MOTOR,
+    WINDER_MOTOR,
+    SCREW_MOTOR,
+    MAX_THREAD_MOTORS_NUM
+}threadMotorsEnum;
+
+extern uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
 
 
 extern MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM];
@@ -53,3 +68,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 
 uint32_t DancerConfigMessage(HardwareDancer * request);
+
+
+#endif //MODULES_THREAD_THREAD_H_
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 13c81fb39..89ff63363 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -10,6 +10,9 @@
 #include "../control/pidalgo.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
+#include "drivers/Motors/Motor.h"
+#include "drivers/Heater/TemperatureSensor.h"
+#include "drivers/Heater/Heater.h"
 ////////////////////////////////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
@@ -23,6 +26,9 @@ typedef enum
     Stop
 } ReturnCode;
 
+
+uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
+
 /********************************************************************************************
 * functions describes motor operation flow and movement state during profile execution
 * used to operate in runtime correct profileflow execution
@@ -186,8 +192,9 @@ static ReturnCode PrepareState(void *JobDetails)
         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
-        AddControlCallback(DeviceId2Motor[Motor_i], ThreadControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,Motor_i);
+        AddControlCallback(ThreadControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,Motor_i,0);
     }
+
     //set 3 dancers to the profile positions
     return NextState;
 }
@@ -196,10 +203,10 @@ static ReturnCode PrepareState(void *JobDetails)
 static ReturnCode PreSegmentState(void *JobDetails)
 {
 
-    int Motor_i;
+    TimerMotors_t Motor_i;
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
-        MotorControlConfig[Motor_i].m_SetParam = getMotorFreq(Motor_i);//need to update SetParams on presegment stage
+        MotorControlConfig[Motor_i].m_SetParam = MotorGetSpeed(getMotorId(Motor_i));//need to update SetParams on presegment stage
     }
     // set the new speed in the dryer motor to the speed of the new segment
     // activate control fr all motors
@@ -278,20 +285,9 @@ bool ThreadPrintingIterate(void *JobDetails)
     //    screw_movement[gPrintingState[_motorId]]++;
     ReturnCode returnCode = state_fun(JobDetails);
 
-    /*if ((_motorId == SCREW_MOTOR)&&(pause_active))
-    {
-        tick = UsersysTickGet();
-        len = usnprintf(str, 60, "\r\n PrintingIterate  tick %d state %d retcode %d ",tick, gPrintingState[_motorId],returnCode);
-        cb_push_back (str, len);
-        //SendInterruptMessageToHost(10+gPrintingState[_motorId],returnCode);
-    }*/
     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);
-
-    }
+//    if (keepstate != gPrintingState){
+//    }
 
     //
     // Enable all interrupts.