Control fixes (Encoders, Motors and more) and FPGA preparations

8 files changed, 128 insertions, 46 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index e0abf6c2b..4e16a5599 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -17,6 +17,7 @@
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
 
+#include "MillisecTask.h"
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 
@@ -31,6 +32,15 @@
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
 /******************** STRUCTURES AND ENUMs  ********************************************/
+typedef struct MillisecMotorData
+{
+    bool Active;
+    bool WaitForData;
+    bool DataRequired;
+    MSecFptr Callback;
+    unsigned long Data;
+    int Length;
+}MillisecMotorDataStruc;
 
 typedef enum
 {
@@ -52,6 +62,8 @@ uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
 bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
+MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
+
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
 bool                    MillisecRestart;
@@ -75,7 +87,11 @@ void MillisecInit(void)
 
     memset(MillisecDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
 
-
+    int i;
+    for (i=0;i<NUM_OF_MOTORS;i++)
+    {
+        MotorData[i].Active = false;
+    }
     gateMillisecDB = GateMutex_create(NULL, &eb);
     if (gateMillisecDB == NULL)
     {
@@ -133,9 +149,33 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     ROM_IntMasterEnable();
     return ;
 }
-
+//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
+int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (MotorId >= NUM_OF_MOTORS) return -1;
+    if (MotorData[MotorId].Active == true) return -2;
+    MotorData[MotorId].Callback = Callback;
+    MotorData[MotorId].Data = Data;
+    MotorData[MotorId].Length = Length;
+    MotorData[MotorId].Active = true;
+    MotorData[MotorId].DataRequired = false;
+    return OK;
+}
+int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (MotorId >= NUM_OF_MOTORS) return -1;
+    if (MotorData[MotorId].Active == true) return -2;
+    MotorData[MotorId].Callback = Callback;
+    MotorData[MotorId].Data = Data;
+    MotorData[MotorId].Length = Length;
+    MotorData[MotorId].Active = true;
+    MotorData[MotorId].DataRequired = true;
+    return OK;
+}
 uint32_t MillisecLoop(uint32_t tick)
 {
+    int Motor_i;
+    unsigned int MotorInfo = 0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
@@ -145,7 +185,37 @@ uint32_t MillisecLoop(uint32_t tick)
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
 
     //gather Motor data from FPGA
-    MotorSpeed_Data[MOTOR_DRYER_DRIVING] = MotorGetSpeedFromFPGA(MOTOR_DRYER_DRIVING);
+    for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
+    {
+        if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
+        {
+            if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA
+            {
+                MotorData[Motor_i].WaitForData = false;
+                MotorData[Motor_i].Callback(Motor_i,MotorInfo);
+            }
+        }
+        if (MotorData[Motor_i].Active == true) //new data to send
+        {
+            if (MotorSendFPGARequest(Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
+            {
+                MotorData[Motor_i].Active = false; //set the Active to false first, because the callback might send a new request immediately
+                if (MotorData[Motor_i].DataRequired == true)
+                {
+                    MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
+                }
+                else
+                {
+                    MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
+                }
+            }
+        }
+    }
+
+#ifndef EVALUATION_BOARD
+    /* this cannot be done within one millisecond, and not needed
+     * instead, check if there is a motor waiting with data to send or read request
+     * 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);
@@ -154,11 +224,11 @@ uint32_t MillisecLoop(uint32_t tick)
     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);
+    MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);*/
     //gather Dancer data from FPGA
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-    //Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
-    //Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER,0);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER,0);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER,0);
     //gather data from FPGA
     if (Ten_msTick)
     {
@@ -189,6 +259,7 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
+#endif
     //gather data from FPGA
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 0c9d66201..fde3a36b3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -13,13 +13,13 @@
 #include "drivers/Motors/Motor.h"
 #include "drivers/Heater/TemperatureSensor.h"
 
+typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
+int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
+
 
 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 bfbdec3bb..6bf238507 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -263,27 +263,27 @@ uint32_t ControlLoop(uint32_t tick)
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( 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);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( 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);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( 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);
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     }
                     break;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index f443a10d7..23bd13231 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -1,6 +1,8 @@
 /************************************************************************************************************************
  * control.c
  **************************************************************************************************************************/
+#ifndef MODULES_CONTROL_CONTROL_H_
+#define MODULES_CONTROL_CONTROL_H_
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -10,7 +12,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 Parameter1, uint32_t Parameter2);
+typedef  uint32_t (* DataReadCBFunction)( uint32_t Parameter1, uint32_t Parameter2);
 typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
@@ -28,3 +30,4 @@ int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1, uint32_t Parameter2);
 
 
+#endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index c737b9ea8..b31c74830 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -50,7 +50,7 @@ extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 
 #define MAX_CONTROL_SAMPLES 6
-extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 
@@ -64,7 +64,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
 uint32_t ThreadPrepareState(void *JobDetails);
-
+uint32_t Winder_Prepare(void);
 
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index da0844c3c..6a05b83ce 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -105,16 +105,21 @@ uint32_t Winder_Presegment(void *JobDetails)
 uint32_t Winder_End(void)
 {
     //stop screw
-    return StopMotor (MOTOR_SCREW,1);
+    return StopMotor (MOTOR_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        StopMotor(MOTOR_SCREW,3); //stop ASAP
+        StopMotor(MOTOR_SCREW,Hard_Stop); //stop ASAP
     }
-    status = MotorSetDirection(MOTOR_SCREW,0);//make sur to move the cart out
+    status = MotorSetDirection(MOTOR_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
+}
+void Winder_ScrewOutLimitSwitchInterrupt(void)
+{
+    uint32_t status;
+    status = MotorSetDirection(MOTOR_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
 }
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 8c4b83ec6..1727c650a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -14,7 +14,7 @@
 HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
-uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 07fecf313..fb9740dec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -27,14 +27,14 @@
 
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
 DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
-
+uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
 typedef struct
 {
     bool                m_isEnabled;
-    uint32_t            m_SetParam;
-    uint32_t            m_mesuredParam;
+    int32_t             m_SetParam;
+    float               m_mesuredParam;
     float               m_preError;
     float               m_integral;
     float               m_calculatedError;
@@ -54,7 +54,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == deviceID)
+        if (ControlIdtoMotorId[i] == deviceID)
         {
             index = i;
             break;
@@ -83,7 +83,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 
  return OK;
 }
-uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue)
+uint32_t ThreadControlCBFunction(uint32_t MotorId, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
@@ -91,46 +91,47 @@ uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue)
 
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
-    int Pid_Id;
+    int Pid_Id,DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
     double NormalizedError;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == DancerId)
+        if (ControlIdtoMotorId[i] == MotorId)
         {
             index = i;
             break;
         }
     if (index==MAX_THREAD_MOTORS_NUM)
     {
-        LOG_ERROR (DancerId, "No motor  for device");
+        LOG_ERROR (MotorId, "No motor  for device");
         return 0xFFFFFFFF;
     }
 
     if(MotorControlConfig[index].m_isEnabled )
     {
         Pid_Id = ThreadMotorIdToControlId[index];
+        DancerId = ThreadMotorIdToDancerId[index];
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;
+        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-
-        MotorControlConfig[index].m_mesuredParam = avreageSampleValue;
-        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
+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);
-        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
+        /*if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
         }
         if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
-        }
+        }*/
 
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].microstep);
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index],  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
 
@@ -165,11 +166,12 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
-            MotorControlConfig[Motor_i].m_params.MAX = MotorsControl[Pid_Id].outputproportionalpowerlimit*OriginalMotorSpd_2PPS[Motor_i];
-            MotorControlConfig[Motor_i].m_params.MIN = 0;
+            MotorControlConfig[Motor_i].m_params.MAX = 1;
+            MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
             MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
+            MotorControlConfig[Motor_i].m_params.epsilon = 0.01;
             MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
@@ -181,23 +183,25 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 
             MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
-            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
             else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
     #else
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 continue;
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i);
             else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
     #endif
     }
     Winder_Prepare();
     //set 3 dancers to the profile positions
-    ControlStart();
+
     return OK;
 }
 
@@ -217,11 +221,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         //(Speed*uStep*PPR)/((2*PI*motor_Radius)
         double motor_speed = (process_speed *  MotorsCfg[HW_Motor_Id].pulseperround *  MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
-
+        //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed;
-        MotorControlConfig[Motor_i].m_SetParam = motor_speed;
     }
-
+    ControlStart();
     // set the new speed in the dryer motor to the speed of the new segment
     MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[MOTOR_DRYER_DRIVING].microstep);
     MotorSetSpeed(MOTOR_RLOADING, 1, 1);