change motors config, improve initialization and control, first control run

17 files changed, 184 insertions, 96 deletions

diff --git a/Software/Embedded_SW/Embedded/Common/Sys_PinOut_Config/MCU_MAIN_pinout.c b/Software/Embedded_SW/Embedded/Common/Sys_PinOut_Config/MCU_MAIN_pinout.c
index 831bd2047..484c99d1a 100644
--- a/Software/Embedded_SW/Embedded/Common/Sys_PinOut_Config/MCU_MAIN_pinout.c
+++ b/Software/Embedded_SW/Embedded/Common/Sys_PinOut_Config/MCU_MAIN_pinout.c
@@ -75,9 +75,10 @@ void PortFunctionInit(void)
 void
 PinoutSet(void)
 {
-    MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);
+    //MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);
     MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_EPHY0);
     MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI3);
+    MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI2);
     MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C4);
     MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
     MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC1);
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 31216b53d..5c82839f8 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.c
@@ -48,6 +48,8 @@ up
 #include <DataDef.h>
 #include "driverlib/uart.h"
 
+#include "SSI_Comm.h"
+
 #include "drivers/FPGA/FPGA_SSI_Comm.h"
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
index f9c4d87ba..a9b07e4b7 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
@@ -7,13 +7,19 @@
 
 #ifndef DRIVERS_DANCER_SSI_SSI_COMM_H_
 #define DRIVERS_DANCER_SSI_SSI_COMM_H_
+
 typedef enum
 {
-    WINDER_DANCER,
-    POOLER_DANCER,
-    FEEDER_DANCER,
-    NUM_OF_DANCERS
-} DANCER_ENUM;
+    //DANSER_ROTENC:
+    ROTENC_LDANCER1,        // pooler Dancer adjustment
+    ROTENC_LDANCER2,        // winder Dancer adjustment #1
+    ROTENC_LSPARE,
+    ROTENC_RDANCER,         // feeder dancer adjustment
+    ROTENC_RSPARE,
+    //DISPENSER_ROTENC:
+
+    NUM_OF_ROTENC
+}RotEnc_t;
 
 
 void SSI1_Init();
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index 4d4993952..cf596f51e 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -8,6 +8,7 @@
 #include "Drivers/Motors/Motor.h"
 #include "modules/control/millisecTask.h"
 #include "drivers/Heater/TemperatureSensor.h"
+#include "PMR/Hardware/HardwareMotor.pb-c.h"
 
 extern TempSensorResponseStruct TempSensorResponse[MAX_TEMPERATURE_SENSOR_ID];
 
@@ -381,34 +382,113 @@ uint32_t FPGA_MotorConfig(TimerMotors_t _motorId, MotorDriverConfigStruc *MotorC
     if( MillisecWriteToMotor(_motorId, x_HARD_HIZ, 1, NULL) < 0 ) return ERROR;
     temp = x_SET_PARAM | x_ACC;
     temp = temp << 12;
-    temp |= MotorDriverCfg[_motorId].ACC;
+    temp |= (int)(MotorDriverCfg[_motorId].maxchangeslope);
     if( MillisecWriteToMotor(_motorId, temp, 3, NULL) < 0 ) return ERROR;
 
     temp = x_SET_PARAM | x_DEC;
     temp = temp << 12;
-    temp |= MotorDriverCfg[_motorId].DEC;
+    temp |= (int)(MotorDriverCfg[_motorId].maxchangeslope);
     if( MillisecWriteToMotor(_motorId, temp, 3, NULL) < 0 ) return ERROR;
 
     temp = x_SET_PARAM | x_MAX_SPEED;
     temp = temp << 10;
-    temp |= MotorDriverCfg[_motorId].MaxSpeed;
+    temp |= MotorDriverCfg[_motorId].maxfrequency;
     if( MillisecWriteToMotor(_motorId, temp, 3, NULL) < 0 ) return ERROR;
 
-    if(MotorDriverCfg[_motorId].HasMicroStep)
+    if(MotorDriverCfg[_motorId].has_microstep)
     {
         temp = x_SET_PARAM | x_STEP_MODE;
         temp = temp << 8;
-        temp |= !x_SYNC_EN | MotorDriverCfg[_motorId].Microstep | x_SYNC_SEL_1;
+        temp |= !x_SYNC_EN | MotorDriverCfg[_motorId].microstep | x_SYNC_SEL_1;
         if( MillisecWriteToMotor(_motorId, temp, 2, NULL) < 0 ) return ERROR;
     }
 
-    if(MotorDriverCfg[_motorId].HasConfigWord)
+    if(MotorDriverCfg[_motorId].has_configword)
     {
         temp = x_SET_PARAM | x_CONFIG;
         temp = temp << 24;
-        temp |= MotorDriverCfg[_motorId].ConfigWord;
+        temp |= MotorDriverCfg[_motorId].configword;
         if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
     }
+    ///////////////////////////////////////////////////////////////
+
+    if(MotorDriverCfg[_motorId].has_kvalhold)
+    {
+        temp = x_SET_PARAM | x_KVAL_HOLD;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].kvalhold;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_kvalrun)
+    {
+        temp = x_SET_PARAM | x_KVAL_RUN;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].kvalrun;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_kvalacc)
+    {
+        temp = x_SET_PARAM | x_KVAL_ACC;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].kvalacc;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_kvaldec)
+    {
+        temp = x_SET_PARAM | x_KVAL_DEC;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].kvaldec;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+
+    ///////////////////////////////////////////////////////////////
+    if(MotorDriverCfg[_motorId].has_stslp)
+    {
+        temp = x_SET_PARAM | x_ST_SLP;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].stslp;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_intspd)
+    {
+        temp = x_SET_PARAM | x_INT_SPD;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].intspd;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_fnslpacc)
+    {
+        temp = x_SET_PARAM | x_FN_SLP_ACC;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].fnslpacc;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+    if(MotorDriverCfg[_motorId].has_fnslpdec)
+    {
+        temp = x_SET_PARAM | x_FN_SLP_DEC;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].fnslpdec;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+        if(MotorDriverCfg[_motorId].has_overcurrentthreshold)
+    {
+        temp = x_SET_PARAM | x_OCD_TH;
+        temp = temp << 24;
+        temp |= MotorDriverCfg[_motorId].overcurrentthreshold;
+        if( MillisecWriteToMotor(_motorId, temp, 4, NULL) < 0 ) return ERROR;
+    }
+
+    ///////////////////////////////////////////////////////////////
+/*
+  protobuf_c_boolean has_overcurrentthreshold;
+  int32_t overcurrentthreshold;
+  protobuf_c_boolean has_stallthreshold;
+  int32_t stallthreshold;
+  protobuf_c_boolean has_thermalcompensationfactor;
+  int32_t thermalcompensationfactor;
+  protobuf_c_boolean has_lowspeedoptimization;
+  protobuf_c_boolean lowspeedoptimization;
+*/
 return OK;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
index f3e3f802f..8b063668f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.c
@@ -10,6 +10,7 @@
 #include <drivers/FPGA/FPGA_Comm.h>
 #include <DataDef.h>
 #include "drivers/FPGA/FPGA_SSI_Comm.h"
+#include "drivers/Danser_SSI/SSI_Comm.h"
 
 extern unsigned short DancerZeroOffset;
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
index 0a703a060..45e102683 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SSI_Comm.h
@@ -8,18 +8,7 @@
 #ifndef DRIVERS_FPGA_FPGA_SSI_COMM_H_
 #define DRIVERS_FPGA_FPGA_SSI_COMM_H_
 
-typedef enum
-{
-    //DANSER_ROTENC:
-    ROTENC_LDANCER1,        // pooler Dancer adjustment
-    ROTENC_LDANCER2,        // winder Dancer adjustment #1
-    ROTENC_LSPARE,
-    ROTENC_RDANCER,         // feeder dancer adjustment
-    ROTENC_RSPARE,
-    //DISPENSER_ROTENC:
-
-    NUM_OF_ROTENC
-}RotEnc_t;
+#include "drivers/Danser_SSI/SSI_Comm.h"
 
 typedef struct  // 24bit
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index 310bbd2af..86dd570b1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -10,6 +10,7 @@
 #include "include.h"
 //#include "Modules/thread/thread.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
+#include "PMR/Hardware/HardwareMotor.pb-c.h"
 
 #include <stdint.h>
 #include <stdbool.h>
@@ -46,7 +47,7 @@
 //  double hightimeoutusec;
 //}MotorDriverConfigStruc;
 
-typedef struct
+/*typedef struct
 {
   uint32_t ACC; //set
   uint32_t DEC;  //set
@@ -55,19 +56,33 @@ typedef struct
   uint32_t Microstep; //set
   bool HasConfigWord;
   uint16_t ConfigWord; //set
-  uint32_t Min_Speed_LSPD_OPT;
-  bool Has_Min_Speed_LSPD_OPT;
-  uint32_t KVAL_HOLD         ;
-  uint32_t KVAL_RUN          ;
-  uint32_t KVAL_ACC          ;
-  uint32_t KVAL_DEC          ;
-  uint32_t ST_SLP            ;
-  uint32_t INT_SPD           ;
-  uint32_t FN_SLP_ACC        ;
-  uint32_t FN_SLP_DEC        ;
-
+  bool has_kvalhold;
+  int32_t kvalhold;
+  bool has_kvalrun;
+  int32_t kvalrun;
+  bool has_kvalacc;
+  int32_t kvalacc;
+  bool has_kvaldec;
+  int32_t kvaldec;
+  bool has_overcurrentthreshold;
+  int32_t overcurrentthreshold;
+  bool has_stallthreshold;
+  int32_t stallthreshold;
+  bool has_thermalcompensationfactor;
+  int32_t thermalcompensationfactor;
+  bool has_lowspeedoptimization;
+  bool lowspeedoptimization;
+  bool has_stslp;
+  int32_t stslp;
+  bool has_intspd;
+  int32_t intspd;
+  bool has_fnslpacc;
+  int32_t fnslpacc;
+  bool has_fnslpdec;
+  int32_t fnslpdec;
 }MotorDriverConfigStruc;
-
+*/
+#define MotorDriverConfigStruc HardwareMotor
 typedef struct
 {
   bool Direction;
diff --git a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
index efc9b932c..4b3402d7f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SPI/SPI_Comm.c
@@ -14,6 +14,7 @@
 
 //#include "graphics_adapter.h"
 #include "drivers/FPGA/Moters_Driver/L6470.h"
+#include "PMR/Hardware/HardwareMotor.pb-c.h"
 
 //#include "drivers/FPGA/FPGA_Comm.h"
 
@@ -431,11 +432,11 @@ void setup(MotorDriverConfigStruc *MotorConfig)
             }
         }
 
-        if(MotorConfig->HasMicroStep)
+        if(MotorConfig->has_microstep)
         {
             SetParam(x_STEP_MODE,
                 !x_SYNC_EN |
-                MotorConfig->Microstep |
+                MotorConfig->microstep |
                 x_SYNC_SEL_1);
         }
         else
@@ -481,9 +482,9 @@ void setup(MotorDriverConfigStruc *MotorConfig)
             }
         }
 
-    if(MotorConfig->MaxSpeed !=0)
+    if(MotorConfig->maxfrequency !=0)
     {
-        SetParam(x_MAX_SPEED, MaxSpdCalc(MotorConfig->MaxSpeed));
+        SetParam(x_MAX_SPEED, MaxSpdCalc(MotorConfig->maxfrequency));
     }
     else
     {
@@ -568,8 +569,8 @@ void setup(MotorDriverConfigStruc *MotorConfig)
     }
     // ACC + DEC writable only when motor is stopped
 
-    if(MotorConfig->ACC != 0)
-        SetParam(x_ACC, MotorConfig->ACC);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
+    if(MotorConfig->maxchangeslope != 0)
+        SetParam(x_ACC, MotorConfig->maxchangeslope);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
     else
         SetParam(x_ACC, 0x0FF);
 /*    if(request->set_acc)
@@ -593,9 +594,8 @@ void setup(MotorDriverConfigStruc *MotorConfig)
         }
     }
 
-
-    if(MotorConfig->DEC != 0)
-        SetParam(x_DEC, MotorConfig->DEC);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
+    if(MotorConfig->maxchangeslope != 0)
+        SetParam(x_DEC, MotorConfig->maxchangeslope);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
     else
         SetParam(x_DEC, 0x0FF);
 
@@ -610,7 +610,7 @@ void setup(MotorDriverConfigStruc *MotorConfig)
     }
 */
 
-    if(MotorConfig->HasConfigWord)
+    if(MotorConfig->has_configword)
     {
         //while(SSIBusy(SSI2_BASE)){};
         timeout = SSI_SPI_TIMEOUT;
@@ -624,24 +624,27 @@ void setup(MotorDriverConfigStruc *MotorConfig)
             }
         }
 
-        SetParam(x_CONFIG, MotorConfig->ConfigWord);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
+        SetParam(x_CONFIG, MotorConfig->configword);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
     }
 
     // Configure the overcurrent detection threshold. The constants
     //  for this are defined in the L6470.h file.
-    //while(SSIBusy(SSI2_BASE)){};
-    timeout = SSI_SPI_TIMEOUT;
-
-    while(SSIBusy(SSI2_BASE))
+    if(MotorConfig->has_overcurrentthreshold)
     {
-        timeout = timeout--;
-        if(timeout == 0)
+       //while(SSIBusy(SSI2_BASE)){};
+        timeout = SSI_SPI_TIMEOUT;
+
+        while(SSIBusy(SSI2_BASE))
         {
-            return ;
+            timeout = timeout--;
+            if(timeout == 0)
+            {
+                return ;
+            }
         }
+        SetParam(x_OCD_TH, MotorConfig->overcurrentthreshold);
     }
 
-    SetParam(x_OCD_TH, x_OCD_TH_3000mA);
 
     // Set up the CONFIG register as follows:
     //  PWM frequency divisor = 1
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index ee98cd105..f54b06669 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -180,7 +180,7 @@ Init_stubs();
 
     IDLE_TASK_package_init();
 
-    ControlStart();
+    //ControlStart();
 
     BIOS_start();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 12603b5ee..05d582eac 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -29,6 +29,8 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 
+#include "modules/thread/thread_ex.h"
+
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
@@ -89,7 +91,7 @@ void MillisecInit(void)
     MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb);
     for (i=0;i<NUM_OF_MOTORS;i++)
     {
-        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 6, NULL,&eb);
+        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 20, NULL,&eb);
     }
 
     MillisecRestart = false;
@@ -123,6 +125,9 @@ void MillisecStart(void)
 {
     MillisecRestart = true;
     ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
+    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
+    ROM_IntEnable(INT_TIMER1A);
+    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
     ADCAcquireStart(0,1);
 }
 
@@ -291,9 +296,9 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
-    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);
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(ROTENC_RDANCER);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER1);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(ROTENC_LDANCER2);
     if (Hundred_msTick)
     {
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index a56457bdb..08001cf01 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -139,7 +139,7 @@ void ControlInit(void)
     ROM_IntEnable(INT_TIMER0A);
     ROM_TimerIntEnable(Control_timerBase, TIMER_TIMA_TIMEOUT);
 
-    ADCAcquireInit();
+    //ADCAcquireInit();
 
     return;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index b41eca9ac..cd7174d10 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -321,7 +321,7 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
     UploadHardwareConfigurationResponse response = UPLOAD_HARDWARE_CONFIGURATION_RESPONSE__INIT;
 
     UploadHardwareConfigurationRequest* UploadRequest = upload_hardware_configuration_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
-    HardwareConfiguration *request = UploadRequest->hardwareconfiguration;
+    HardwareConfiguration *request =    UploadRequest->hardwareconfiguration;
     if (request->n_winders == 1)
         status += InternalWinderConfigMessage(request->winders);
     for (Motor_i = 0; Motor_i < request->n_motors ; Motor_i++)
@@ -342,7 +342,7 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
         status += DispenserConfigMessage(request->dispensers[Dispenser_i]);
     //status += HeaterConfigSetSharedHeatersParams (request->outputproportionalcycletime, request->outputproportionalsinglestep);
 
-
+    ControlStart();
     ThreadInitialTestStub(request);
 
 
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 fef551748..e7b75cc9d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -179,6 +179,8 @@ 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);
@@ -210,9 +212,6 @@ void Stub_MotorRunRequest(MessageContainer* requestContainer)
     response.motor_id = request->motor_id;
     response.has_motor_id = true;
 #ifdef EVALUATION_BOARD
-
-    uint32_t read_status;
-
     read_status = Get_and_Clear_Status();
 
     response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
@@ -262,7 +261,7 @@ void Stub_MotorRunRequest(MessageContainer* requestContainer)
 
 void Stub_MotorStopRequest(MessageContainer* requestContainer)
 {
-
+    uint32_t read_status;
     //uint32_t status = FAILED;
 
     MessageContainer responseContainer;
@@ -301,9 +300,6 @@ void Stub_MotorStopRequest(MessageContainer* requestContainer)
     response.motor_id = request->motor_id;
     response.has_motor_id = true;
 #ifdef EVALUATION_BOARD
-
-    uint32_t read_status;
-
     read_status = Get_and_Clear_Status();
 
     response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
@@ -541,6 +537,7 @@ void Stub_MotorPositionRequest(MessageContainer* requestContainer)
 
 void Stub_MotorMovRequest(MessageContainer* requestContainer)
 {
+    uint32_t read_status;
 
     MessageContainer responseContainer;
 
@@ -563,23 +560,10 @@ void Stub_MotorMovRequest(MessageContainer* requestContainer)
     response.has_motor_id = true;
 
     #ifdef EVALUATION_BOARD
-
-        uint32_t read_status;
-
         response.position = Get_Param(x_ABS_POS);
         response.has_position = true;
 
-        //while(SSIBusy(SSI2_BASE)){};
-        uint32_t timeout = SSI_SPI_TIMEOUT;
-
-        while(SSIBusy(SSI2_BASE))
-        {
-            timeout = timeout--;
-            if(timeout == 0)
-            {
-                return ERROR;
-            }
-        }
+        while(SSIBusy(SSI2_BASE)){};
 
         read_status = Get_and_Clear_Status();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 7690b131f..0bda4964b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -15,6 +15,7 @@
 #include "drivers/Motors/Motor.h"
 #include "drivers/Danser_SSI/ssi_comm.h"
 
+#include "thread_ex.h"
 
 typedef struct
 {
@@ -37,6 +38,7 @@ typedef enum threadMotorsEnum
     SCREW_MOTOR,
     MAX_THREAD_MOTORS_NUM
 }threadMotorsEnum;
+
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 5f49c7d10..3fb78d2f0 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -1,6 +1,15 @@
 
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
+
+typedef enum
+{
+    WINDER_DANCER,
+    POOLER_DANCER,
+    FEEDER_DANCER,
+    NUM_OF_DANCERS
+} DANCER_ENUM;
+
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 5a8c3df76..0e276fe92 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -51,7 +51,6 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
 {
     uint32_t status = PASSED;
     TimerMotors_t Motor_i;
-    MotorDriverConfigStruc MotorDriverConfig;
     Motor_i = request->hardwaremotortype;
     /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
     {
@@ -69,15 +68,8 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     //if (Motor_i< MAX_THREAD_MOTORS_NUM)
     //{
         memcpy (&MotorsCfg[Motor_i],request,sizeof(HardwareMotor));
-        MotorDriverConfig.ACC = MotorsCfg[Motor_i].maxchangeslope;
-        MotorDriverConfig.DEC = MotorsCfg[Motor_i].maxchangeslope;
-        MotorDriverConfig.MaxSpeed = MotorsCfg[Motor_i].maxfrequency;
-        MotorDriverConfig.HasMicroStep = MotorsCfg[Motor_i].has_microstep;
-        MotorDriverConfig.Microstep = MotorsCfg[Motor_i].microstep;
-        MotorDriverConfig.HasConfigWord = MotorsCfg[Motor_i].has_configword;
-        MotorDriverConfig.ConfigWord = MotorsCfg[Motor_i].configword;
 
-        status = MotorConfig( Motor_i,  &MotorDriverConfig);
+        status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
 //        if (Motor_i == MOTOR_RDRIVING)
 //            ThreadInitialTestStub(request);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 0e68a6406..434ac0f21 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -216,7 +216,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-if (index != 0) return OK;
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);