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

12 files changed, 151 insertions, 105 deletions

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 657b444f2..f9c4d87ba 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Danser_SSI/SSI_Comm.h
@@ -9,9 +9,9 @@
 #define DRIVERS_DANCER_SSI_SSI_COMM_H_
 typedef enum
 {
-    FEEDER_DANCER,
-    POOLER_DANCER,
     WINDER_DANCER,
+    POOLER_DANCER,
+    FEEDER_DANCER,
     NUM_OF_DANCERS
 } DANCER_ENUM;
 
diff --git a/Software/Embedded_SW/Embedded/Embedded.cfg b/Software/Embedded_SW/Embedded/Embedded.cfg
index 70158a688..11cc80465 100644
--- a/Software/Embedded_SW/Embedded/Embedded.cfg
+++ b/Software/Embedded_SW/Embedded/Embedded.cfg
@@ -100,6 +100,7 @@ Program.global.adcProcess = Task.create("&ADCProcessTask", task0Params);
 var task2Params = new Task.Params();
 task2Params.instance.name = "communication";
 task2Params.priority = 4;
+task2Params.stackSize = 8192;
 Program.global.communication = Task.create("&communicationTask", task2Params);
 
 var task3Params = new Task.Params();
diff --git a/Software/Embedded_SW/Embedded/Include.h b/Software/Embedded_SW/Embedded/Include.h
index 699f20a98..bfacbff4a 100644
--- a/Software/Embedded_SW/Embedded/Include.h
+++ b/Software/Embedded_SW/Embedded/Include.h
@@ -54,6 +54,8 @@
 
 #define SYS_CLK_FREQ                120000000
 
+#define PI 3.1415926
+
 typedef  uint32_t (* callback_fptr)(uint32_t deviceID, uint32_t ReadValue);
 
 extern void SysTickHandler(void);
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 5f8f4750d..b7f926da6 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -328,18 +328,25 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
     for (Dancer_i = 0; Dancer_i < request->n_dancers ; Dancer_i++)
         status += DancerConfigMessage(request->dancers[Dancer_i]);
     for (PID_i = 0; PID_i < request->n_pidcontrols ; PID_i++)
-        status += HeaterConfigRequestMessage(request->pidcontrols[PID_i]);
+    {
+        if (request->pidcontrols[PID_i]->hardwarepidcontroltype <= HARDWARE_PID_CONTROL_TYPE__MixerHeater)
+            status += HeaterConfigRequestMessage(request->pidcontrols[PID_i]);
+        else if (request->pidcontrols[PID_i]->hardwarepidcontroltype >= HARDWARE_PID_CONTROL_TYPE__MotorDryer)
+            status += MotorPidRequestMessage(request->pidcontrols[PID_i]);
+    }
     for (Dispenser_i = 0; Dispenser_i < request->n_dispensers ; Dispenser_i++)
         status += DispenserConfigMessage(request->dispensers[Dispenser_i]);
     //status += HeaterConfigSetSharedHeatersParams (request->outputproportionalcycletime, request->outputproportionalsinglestep);
 
 
+    ThreadInitialTestStub(request);
 
 
     responseContainer = createContainer(MESSAGE_TYPE__UploadHardwareConfigurationResponse, requestContainer->token, true, &response, &upload_hardware_configuration_response__pack, &upload_hardware_configuration_response__get_packed_size);
     uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     free(responseContainer.data.data);
+    upload_hardware_configuration_request__free_unpacked(UploadRequest,NULL);
     SendChars(container_buffer, container_size);
     //free(container_buffer);
     free(requestContainer);
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index fdfcdd92a..dfc00834e 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -154,7 +154,7 @@ void ProcessRequestFunc(MessageContainer* requestContainer)
 
     UploadProcessParametersResponse response = UPLOAD_PROCESS_PARAMETERS_RESPONSE__INIT;
 
-    responseContainer = createContainer(MESSAGE_TYPE__UploadProcessParametersResponse, requestContainer->token, true, &response, &upload_process_parameters_request__pack, &upload_process_parameters_request__get_packed_size);
+    responseContainer = createContainer(MESSAGE_TYPE__UploadProcessParametersResponse, requestContainer->token, true, &response, &upload_process_parameters_response__pack, &upload_process_parameters_response__get_packed_size);
     container_buffer = malloc(message_container__get_packed_size(&responseContainer));
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index 1d770acce..f908e69fe 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -279,6 +279,7 @@ uint32_t HeaterConfigRequestMessage(HardwarePidControl* request)
         }
         return OK;
     }
+
     return ERROR;
 
 }
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 b82bfa283..de3587ff2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -118,12 +118,7 @@ void Stub_MotorInitRequest(MessageContainer* requestContainer)
     MotorConfig.hardwaremotortype = request->motor_id;
     MotorConfig.minfrequency = 0;
     MotorConfig.maxfrequency = request->max_speed;
-    MotorConfig.kp = 5000;
-    MotorConfig.ki = 100;
-    MotorConfig.kd = 0;
-    MotorConfig.minmicrostep = request->micro_steps;
-    MotorConfig.maxmicrostep = request->micro_steps;
-    MotorConfig.ratiotodryerspeed = 1;
+    MotorConfig.microstep = request->micro_steps;
     MotorsConfigMessage(&MotorConfig);
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index d2675452c..c737b9ea8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -8,6 +8,7 @@
 #include "PMR/Hardware/HardwareDancer.pb-c.h"
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 #include "PMR/Hardware/HardwareWinder.pb-c.h"
+#include "PMR/Hardware/HardwarePidControl.pb-c.h"
 #include "PMR/Printing/JobSpool.pb-c.h"
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
@@ -17,23 +18,6 @@
 
 typedef struct
 {
-  HardwareMotorType id;
-  uint32_t minfreq;
-  uint32_t maxfreq;
-  uint32_t minmicrostep;
-  uint32_t maxmicrostep;
-  double linearratio;
-  uint32_t medianposition;
-  double correctiongain;
-  double ratio2dryerspeed;
-  double kp;
-  double ki;
-  double kd;
-  double changeslope;
-  double hightimeoutusec;
-}MotorConfigStruc;
-typedef struct
-{
     uint32_t startoffsetpulses;
     uint32_t spoolbackingrate;
     uint32_t segmentoffsetpulses;// the spool winding initial length in mm
@@ -43,14 +27,6 @@ typedef struct
     double diameter;
 }InternalWinderConfigStruc;
 
-typedef struct
-{
-    bool fixorgradual;
-    double k;
-    double x;
-    int32_t pulsepermmspring;
-    int id;  //HardwareDancerType
-}DancerConfigStruc;
 
 typedef enum threadMotorsEnum
 {
@@ -62,17 +38,28 @@ typedef enum threadMotorsEnum
     MAX_THREAD_MOTORS_NUM
 }threadMotorsEnum;
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
+#define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
+
 extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
 extern DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
+extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM];
+
+extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
+extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
+extern InternalWinderConfigStruc    InternalWinderCfg;
+extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 
-extern MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM];
-extern InternalWinderConfigStruc InternalWinderCfg;
+#define MAX_CONTROL_SAMPLES 6
+extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
+extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
 uint32_t MotorsConfigMessage(HardwareMotor * request);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadInitialTestStub(HardwareMotor * request);
+uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 
 uint32_t DancerConfigMessage(HardwareDancer * request);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index eacdd2ed9..da0844c3c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -13,6 +13,8 @@ bool Winder_ScrewHoming = false;
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
 uint32_t Winder_PrepareStage2(void);
 
+double ScrewSpeed = 0;
+
 uint32_t Winder_Init(void)
 {
     return OK;
@@ -20,6 +22,8 @@ uint32_t Winder_Init(void)
 uint32_t Winder_Prepare(void)
 {
     uint32_t status = 0;
+    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
      * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
@@ -34,7 +38,7 @@ uint32_t Winder_Prepare(void)
     {
         Winder_ScrewHoming = true;
         status = MotorSetDirection(MOTOR_SCREW,1);//make sur to move the cart home
-        status |=  MotorSetSpeed(MOTOR_SCREW, MotorsCfg[SCREW_MOTOR].maxfreq, 8);
+        status |=  MotorSetSpeed(MOTOR_SCREW, ScrewSpeed, MotorsCfg[SCREW_MOTOR].microstep);
     }
     return status;
 }
@@ -62,10 +66,11 @@ uint32_t Winder_Presegment(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
     int process_speed = JobTicket->processparameters->dyeingspeed;
-    int dryer_speed = process_speed * MotorsCfg[DRYER_MOTOR].ratio2dryerspeed; //set dryer_speed_translation_here
     float screw_speed = 0;
     float RotationsPerSecond;
 
+    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+
 /*typedef struct
 {
     uint32_t startoffsetpulses;
@@ -92,7 +97,7 @@ uint32_t Winder_Presegment(void *JobDetails)
 //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
 
     //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back.
-     MotorSetSpeedWithCallback (MOTOR_SCREW, screw_speed, MotorsCfg[SCREW_MOTOR].minmicrostep,WinderPresegmentReady);
+     MotorSetSpeedWithCallback (MOTOR_SCREW, screw_speed, MotorsCfg[SCREW_MOTOR].microstep,WinderPresegmentReady);
     //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 9b371ae07..c1d591a38 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -6,6 +6,7 @@ uint32_t ThreadSegmentState(void *JobDetails);
 uint32_t ThreadEndState(void *JobDetails);
 uint32_t ThreadInitialTestStub();
 
+//uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 7119238de..c9876f89c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -11,10 +11,16 @@
 #include "PMR/common/MessageContainer.pb-c.h"
 
 #include "thread.h"
-#define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
-MotorConfigStruc  MotorsCfg[MAX_THREAD_MOTORS_NUM]={0};
+HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
+HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
+
+uint32_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};
-DancerConfigStruc DancersCfg[MAX_SYSTEM_DANCERS] = {0};
+HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
+
+HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};
 
 
 
@@ -33,8 +39,8 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
     InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
-    InternalWinderCfg.SpoolBottomBackingRate =  request->spoolbottombackingrate;
-    InternalWinderCfg.NumberOfRotationPerPassage = request->numberofrotationperpassage;
+    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
+    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
     InternalWinderCfg.diameter = request->diameter;
 
     return status;
@@ -47,50 +53,63 @@ uint32_t MotorsConfigMessage(HardwareMotor * request)
     int Motor_i;
     MotorDriverConfigStruc MotorDriverConfig;
     Motor_i = request->hardwaremotortype;
-    if (Motor_i< MAX_THREAD_MOTORS_NUM)
+    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
     {
-        MotorsCfg[Motor_i].id               = request->hardwaremotortype;
-        MotorsCfg[Motor_i].minfreq          = request->minfrequency;
-        MotorsCfg[Motor_i].maxfreq          = request->maxfrequency;
-        MotorsCfg[Motor_i].minmicrostep     = request->minmicrostep;
-        MotorsCfg[Motor_i].maxmicrostep     = request->maxmicrostep;
-        MotorsCfg[Motor_i].linearratio      = request->linearratio;
-        MotorsCfg[Motor_i].medianposition   = request->medianposition;
-        MotorsCfg[Motor_i].correctiongain   = request->correctiongain;
-        MotorsCfg[Motor_i].ratio2dryerspeed  = request->ratiotodryerspeed;
-        MotorsCfg[Motor_i].kp               = request->kp;
-        MotorsCfg[Motor_i].ki               = request->ki;
-        MotorsCfg[Motor_i].kd               = request->kd;
-        MotorsCfg[Motor_i].changeslope      = request->changeslope;
-        MotorsCfg[Motor_i].hightimeoutusec  = request->highlengthmicrosecond;
-        if (Motor_i == MOTOR_LDRIVING)
-            ThreadInitialTestStub(request);
-        /*
-        typedef struct
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
         {
-              uint32_t ACC; //set
-              uint32_t DEC;  //set
-              uint32_t MaxSpeed; //set
-              bool HasMicroStep;
-              uint32_t Microstep; //set
-              bool HasConfigWord;
-              uint16_t ConfigWord; //set
-        }MotorDriverConfigStruc;
-        */
-        MotorDriverConfig.ACC = MotorsCfg[Motor_i].changeslope;
-        MotorDriverConfig.DEC = MotorsCfg[Motor_i].changeslope;
-        MotorDriverConfig.MaxSpeed = MotorsCfg[Motor_i].maxfreq;
-        MotorDriverConfig.HasMicroStep = true;
-        MotorDriverConfig.Microstep = MotorsCfg[Motor_i].minmicrostep;
-        MotorDriverConfig.HasConfigWord = false;
-        MotorDriverConfig.ConfigWord = 0;
+            Motor_i = i;
+            break;
+        }
+    }
+    if (Motor_i == 0)
+    {
+        LOG_ERROR(request->hardwarepidcontroltype,"ERROR Control Id");
+        return ERROR;
+    }*/
+    //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,  &MotorDriverConfig);
+//        if (Motor_i == MOTOR_RDRIVING)
+//            ThreadInitialTestStub(request);
 
         return status;
+//    }
+//    else return Motor_i;
+
+}
+uint32_t MotorPidRequestMessage(HardwarePidControl* request)
+{
+    int Motor_i,i;
+    int temp;
+    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    {
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
+        {
+            Motor_i = i;
+            break;
+        }
     }
-    else return Motor_i;
+    memcpy (&MotorsControl[Motor_i],request,sizeof(HardwarePidControl));
+    if (MotorsControl[Motor_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
+        MotorsControl[Motor_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+    for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+        MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
+    NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+    temp=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
+    NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
+//    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 
+    return OK;
 }
 uint32_t DancerConfigMessage(HardwareDancer * request)
 {
@@ -100,11 +119,7 @@ uint32_t DancerConfigMessage(HardwareDancer * request)
     Dancer_i = request->hardwaredancertype;
     if (Dancer_i<MAX_SYSTEM_DANCERS )
     {
-        DancersCfg[Dancer_i].id = Dancer_i;
-        DancersCfg[Dancer_i].fixorgradual = request->gradual;
-        DancersCfg[Dancer_i].k = request->k;
-        DancersCfg[Dancer_i].x = request->x;
-        DancersCfg[Dancer_i].pulsepermmspring = request->pulsepermmspring;
+        memcpy (&DancersCfg[Dancer_i],request,sizeof(HardwareDancer));
         return status;
     }
     else
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 7bc38c936..1eb49ef6d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -28,6 +28,8 @@
 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};
 
+int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
+
 typedef struct
 {
     bool                m_isEnabled;
@@ -81,25 +83,42 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 
  return OK;
 }
-uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
+uint32_t ThreadControlCBFunction(uint32_t DancerId, uint32_t ReadValue)
 {
+//#define MAX_CONTROL_SAMPLES 6
+//extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+//extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
+
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
-    uint32_t TranslatedReadValue = ReadValue - 8740;
+    int Pid_Id;
+    int32_t TranslatedReadValue, avreageSampleValue = 0;
+    double NormalizedError;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == deviceID)
+        if (ThreadMotorIdToDancerId[i] == DancerId)
         {
             index = i;
             break;
         }
     if (index==MAX_THREAD_MOTORS_NUM)
     {
-        LOG_ERROR (deviceID, "No motor  for device");
+        LOG_ERROR (DancerId, "No motor  for device");
         return 0xFFFFFFFF;
     }
+
     if(MotorControlConfig[index].m_isEnabled )
     {
-        MotorControlConfig[index].m_mesuredParam = TranslatedReadValue;
+        Pid_Id = ThreadMotorIdToControlId[index];
+        TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+        MotorSamples[index][MotorSamplePointer[index]] = 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,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
@@ -111,7 +130,7 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }
 
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].minmicrostep);
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].microstep);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
 
@@ -140,16 +159,17 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
-    int Motor_i, HW_Motor_Id;
+    int Motor_i, HW_Motor_Id, Pid_Id;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
-            MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq;
-            MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq;
-            MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd;
-            MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp;
-            MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].ki;
+        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.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.dt = eOneMillisecond;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
@@ -158,12 +178,14 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
             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
+
+            MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
-            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            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);
-            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
+            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);
     #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
@@ -184,13 +206,23 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-//    int process_speed = JobTicket->processparameters->dyeingspeed;
-    int process_speed = 500;
+    int Motor_i, HW_Motor_Id;
+
+    int process_speed = JobTicket->processparameters->dyeingspeed;
+
+    process_speed = 10; //debug
 
-    int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here
+    for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+    {
+        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);
+
+        OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed;
+    }
 
     // set the new speed in the dryer motor to the speed of the new segment
-    MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed,  MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep);
+    MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[MOTOR_DRYER_DRIVING].microstep);
     MotorSetSpeed(MOTOR_RLOADING, 1, 1);
     MotorSetSpeed(MOTOR_LLOADING, 1,1);