merge tpf

6 files changed, 194 insertions, 52 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 638611c8b..97f3811c7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -41,8 +41,8 @@ typedef struct
 }MotorControlConfig_t;
 
 
-#define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
-#define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
+#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 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
@@ -64,6 +64,8 @@ extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
 extern    MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
+extern int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index ff319918e..ab5075ab9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -78,6 +78,13 @@
     //RUN MOTOR A FULL CYCLE
     //RUN A MOTOR NUMBER OF STEPS
     //RUN CONTROL FOR A SINGLE DANCER
+    bool ThreadLoadingActive(void)
+    {
+        if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END))
+            return true;
+        else
+            return false;
+    }
     uint32_t Thread_Load_Init(void)
     {
         void* buffer = NULL;
@@ -200,6 +207,7 @@
     }
     uint8_t CallbackCounter = 0;
     uint8_t TimeoutsCounter = 0;
+
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
     {
         Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
@@ -222,6 +230,13 @@
                 else
                 {
                     LoadStages++;
+                    if (LoadStages == THREAD_LOAD_LIFT_ROCKERS)
+                    {
+                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
+                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
+                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
+                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
+                    }
                     if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
                     {
                         //ThreadLoadStateMachine(LoadStages);
@@ -237,9 +252,9 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000);
         return OK;
     }
     uint32_t Thread_Load_Open_Covers(void)
@@ -270,10 +285,15 @@
     //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1);
+        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
+        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 35);
+        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 35);
+        Task_sleep(10);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 50,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,12000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 50,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000);
 
         return OK;
     }
@@ -357,6 +377,7 @@
     {
         Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id");
+        MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,0,0);
         if(ControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 0277d1daa..df88db3f9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -40,8 +40,8 @@ void ScrewsStartControlTimer (void);
 bool     Winder_ScrewHoming = false;
 bool     ScrewCurrentDirection = false;  //holds current screw direction
 uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
-uint32_t ScrewChangeCounter = 0;
-uint32_t ScrewChangeLimit = 0;
+//uint32_t ScrewChangeCounter = 0;
+//uint32_t ScrewChangeLimit = 0;
 uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
 uint16_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
 uint16_t WinderMotorSpeedCounter = 0;
@@ -54,6 +54,9 @@ uint32_t ScrewControlId = 0xFF;
 
 InternalWinderConfigStruc InternalWinderCfg = {0};
 
+uint32_t ScrewLocationLimitSwitch = 0,ScrewLocationStart = 0;
+uint32_t ScrewLocationRun[3];
+
 uint32_t Winder_Init(void)
 {
     ScrewTimerInterruptInit();
@@ -77,7 +80,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
     InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
-    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
+    InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*2;//request->rotationsperpassage;
     InternalWinderCfg.diameter = request->diameter;
 
     return status;
@@ -109,6 +112,10 @@ uint32_t Winder_Prepare(void)
         AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
         return ERROR;
     }*/
+
+    ScrewLocationRun[0] = 0;
+    ScrewLocationRun[1] = 0;
+
     if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
     {
         //REPORT_MSG(LIMIT, "Winder_Prepare at limit");
@@ -136,9 +143,22 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
-    REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
+    //Read_Screw_Encoder();
+    //ScrewLocationLimitSwitch = Screw_RotEnc.Position;
+    //REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
+
 
-    status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000);
+    REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
+    if (ReadValue != LIMIT)
+    {
+        LOG_ERROR(ReadValue,"Screw failed to reach the limit switch!!");
+        //returned with a timeout
+        Winder_ScrewAtOffsetCallback(0,0);
+    }
+    else
+    {
+        status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000);
+    }
      //set motor location 0 here
         return status;
 
@@ -149,7 +169,13 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
 
+    //Read_Screw_Encoder();
+    //ScrewLocationStart = Screw_RotEnc.Position;
+
+    REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
+
     ScrewCurrentDirection = false;
+    ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
     ScrewSpeed = 0;
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
@@ -190,6 +216,8 @@ char ScrewStr[100];
 //char TempScrewStr[100];
 double WinderReferenceSpeed=0;
 int32_t  TotalWinderSpeed=0;
+bool Add100 = false;
+double Rotations = 6.0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
@@ -198,14 +226,18 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     double RotationsPerSecond;
     int32_t Averagewinderspeed = 0;
 
-    ScrewChangeCounter++;
-    if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
+    //ScrewChangeCounter++;
+    //if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
     {
         TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
         WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
         TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
         {
+            if (WinderMotorSpeedRollOver == false)
+            {
+                Add100 = true;
+            }
             WinderMotorSpeedCounter=0;
             WinderMotorSpeedRollOver=true;
         }
@@ -216,13 +248,23 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
+    //REPORT_MSG(ScrewLocationRun[1] - ScrewLocationRun[0], "Screw Run NumberOfSteps");
+    //usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]));
+    //usnprintf(ScrewStr, 100, "Winder Encoder: 0 %d 1 %d diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],ScrewLocationRun[1] - ScrewLocationRun[0]);
+    //Report(ScrewStr,__FILE__,__LINE__,ScrewLocationLimitSwitch,RpWarning,ScrewLocationStart, 0);
 
     if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
     {
+        if (Add100 == true) //once per job
+        {
+            ScrewNumberOfSteps += 100;
+            Add100 = false;
+        }
+
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
-            //REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
+            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
 
         }
     }
@@ -231,7 +273,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
             ScrewNumberOfSteps++;
-            //REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
+            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
         }
     }
     if (WinderMotorSpeedRollOver)
@@ -246,7 +288,9 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
         WinderReferenceSpeed = Averagewinderspeed;
     }
-    screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
+    screw_horizontal_speed = ScrewNumberOfSteps /  Rotations;//InternalWinderCfg.NumberOfRotationPerPassage;
+    if (Rotations > 7.0)
+        Rotations = 6.0;
     RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     //ROM_IntMasterDisable();
@@ -266,7 +310,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         //Report(TempScrewStr,__FILE__,__LINE__,0,RpWarning,0, 0);
         //Report(ScrewStr,__FILE__,__LINE__,ScrewCurrentDirection,RpWarning,CalculationDirectionChangeCounter, 0);
         //REPORT_MSG(temp , "new winder speed");
-        Report("new winder speed",__FILE__,ScrewNumberOfSteps,temp,RpWarning,ScrewSpeed,0);
+        //Report("new winder speed",__FILE__,ScrewNumberOfSteps,temp,RpWarning,ScrewSpeed,0);
     }
 
     /********************************************************************************/
@@ -280,7 +324,7 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
 
-uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
+uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
 {
     //JobTicket* JobTicket = JobDetails;
     double screw_horizontal_speed = 0;
@@ -300,7 +344,9 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         //     * calculate
         //     * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
         //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
-        screw_horizontal_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
+        ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+        ScrewNumberOfSteps -= 100;
+        screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
         // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
         //RotationsPerSecond = dyeingspeed / (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulleyradius * PI);
         RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
@@ -321,9 +367,8 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     //     * 3. calculate cart travel length from winding parameters
     //     * 4. start move of travel length
     //     * 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
-        ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
         temp = SYS_CLK_FREQ;
-        temp *= InternalWinderCfg.segmentoffsetpulses;
+        temp *= ScrewNumberOfSteps;
         temp /= ScrewSpeed;
         ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed;
         REPORT_MSG(ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps");
@@ -341,6 +386,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         WinderMotorSpeedCounter=0;
         TotalWinderSpeed = 0;
         WinderMotorSpeedRollOver=false;
+        Add100 = false;
    }
     PreSegmentReady(Module_Winder,ModuleDone);
 
@@ -358,8 +404,8 @@ uint32_t Winder_End(void)
     CurrentControlledSpeed[SCREW_MOTOR] = 0;
     ScrewsStopControlTimer();
     //move the cart to the edge so the spool can be easily replaced
-    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
-
+    //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
+    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
     return OK;
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
@@ -410,18 +456,22 @@ void ScrewsStartControlTimer (void)
     return;
 }
 
+int random = 0;
 void ScrewTimerInterrupt(int ARG0)
 {
     ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     ROM_IntMasterDisable();
 
+    //Read_Screw_Encoder();
+    //ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
+
     if (SCREW_TimerActivated == true)
     {
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-        ScrewChangeCounter = 0;
-        ScrewChangeLimit = ScrewRunningTime/12000000;
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
+      //  ScrewChangeCounter = 0;
+      //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
     }
     else
@@ -429,10 +479,8 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    //Report("ScrewTimerInterrupt dir, duration, speed", __FILE__,ScrewCurrentDirection,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
-    //
-    // Enable all interrupts.
-    //
+    Rotations+=0.03;
+
     return ;
 
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index cd71c73b1..91eedeb6b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -9,7 +9,7 @@
 #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer
 #define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer
 #define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer
-#define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
+#define NUM_OF_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1)
 //} DANCER_ENUM;
 typedef enum threadMotorsEnum
 {
@@ -31,10 +31,10 @@ uint32_t ThreadConfigBreakSensor(void *request);
 uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
 double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
-uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId);
-uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
+uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId);
+uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId);
 uint32_t ThreadDistanceToSpoolState(void);
-uint32_t ThreadEndState(void *JobDetails);
+uint32_t ThreadEndState();
 uint32_t ThreadInitialTestStub();
 
 uint32_t StoreDancerConfigMessage(void);
@@ -67,9 +67,11 @@ typedef enum
     THREAD_LOAD_END
 }THREAD_LOAD_STAGES_ENUM;
 uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue);
+bool ThreadLoadingActive(void);
 
 
 void ThreadLoadPollRequest(MessageContainer* requestContainer);
 void ThreadLoadRequest(MessageContainer* requestContainer);
 
+
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index a1e91f6fc..4454565c1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -2,6 +2,7 @@
  **************************************************************************************************************************/
 
 #include <DataDef.h>
+#include <Drivers/FPGA/Motors_Driver/PowerSTEP01.h>
 #include "include.h"
 
 #include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
@@ -17,12 +18,13 @@
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/SSI_Comm/Dancer/Dancer.h"
 
-#include "drivers/FPGA/Moters_Driver/PowerSTEP01.h"
-
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
+int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {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};
@@ -105,7 +107,10 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
 #endif
     for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+    {
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
+        MotorSpeedSamples[Motor_i][i] = 0;
+    }
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
     temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index f4dc6a170..c292f7dcd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -68,6 +68,12 @@ double TempPoolerTotalProcessedLength = 0.0;
 double TempTotalProcessedLength = 0.0;
 
 bool PrepareState = false;
+// job parameters
+bool EnableLubrication = false;
+bool EnableIntersegment = false;
+double IntersegmentLength = 0;
+
+
 int CurrentSegmentId = 0;
 typedef  void (* ProcessedLengthFunc)(void);
 ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
@@ -77,6 +83,11 @@ void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue);
 
+bool SegmentState = false;
+bool PreSegmentState = false;
+bool DTSState = false;
+void SendSegmentFail(void);
+
 double KeepNormalizedError = 0;
 bool ThreadControlActive = false;
 ////////////////////////Slow Motor State////////////////////////////////////
@@ -122,6 +133,7 @@ uint32_t Poolerinitialpos = 0xFFFF;
 
 void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
+    REPORT_MSG(length,"ThreadUpdateProcessLength");
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
@@ -354,7 +366,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     int DancerId;
-    int32_t TranslatedReadValue, avreageSampleValue = 0;
+    int32_t TranslatedReadValue, avreageSampleValue = 0,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
@@ -373,6 +385,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     if(MotorControlConfig[index].m_isEnabled )
     {
+        //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
+        //    return OK;
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
         {
@@ -428,7 +442,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                             JobEndReason = JOB_THREAD_BREAK;
                             ThreadControlActive = false;
                             SendJobProgress(0.0,0,false, TMessage);
-                            SegmentReady(Module_Thread,ModuleFail);
+                            SendSegmentFail();
                             //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
                             //EndState(CurrentJob,"ReadBreakSensor Error" );
                             LOG_ERROR(index, "ReadBreakSensor Error");
@@ -458,7 +472,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
             SendJobProgress(0.0,0,false, TMessage);
             //EndState(CurrentJob,TMessage );
-            SegmentReady(Module_Thread,ModuleFail);
+            SendSegmentFail();
             /*switch (index)
             {
                 case POOLER_MOTOR:
@@ -487,7 +501,23 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             //KeepNormalizedError = NormalizedError;
         }
+        if ((JobCounter % 1000) == 0)
+        {
+            if (JobCounter >= 20000)
+            {
+                MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue);
+                MotorSpeedSamplePointer[index]++;
+                if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES)
+                    MotorSpeedSamplePointer[index] = 0;
+                for (i=0;i<MAX_CONTROL_SAMPLES;i++)
+                    avreageMotorSampleValue += MotorSpeedSamples[index][i];
+                avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES;
+                //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0);
+                OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue;
+            }
+        }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
         {
             /*if (keepdata == true)
@@ -552,6 +582,7 @@ bool InitialProcess = false;
  uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
+    JobTicket* JobTicket = JobDetails;
     CurrentSegmentId = 0;
 
     JobCounter = 0;
@@ -564,6 +595,11 @@ bool InitialProcess = false;
     AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false);
     AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false);
 
+    EnableLubrication = JobTicket->enablelubrication;
+    EnableIntersegment = JobTicket->enableintersegment;
+    IntersegmentLength = JobTicket->intersegmentlength;
+
+
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
@@ -660,7 +696,7 @@ bool InitialProcess = false;
 
 void SetOriginMotorSpeed(float process_speed)
 {
-    int Motor_i, HW_Motor_Id;
+    int i,Motor_i, HW_Motor_Id;
     for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
@@ -672,14 +708,16 @@ void SetOriginMotorSpeed(float process_speed)
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
         CurrentControlledSpeed[Motor_i] = (int) motor_speed;
+        for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
+            MotorSpeedSamples[Motor_i][i] = motor_speed;
     }
 }
 
 //********************************************************************************************************************
-uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
+uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
 {
 //set the speed only before the first segment, speed is constant across all job segments and intersegments
-    JobTicket* JobTicket = JobDetails;
+    //JobSegment* Segment = SegmentDetails;
 
     float process_speed = dyeingspeed;
     if (dyeingspeed == 0)
@@ -709,15 +747,22 @@ uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
             MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
             MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
         }
-        IDS_StartLubrication();
+        if (EnableLubrication == true)
+        {
+            IDS_StartLubrication();
+        }
     }
     // activate control fr all motors
     //set speed for both rocker motors
     //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
-    if ((InitialProcess==false) && JobTicket->enableintersegment == true)
-    {
-        ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded);
+    if ((InitialProcess==false) && (EnableIntersegment == true)) //&& (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0
+    {//add initial presegment and cleaning before first segment
+        ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded);
+        REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength");
+        SegmentState = false;
+        PreSegmentState = true;
+        DTSState = false;
     }
     else
     {
@@ -730,14 +775,26 @@ uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
     return OK;
 }
 int REPSegmentId = 0;
+void SendSegmentFail(void)
+{
+    if (SegmentState == true)
+        SegmentReady(Module_Thread,ModuleFail);
+    else if (PreSegmentState == true)
+        PreSegmentReady(Module_Thread,ModuleFail);
+    else if (DTSState == true)
+        DistanceToSpoolReady(Module_Thread,ModuleFail);
+
+}
+
 void ThreadInterSegmentEnded(void)
 {
     REPORT_MSG (REPSegmentId,"ThreadInterSegmentEnded");
+    //ThreadUpdateProcessLength (0,(void *)NULL);
     PreSegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadSegmentEnded(void)
 {
-    REPORT_MSG (REPSegmentId," ThreadSegmentState");
+    REPORT_MSG (REPSegmentId," ThreadSegmentEnded");
     SegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadDistanceToSpoolEnded(void)
@@ -747,14 +804,17 @@ void ThreadDistanceToSpoolEnded(void)
 }
 double seglength = 0.0;
 //********************************************************************************************************************
-uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
+uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId)
 {
-    JobTicket* JobTicket = JobDetails;
+    JobSegment* Segment = SegmentDetails;
     REPSegmentId = SegmentId;
-    seglength = JobTicket->segments[SegmentId]->length;
+    seglength = Segment->length;
     CurrentSegmentId = SegmentId;
     REPORT_MSG (seglength," ThreadSegmentState");
     ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
+    SegmentState = true;
+    PreSegmentState = false;
+    DTSState = false;
     return OK;
 }
 
@@ -764,12 +824,15 @@ uint32_t ThreadDistanceToSpoolState(void )
     seglength = dryerbufferlength;
     REPORT_MSG (seglength,"ThreadDistanceToSpoolState");
     ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded);
+    SegmentState = false;
+    PreSegmentState = false;
+    DTSState = true;
     return OK;
 }
 
 char Endstr[150];
 //********************************************************************************************************************
- uint32_t ThreadEndState(void *JobDetails)
+ uint32_t ThreadEndState(void )
 {
      int Motor_i;
      ThreadControlActive = false;
@@ -779,7 +842,8 @@ char Endstr[150];
      Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
 
      ThreadUpdateProcessLength (0.0,(void *)NULL);
-    SetOriginMotorSpeed(0);
+     //TotalProcessedLength = 0.0;
+     SetOriginMotorSpeed(0);
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
      MillisecLogClose();
 #endif