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

1 files changed, 535 insertions, 178 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 69f640c45..bc4e6cb8f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -37,6 +37,8 @@
 #include "Control/MillisecTask.h"
 #include "drivers/Flash_ram/MCU_E2Prom.h"
 
+#include "drivers/SSI_Comm/SSI_Comm.h"
+
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
 //by recieved esign flow of the user from the UI
@@ -44,9 +46,15 @@
 
 double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
+#ifdef FOUR_WINDERS
+TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
+Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS};
+uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
+#else
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
 HardwareDancerType 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,0xFF,0xFF,0xFF,0xFF};
+#endif
 uint32_t    SpeedControlId=0xFF;
 uint32_t    PoolerSpeedControlId=0xFF;
 
@@ -58,6 +66,10 @@ uint32_t JobCounter = 0;
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 
+int MotorTiming[MAX_THREAD_MOTORS_NUM];
+int MotorTimer[MAX_THREAD_MOTORS_NUM];
+
+
 uint32_t PreviousPosition = 0, CurrentPosition = 0;
 double CurrentRequestedLength = 0.0;
 double CurrentProcessedLength = 0.0;
@@ -140,15 +152,22 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
  * **************************************************************************************/
 uint32_t initialpos = 0xFFFF;
 uint32_t Poolerinitialpos = 0xFFFF;
+#define SPEED_STORE_SIZE 20
+float PullerSpeedStore[SPEED_STORE_SIZE];
+float PullerSpeedAverage;
+int PullerSpeedIndex = 0;
+float FeederSpeedStore[SPEED_STORE_SIZE];
+float FeederSpeedAverage;
+int FeederSpeedIndex = 0,Speed_i;
 
 void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
-    REPORT_MSG(length,"ThreadUpdateProcessLength");
+    ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0);
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
 }
-char Lenstr[160];
+char Lenstr[190];
 uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     uint32_t positionDiff = 0,prevprev;
@@ -206,6 +225,16 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }
 
     }
+    /*FeederSpeedStore[FeederSpeedIndex++] = length;
+    if (FeederSpeedIndex>=SPEED_STORE_SIZE)
+    {
+        FeederSpeedIndex = 0;
+        FeederSpeedAverage = 0;
+        for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++)
+            FeederSpeedAverage+=FeederSpeedStore[Speed_i];
+        FeederSpeedAverage = FeederSpeedAverage/SPEED_STORE_SIZE;
+        ReportWithPackageFilter(ThreadFilter,"Avg len 100ms last 2 sec",__FILE__,(int)PoolerTotalProcessedLength,(int)(FeederSpeedAverage*1000),RpWarning,(int)(PullerSpeedAverage*1000),0);
+    }*/
     TotalProcessedLength += (length/100);
     TempTotalProcessedLength = TotalProcessedLength;
 #ifdef FEEDER_LENGTH_CALCULATION
@@ -242,7 +271,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     return OK;
 }
-
 uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     uint32_t positionDiff = 0,prevprev;
@@ -297,9 +325,22 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             length = 0;
 
         }
+        /*PullerSpeedStore[PullerSpeedIndex++] = length;
+        if (PullerSpeedIndex>=SPEED_STORE_SIZE)
+        {
+            PullerSpeedIndex = 0;
+            PullerSpeedAverage = 0;
+            for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++)
+                PullerSpeedAverage+=PullerSpeedStore[Speed_i];
+            PullerSpeedAverage = PullerSpeedAverage/SPEED_STORE_SIZE;
+            //ReportWithPackageFilter(ThreadFilter,"Average Speed 2 second",__FILE__,__LINE__,(int)(FeederSpeedAverage*100),RpWarning,(int)(PullerSpeedAverage*100),0);
+        }**/
 
     //}
-
+#ifdef BTSR_NO_TFU
+        if (CurrentControlledSpeed[WINDER_MOTOR]>100)
+            length = dyeingspeed/10;
+#endif
     PoolerTotalProcessedLength+= (length/100);
     TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
 #ifndef FEEDER_LENGTH_CALCULATION
@@ -322,9 +363,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0))
     {
-        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
         SendJobProgress(0.0,0,false, Lenstr);
-        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0);
         // segment/intersegment/distance to spool finished
         if (ProcessedLengthFuncPtr)
             ProcessedLengthFuncPtr();
@@ -393,7 +434,72 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
+uint16_t BreakSensorCounter = 0;
+uint16_t BreakSensorLatchCounter = 0;
+char TMessage[150];
+#ifdef FOUR_WINDERS
+char ATMessage[MAX_THREAD_MOTORS_NUM][150];
+int c = 0;
+#endif
+uint32_t checkBreakSensor(uint32_t index)
+{
+    if (BreakSensorenabled == true)
+    {
+        if (JobCounter > eOneSecond)
+        {
+            if (ReadBreakSensor()==ERROR)
+            {
+                BreakSensorCounter++;
+                BreakSensorLatchCounter++;
+                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
+                {
+                    //consider applying the debouce parameters later
+                    usnprintf(TMessage, 60, "ReadBreakSensor Error");
+                    //BreakSensordebouncetimemilli
+                    JobEndReason = JOB_THREAD_BREAK;
+                    ThreadControlActive = false;
+                    SendJobProgress(0.0,0,false, TMessage);
+                    strcpy(AlarmReasonStr,TMessage);
+                    SendSegmentFail();
+                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
+                    //EndState(CurrentJob,"ReadBreakSensor Error" );
+                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                    return ERROR;
+                } //passed limit
+            }//ReadBreakSensor()==ERROR
+            else //reset counter - we are looking for consequent calls
+            {
+                if (BreakSensorCounter)
+                {
+                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                }
+                BreakSensorCounter = 0;
+            }
+            if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
+            {
+                BreakSensorCounter++;
+                BreakSensorLatchCounter++;
+                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
+                {
+                    //consider applying the debouce parameters later
+                    usnprintf(TMessage, 60, "thread speed too low");
+                    strcpy(AlarmReasonStr,TMessage);
+                    JobEndReason = JOB_THREAD_BREAK;
+                    ThreadControlActive = false;
+                    SendJobProgress(0.0,0,false, TMessage);
+                    SendSegmentFail();
+                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
+                    //EndState(CurrentJob,"ReadBreakSensor Error" );
+                    ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                    return ERROR;
+                }
+            }
 
+        }
+    }
+    return OK;
+
+}
 int controlIndex = 0;
 bool keepdata = true;
 int32_t KeepReadValue = 0;
@@ -447,20 +553,15 @@ void testDancersControl()
 }
 #endif
 int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
-char TMessage[150];
 //char time[150];
-uint16_t BreakSensorCounter = 0;
-uint16_t BreakSensorLatchCounter = 0;
 bool FirstCalcInJob = true;
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, 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;
-    int len;
     int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
@@ -480,6 +581,23 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     index = IfIndex&0xFF;
 
+    if (index == POOLER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec
+    {
+        if (checkBreakSensor(index) == ERROR)
+            return OK;
+    }
+    if (MotorTiming[index]>1)
+    {
+        MotorTimer[index]++;
+        if (MotorTimer[index]>=MotorTiming[index])
+        {
+            MotorTimer[index]=0;
+        }
+        else
+        {
+            return OK;
+        }
+    }
     if(MotorControlConfig[index].m_isEnabled )
     {
         //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
@@ -511,6 +629,30 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 			//pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
         }
+#ifdef FOUR_WINDERS
+        if (index == WINDER_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_2_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_3_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_4_MOTOR)
+        {
+            c++;
+        }
+        //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR))
+        /*if (index >= WINDER_MOTOR)
+        {
+            //pooler dancer is right sided: data is opposite
+            TranslatedReadValue = (-1*TranslatedReadValue);
+        }*/
+#endif
         if (index == POOLER_MOTOR)
         {
             //pooler dancer is right sided: data is opposite
@@ -529,61 +671,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;
 
-        if (BreakSensorenabled == true)
-        {
-            if (index == POOLER_MOTOR)
-            {
-                if (JobCounter > eOneSecond)
-                {
-                    if (ReadBreakSensor()==ERROR)
-                    {
-                        BreakSensorCounter++;
-                        BreakSensorLatchCounter++;
-                        if (BreakSensorCounter>=BreakSensordebouncetimemilli)
-                        {
-                            //consider applying the debouce parameters later
-                            usnprintf(TMessage, 60, "ReadBreakSensor Error");
-                            //BreakSensordebouncetimemilli
-                            JobEndReason = JOB_THREAD_BREAK;
-                            ThreadControlActive = false;
-                            SendJobProgress(0.0,0,false, TMessage);
-                            SendSegmentFail();
-                            //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
-                            //EndState(CurrentJob,"ReadBreakSensor Error" );
-                            ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-                            return OK;
-                        } //passed limit
-                    }//ReadBreakSensor()==ERROR
-                    else //reset counter - we are looking for consequent calls
-                    {
-                        if (BreakSensorCounter)
-                        {
-                            ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-                        }
-                        BreakSensorCounter = 0;
-                    }
-                    if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
-                    {
-                        BreakSensorCounter++;
-                        BreakSensorLatchCounter++;
-                        if (BreakSensorCounter>=BreakSensordebouncetimemilli)
-                        {
-	                        //consider applying the debouce parameters later
-	                        usnprintf(TMessage, 60, "thread speed too low");
-	                        JobEndReason = JOB_THREAD_BREAK;
-	                        ThreadControlActive = false;
-	                        SendJobProgress(0.0,0,false, TMessage);
-	                        SendSegmentFail();
-	                        //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
-	                        //EndState(CurrentJob,"ReadBreakSensor Error" );
-                            ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-	                        return OK;
-						}
-                    }
-
-                }
-            }
-        }
 
         //Stop Execution if the dancer moves too much
         if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
@@ -595,7 +682,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             ThreadControlActive = false;
             //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
             JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
+#ifdef FOUR_WINDERS
+            if (DancerId>HARDWARE_DANCER_2)
+                JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0;
+#endif
             SendJobProgress(0.0,0,false, TMessage);
+            strcpy(AlarmReasonStr,TMessage);
             //EndState(CurrentJob,TMessage );
             SendSegmentFail();
             /*switch (index)
@@ -647,21 +739,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             }*`/
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (index < WINDER_MOTOR)
+            calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
-        if (FirstCalcInJob == true)
+        //if (0)//(JobCounter % 1000 == 0)
+#ifdef FOUR_WINDERS
+        if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true)
         {
-            if (index == FEEDER_MOTOR)
+            if (index >= WINDER_MOTOR)
             {
-                FirstCalcInJob = false;
-                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
-                                ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
+           //     FirstCalcInJob = false;
+                usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1,
+                                TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
-                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
-                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));
+                ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);
 
             }
+            JobCounter++;
         }
-
+#endif
 #ifndef TEST_PID_THREAD
         if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
 #else
@@ -670,6 +767,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             if (calculated_speed>5.0)
             {
+                /*if (calculated_speed>(CurrentControlledSpeed[index]+100))
+                {
+                        ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,index,0);
+                        calculated_speed=CurrentControlledSpeed[index]+100;
+                }*/
                 CurrentControlledSpeed[index] = calculated_speed;
                 MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
             }
@@ -684,7 +786,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
             }*/
 #ifdef TEST_PID_THREAD
-            int len;
             if ((JobCounter % 2000) == index*100)
              //if (keepdata == true)
             {
@@ -696,7 +797,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam;
                 mIntegral[controlIndex] = MotorControlConfig[index].m_integral;
                 timestamp[controlIndex] = msec_millisecondCounter;*/
-                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
+                usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
                                 ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
                                 (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
@@ -740,64 +841,166 @@ double ThreadGetMotorCalculatedError(int DancerId)
 }
 
 //********************************************************************************************************************
-uint32_t ThreadInitialTestStub(HardwareMotor * request)
-{
-
-
-    //MotorsConfigMessage(request);
-     ThreadPrepareState(request);
-     ThreadPreSegmentState(request,0);
-    return OK;
-}
 uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
 {
-    /*
-         FEEDER_MOTOR,
-    DRYER_MOTOR,
-    POOLER_MOTOR,
-    WINDER_MOTOR,
-    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;
-
-     */
     if (ThreadParams == NULL)
     {
         return OK;
     }
+    if((ThreadParams->bypassrockers != true)&&(ThreadParams->bypassrockers != false))
+    {
+        ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0);
+        return OK;
+    }
+    if ((ThreadParams->feederp>100000)||(ThreadParams->feederi>100000))
+    {
+        ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0);
+        return OK;
+    }
     if(ThreadParams->feederp)
-        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederp;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp;
     if(ThreadParams->feederi)
         MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi;
     if(ThreadParams->feederd)
         MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd;
 
     if(ThreadParams->pullerp)
-        MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerp;
+        MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp;
     if(ThreadParams->pulleri)
         MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri;
     if(ThreadParams->pullerd)
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd;
 
     if(ThreadParams->winderp)
-        MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderp;
+    {
+        MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp;
+#endif
+    }
     if(ThreadParams->winderi)
+    {
         MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi;
+#endif
+    }
     if(ThreadParams->winderd)
+    {
         MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd;
+#endif
+    }
+    ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0);
     if(ThreadParams->bypassrockers)
         Thread_Rockers_Bypass = true;
 
     return OK;
 }
+bool RTFU_Up = false;
+
+uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag)
+{
+    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
+    Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0);
+
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands
+    return OK;
+}
+uint32_t Release_Right_TFU_Tension()
+{
+    uint32_t status = OK;
+#ifndef BTSR_NO_TFU
+    if (RTFU_Up == true)
+    {
+        Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0);
+        RTFU_Up = false;
+        status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000);
+    }
+#endif
+    return status;
+}
+int SecondFeederCorrection = 4;
+int PrepareWaitCount = 0;
+uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue)
+{
+#ifndef BTSR_NO_TFU
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands
+    Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0);
+    if (JobIsActive()==false)
+    {
+        Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0);
+        Release_Right_TFU_Tension();
+    }
+    if (PrepareWaitCount)
+    {
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareWaitCount--;
+    }
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
+    {
+        PrepareState = false;
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareReady(Module_Thread,ModuleDone);
+    }
+#endif
+    return OK;
+}
+uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
+{
+#ifndef BTSR_NO_TFU
+    Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0);
+    MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000);
+    RTFU_Up = true;
+#endif
+    return OK;
+}
+
+uint32_t Adjust_Right_TFU_Tension(double tension)
+{
+    uint32_t status = OK;
+#ifndef BTSR_NO_TFU
+    if (tension > 0.5) //0 = lower position, 1 = high position
+    {
+        if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
+        {
+            PrepareWaitCount++;
+            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
+            Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0);
+        }
+    }
+#endif
+    return status;
+}
+uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension)
+{
+    //MotorStop(MotorId,Hard_Hiz);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0);
+    if (PrepareWaitCount)
+    {
+        PrepareWaitCount--;
+    }
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
+    {
+        PrepareState = false;
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0);
+        PrepareReady(Module_Thread,ModuleDone);
+    }
+    return OK;
+}
 uint32_t ThreadPrepare_Tension (int DancerId, double tension)
 {
     int current, request = (int)tension,movement;
     int HW_Motor_Id;
     bool direction;
     uint32_t status = OK, address = 0;
-    if (tension < 100)
-        return OK; //do not handle tension of zero
     switch (DancerId)
     {
     case HARDWARE_DANCER_TYPE__LeftDancer:
@@ -811,10 +1014,13 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
         HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2;
         break;
     case HARDWARE_DANCER_TYPE__RightDancer:
-        return ERROR;
+        return Adjust_Right_TFU_Tension(tension);
+        //break;
     default:
         return ERROR;
     }
+    if (tension < 100)
+        return OK; //do not handle tension of zero
     status |= MCU_E2PromRead(address,&current);
     if ((status!= OK )||(current == 0xFFFF))
         return status;
@@ -832,24 +1038,37 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize;
             movement = current - request;
         }
-        MotorSetMaxSpeed (HW_Motor_Id, 500);
-        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000);
+        MotorSetMaxSpeed (HW_Motor_Id, 800);
+        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000);
+        PrepareWaitCount++;
+        ReportWithPackageFilter(ThreadFilter,"PrepareWaitCount",__FILE__,PrepareWaitCount,current,RpWarning,request,0);
         status |= MCU_E2PromProgram(address,request);
     }
-    usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d",
-              DancerId,request,current,movement,direction,HW_Motor_Id,address);
-    ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0);
+    if (DancerId == HARDWARE_DANCER_TYPE__LeftDancer)
+    {
+        usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d",
+                  DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount);
+        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,current,RpWarning,request,0);
 
+    }
+    else
+    {
+        usnprintf(TMessage, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d",
+                  DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount);
+        ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,address,current,RpWarning,request,0);
+
+    }
     return status;
 }
 //********************************************************************************************************************
- uint32_t ThreadPrepareState(void *JobDetails)
+bool SkipOpenLids = true;
+uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i,i, HW_Motor_Id, Pid_Id;
     JobTicket* JobTicket = JobDetails;
     uint32_t status = OK;
     CurrentSegmentId = 0;
-
+    float temp_dt = 0;
     JobCounter = 0;
     TotalProcessedLength = 0.0;
     PoolerTotalProcessedLength = 0.0;
@@ -873,13 +1092,15 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     EnableIntersegment = JobTicket->enableintersegment;
     IntersegmentLength = JobTicket->intersegmentlength;
 
+    PrepareWaitCount = 0;
+#ifndef BTSR_NO_TFU
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
-    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,status,RpFatalError,(int)windertension,0);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0);
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
-    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,status,RpFatalError,(int)pullertension,0);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0);
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension);
-    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,status,RpFatalError,(int)feedertension,0);
-
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0);
+#endif
     FirstCalcInJob = true;
     if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
     {
@@ -893,19 +1114,57 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     }
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop);
 
-    if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)
+    /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)
     {
-        ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0);
+        ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0);
         //JobEndReason = JOB_LIDS_OPEN;
+        // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!");
         //PrepareReady(Module_Thread,ModuleFail);
         //return ERROR;
     }
     if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)
     {
-        ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpFatalError,LIMIT,0);
+        ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0);
         //JobEndReason = JOB_LIDS_OPEN;
+        // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!");
         //PrepareReady(Module_Thread,ModuleFail);
         //return ERROR;
+    }*/
+
+//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false))
+    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false))
+    {
+        if(Head_Type != HEAD_TYPE_ARC)
+        {
+            ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0);
+            if (SkipOpenLids == false)
+            {
+                JobEndReason = JOB_LIDS_OPEN;
+                usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!");
+                PrepareReady(Module_Thread,ModuleFail);
+                return ERROR;
+            }
+        }
+    }
+    if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) {
+        if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) {
+            ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0);
+        }
+        if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) {
+            ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0);
+        }
+    }
+//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false))
+    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false))
+    {
+        ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0);
+        if (SkipOpenLids == false)
+        {
+            JobEndReason = JOB_LIDS_OPEN;
+            usnprintf(AlarmReasonStr, 100, "Dryer lid is open!!!");
+            PrepareReady(Module_Thread,ModuleFail);
+            return ERROR;
+        }
     }
 
     //start thread control for all motors
@@ -913,61 +1172,81 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
-            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.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
-            MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
-            MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
-            MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
-            MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
-            MotorControlConfig[Motor_i].m_calculatedError = 0;
-            MotorControlConfig[Motor_i].m_integral = 0;
-            MotorControlConfig[Motor_i].m_isEnabled = true;
-            MotorControlConfig[Motor_i].m_isReady = true;
-            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
+#ifdef FOUR_WINDERS
+        if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR;
+        if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR;
+        if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR;
+#endif
+        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.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
+        MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
+        MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
+        MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
+        MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[Motor_i].m_calculatedError = 0;
+        MotorControlConfig[Motor_i].m_integral = 0;
+        MotorControlConfig[Motor_i].m_isEnabled = true;
+        MotorControlConfig[Motor_i].m_isReady = true;
+        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
+
+        HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
+
+        temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+        MotorTiming[Motor_i] = (int)temp_dt;
+        if (MotorTiming[Motor_i])
+        {
+            MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+            ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
+        }
 //////////////////////////////////////////////////
-            for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
-            {
-                //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                    MotorSamples[Motor_i][i] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0);  //reset the samples value for control beginning
-               // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
-                //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
-                //MotorSpeedSamples[Motor_i][i] = 0;
-            }
-            MotorSamplePointer[Motor_i] = 0;
+        for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+        {
+            //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                MotorSamples[Motor_i][i] = 0;
+                if (Motor_i == FEEDER_MOTOR)
+                    MotorSamples[Motor_i][i] = -500;
+           // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
+            //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
+            //MotorSpeedSamples[Motor_i][i] = 0;
+        }
+        MotorSamplePointer[Motor_i] = 0;
 /////////////////////////////////////////////////////
-            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
+        MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
-            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
+#ifndef BTSR_NO_TFU
+        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
+        {
+            ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (SpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (SpeedControlId != 0xFF)
-                {
-                    RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
-                    SpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                SpeedControlId = 0xFF;
             }
-            if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
+#endif
+        if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        {
+            ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (PoolerSpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (PoolerSpeedControlId != 0xFF)
-                {
-                    if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
-                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
-                     PoolerSpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
+                    ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
+                 PoolerSpeedControlId = 0xFF;
             }
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
+#ifndef BTSR_NO_TFU
             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
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
@@ -995,6 +1274,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
+#endif
             if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
             {
                 ReportWithPackageFilter(ThreadFilter,"Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
@@ -1009,25 +1289,87 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
-//            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,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
-            if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                continue;
+#ifdef FOUR_WINDERS
+            if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
+            {
+                ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0);
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
+                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
+                    CurrentControlledSpeed[Motor_i] = 0;
+                    ControlIdtoMotorId[Motor_i] = 0xFF;
+                }
+#ifndef TEST_PID_THREAD
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+#endif
+            }
+#endif
     }
-    HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
 #ifdef TEST_PID_THREAD
     testDancersControl();
 #endif
-    PrepareReady(Module_Thread,ModuleDone);
+    if (PrepareWaitCount == 0)
+        PrepareReady(Module_Thread,ModuleDone);
     //set 3 dancers to the profile positions
-    return OK;
+    return status;
 }
+uint32_t UpdatePidDuringRun(HardwarePidControl *request)
+{
+    int Motor_i = MAX_THREAD_MOTORS_NUM,i;
+    double temp_dt;
+
+    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    {
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
+        {
+            Motor_i = i;
+            break;
+        }
+    }
+    if (Motor_i == MAX_THREAD_MOTORS_NUM)
+        return ERROR;
 
+
+    if (request->derivativetime == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0);
+    }
+    if (request->proportionalgain == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0);
+    }
+    if (request->integraltime == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Ki = request->integraltime;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0);
+    }
+    if (request->epsilon == true)
+    {
+        MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0);
+    }
+    if (request->has_controloutputtype == true)
+    {
+        MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype;
+        temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+        MotorTiming[Motor_i] = (int)temp_dt;
+        if (MotorTiming[Motor_i])
+        {
+            MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+        }
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0);
+    }
+//////////////////////////////////////////////////
+    return OK;
+}
 void SetOriginMotorSpeed(float process_speed)
 {
     int Motor_i, HW_Motor_Id;
-    for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++)
+    for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         //(Speed*uStep*PPR)/((2*PI*motor_Radius)
@@ -1100,7 +1442,11 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
     {
         SetOriginMotorSpeed(process_speed);
         ThreadControlActive = true;
-        PrepareState = false;
+        if (PrepareWaitCount == 0)
+            PrepareState = false;
+        PullerSpeedIndex = 0;
+        FeederSpeedIndex = 0;
+
 #ifndef TEST_PID_THREAD
         // set the new speed in the dryer motor to the speed of the new segment
         if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
@@ -1111,7 +1457,7 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
         {
             DrierDivider = dyeingspeed/5;  //ramp up drier in 5 cm/sec steps
         }
-        ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0);
+        ReportWithPackageFilter(ThreadFilter,"Dryer ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0);
         InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider;
         MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed );
         ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0);
@@ -1235,7 +1581,7 @@ char Endstr[150];
      int Motor_i;
      ThreadControlActive = false;
      uint32_t status = OK;
-     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
      SendJobProgress(0.0,0,false, Endstr);
      ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
 
@@ -1258,7 +1604,7 @@ char Endstr[150];
          PoolerSpeedControlId = 0xFF;
      }
 
-     for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
      {
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {
@@ -1271,18 +1617,29 @@ char Endstr[150];
 
      }
 	 Task_sleep(100);
-     for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
      {
         MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+
      }
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+     Release_Right_TFU_Tension();
      IDS_StopLubrication();
     return OK;
 }
 
-
-
+void ThreadCheckArcHeadCovers(void)
+{
+    if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) {
+         if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) {
+             ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0);
+         }
+         if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) {
+             ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0);
+         }
+     }
+}
 //********************************************************************************************************************
 
 void ThreadStartPrinting(void)