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

6 files changed, 133 insertions, 48 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 638611c8b..3e2a6aa40 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -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 a82fe37e6..74a07e99c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -38,21 +38,21 @@
 /*    typedef enum
     {
         0 THREAD_LOAD_INIT,
-        1 THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
-        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
-        3 THREAD_LOAD_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
-        4 THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
+        1 THREAD_LOAD_REDUCE_HEAT,                           //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
+        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,        //USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
+        3 THREAD_LOAD_CENTER_HEAD_ROCKERS,                   //puthead cleaning rockers to middle position
+        4 THREAD_LOAD_OPEN_COVERS,                           //OPEN DYEING HEAD COVER AND DRYER LID
         5 THREAD_LOAD_LIFT_DANCERS,
-        6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
-        7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
+        6 THREAD_LOAD_LIFT_ROCKERS,                          //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
+        7 THREAD_LOAD_INITIAL_TENSION,                       //CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
         8 THREAD_LOAD_CLOSE_ROCKERS,
-        9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
+        9 THREAD_LOAD_CLOSE_DANCERS,                         //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
         10 THREAD_LOAD_CLOSE_LIDS,
         11 THREAD_LOAD_RESUME_HEATING,
-        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
-        13 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
-                                    //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
-        14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
+        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,           //JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
+        13 THREAD_LOAD_DRYER_LOADING,                        //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
+                                                             //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
+        14 THREAD_LOAD_JOG_THREAD,                           //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
         15 THREAD_LOAD_END
     }THREAD_LOAD_STAGES_ENUM;*/
     THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
@@ -222,9 +222,9 @@
                 else
                 {
                     LoadStages++;
-                    if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+                    if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
                     {
-                        ThreadLoadStateMachine(LoadStages);
+                        //ThreadLoadStateMachine(LoadStages);
                     }
                 }
             }
@@ -340,7 +340,7 @@
         {
             Control_Dryer_Fan(START,75);//use START or STOP,  0 - 100%
             LoadStages++;
-            ThreadLoadStateMachine(LoadStages);
+            //ThreadLoadStateMachine(LoadStages);
         }
         return OK;
     }
@@ -357,6 +357,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);
@@ -371,7 +372,7 @@
                 LoadStages++;
                 if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
                 {
-                    ThreadLoadStateMachine(LoadStages);
+                    //ThreadLoadStateMachine(LoadStages);
                 }
             }
         }
@@ -417,7 +418,7 @@
 
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmInfo.LoadArmBackLash = 0;
-        //LoadArmInfo.LoadArmRounds = 0xFF;
+        LoadArmInfo.LoadArmRounds = 0xFF;
         FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
         return OK;
     }
@@ -428,7 +429,7 @@
         ControlId = 0xFF;
         ThreadAbortJoggingFunc();
         LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        //ThreadLoadStateMachine(LoadStages);
         return OK;
     }
     uint32_t Thread_Load_Jog_Thread(void)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 18aceb767..faee55608 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -40,6 +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 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;
@@ -75,7 +77,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;
@@ -135,8 +137,16 @@ 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");
-
-    status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000);
+    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;
 
@@ -188,6 +198,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;
@@ -196,13 +208,21 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     double RotationsPerSecond;
     int32_t Averagewinderspeed = 0;
 
-    TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
-    WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
-    TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
-    if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
+    //ScrewChangeCounter++;
+    //if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
     {
-        WinderMotorSpeedCounter=0;
-        WinderMotorSpeedRollOver=true;
+        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;
+        }
     }
 
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
@@ -213,6 +233,12 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
     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--;
@@ -240,7 +266,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();
@@ -260,7 +288,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__,__LINE__,temp,RpWarning,ScrewSpeed,0);
+        Report("new winder speed",__FILE__,ScrewNumberOfSteps,temp,RpWarning,ScrewSpeed,0);
     }
 
     /********************************************************************************/
@@ -274,7 +302,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;
@@ -316,6 +344,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     //     * 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;
+        ScrewNumberOfSteps -= 100;
         temp = SYS_CLK_FREQ;
         temp *= InternalWinderCfg.segmentoffsetpulses;
         temp /= ScrewSpeed;
@@ -335,6 +364,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         WinderMotorSpeedCounter=0;
         TotalWinderSpeed = 0;
         WinderMotorSpeedRollOver=false;
+        Add100 = false;
    }
     PreSegmentReady(Module_Winder,ModuleDone);
 
@@ -397,12 +427,14 @@ void ScrewsStartControlTimer (void)
     //ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
     TimerEnable(Screw_timerBase, TIMER_A);
     ROM_IntEnable(INT_TIMER3A);
+    //IntPrioritySet(Screw_timerBase, 0x40);
     ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
     ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)1200000/*10 millisec*/ );
     Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0);
     return;
 }
 
+int random = 0;
 void ScrewTimerInterrupt(int ARG0)
 {
     ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
@@ -410,9 +442,11 @@ void ScrewTimerInterrupt(int ARG0)
 
     if (SCREW_TimerActivated == true)
     {
-        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
+        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
+      //  ScrewChangeCounter = 0;
+      //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
     }
     else
@@ -420,6 +454,10 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
+    Rotations+=0.1;
+    /*random++;
+    if (random >= 2)
+        random = -1;*/
     //Report("ScrewTimerInterrupt dir, duration, speed", __FILE__,ScrewCurrentDirection,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
     //
     // Enable all interrupts.
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index cd71c73b1..69ac4b6ad 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -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);
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 6a8474438..4131fbca5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -30,6 +30,7 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 #include "modules/General/process.h"
+#include "modules/ids/ids_ex.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
 #include "Control/MillisecTask.h"
 
@@ -67,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;
@@ -353,7 +360,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;
@@ -372,6 +379,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)
         {
@@ -486,7 +495,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)
@@ -551,6 +576,7 @@ bool InitialProcess = false;
  uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
+    JobTicket* JobTicket = JobDetails;
     CurrentSegmentId = 0;
 
     JobCounter = 0;
@@ -563,6 +589,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++)
     {
@@ -659,7 +690,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];
@@ -671,14 +702,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)
@@ -708,14 +741,19 @@ 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);
         }
+        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)
+    if ((InitialProcess==false) && (EnableIntersegment == true) && (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0
     {
-        ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded);
+        ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded);
+        REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength");
     }
     else
     {
@@ -731,11 +769,12 @@ int REPSegmentId = 0;
 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)
@@ -745,11 +784,11 @@ 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);
@@ -767,7 +806,7 @@ uint32_t ThreadDistanceToSpoolState(void )
 
 char Endstr[150];
 //********************************************************************************************************************
- uint32_t ThreadEndState(void *JobDetails)
+ uint32_t ThreadEndState(void )
 {
      int Motor_i;
      ThreadControlActive = false;
@@ -807,7 +846,7 @@ char Endstr[150];
      }
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
-
+     IDS_StopLubrication();
     return OK;
 }