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

6 files changed, 91 insertions, 61 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 19201c708..0093a5a2a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -56,8 +56,8 @@ extern int32_t BreakSensordebouncetimemilli;
 extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
 //extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
-extern uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
-extern int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM];
+extern double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
+extern double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM];
 
 #define MAX_CONTROL_SAMPLES 10
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
@@ -65,7 +65,7 @@ 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 double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index ab5075ab9..66d6baed5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -594,7 +594,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
     }*/
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index f1c8956ff..a68995168 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -43,10 +43,11 @@ uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of
 //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];
+double WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
 uint16_t WinderMotorSpeedCounter = 0;
 bool     WinderMotorSpeedRollOver = false;
-double   ScrewSpeed = 0;
+#define DEFAULT_SCREW_SPEED 1400
+double   ScrewSpeed = DEFAULT_SCREW_SPEED;
 double   ScrewRunningTime = 0;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 bool     SCREW_TimerActivated = false;
@@ -96,7 +97,7 @@ uint32_t Winder_Prepare(void)
     uint32_t status = 0;
     //JobTicket* JobTicket = JobDetails;
     //float process_speed = JobTicket->processparameters->dyeingspeed;
-    double ScrewSpeed = 1500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
+    double ScrewSpeed = DEFAULT_SCREW_SPEED;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
     /*
@@ -143,9 +144,9 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
-    //Read_Screw_Encoder();
-    //ScrewLocationLimitSwitch = Screw_RotEnc.Position;
-    //REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
+    Read_Screw_Encoder();
+    ScrewLocationLimitSwitch = Screw_RotEnc.Position;
+    REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
 
 
     REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
@@ -169,8 +170,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;
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
+    Task_sleep(5);
+    Reset_Screw_Encoder();
+    Task_sleep(5);
+    Read_Screw_Encoder();
+    Task_sleep(5);
+    ScrewLocationStart = Screw_RotEnc.Position;
 
     REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
 
@@ -180,7 +186,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
     REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback");
-    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
@@ -212,26 +217,28 @@ InternalWinderCfg.segmentoffsetpulses
   
 numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 */
-char ScrewStr[100];
+char ScrewStr[150];
 //char TempScrewStr[100];
 double WinderReferenceSpeed=0;
-int32_t  TotalWinderSpeed=0;
+double  TotalWinderSpeed=0;
 bool Add100 = false;
 double Rotations = 6.0;
+int flipflop = 0;
+uint32_t motspeed;
+float speedf;
+int WinderCalculation = 0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
     double temp,tempScrewSpeed;
     double screw_horizontal_speed = 0;
     double RotationsPerSecond;
-    int32_t Averagewinderspeed = 0;
+    double Averagewinderspeed = 0;
 
-    //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];
+//    {
+//        TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
+//        WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
+//        TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
         {
             if (WinderMotorSpeedRollOver == false)
@@ -239,20 +246,34 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
                 Add100 = true;
                 LOG_ERROR(Add100, "Add100 = true");
             }
-            WinderMotorSpeedCounter=0;
+//            WinderMotorSpeedCounter=0;
             WinderMotorSpeedRollOver=true;
         }
+//    }
+    if (flipflop == 0)
+    {
+        MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
+    else if (flipflop == 1)
+    {
+        speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    }
+    flipflop ++;
 
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
         return OK;
+    //deley TODO
+    flipflop = 0;
 
     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]));
+    //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed;
+    //REPORT_MSG((abs(ScrewLocationRun[1] - ScrewLocationRun[0]), "Screw Run NumberOfSteps");
+//    usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d intent %d rot %d",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,Rotations*10);
+    usnprintf(ScrewStr, 150, "Winder Encoder:id, diff, intended,  winderspeed,  rotation, speed, time, mot speed {, %d, %d, %d, %d, %d, %d, %d, %d, }",CalculationDirectionChangeCounter,
+              abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,(int)(WinderReferenceSpeed),(int)(Rotations*10),(int)ScrewSpeed,(int)ScrewRunningTime,(int)speedf);
     //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);
+    Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0);
 
     if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
     {
@@ -265,8 +286,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
-
+            Report("Head Backing",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
     else //next time going back
@@ -274,42 +294,46 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
             ScrewNumberOfSteps++;
-            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
+            Report("Bottom Backing ",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
-    if (WinderMotorSpeedRollOver)
+   /* if (WinderMotorSpeedRollOver)
     {
-        /*for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
+        if (WinderCalculation%60000 == 0)//100 minutes
         {
-            TotalWinderSpeed+=WinderMotorSpeed[i];
-        }*/
-        Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
-        //REPORT_MSG(winderspeed, "WinderSpeedUpdated");
-        //Report("WinderSpeedUpdated",__FILE__,__LINE__,TotalWinderSpeed,RpWarning,Averagewinderspeed,0);
-
-        WinderReferenceSpeed = Averagewinderspeed;
-    }
+            Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
+            //Report("WinderSpeedUpdated",__FILE__,__LINE__,(int)TotalWinderSpeed,RpWarning,(int)Averagewinderspeed,0);
+            WinderReferenceSpeed = Averagewinderspeed;
+        }
+        WinderCalculation++;
+    }*/
+    //WinderReferenceSpeed = 1000;
+    //ScrewNumberOfSteps = 1000;
     screw_horizontal_speed = ScrewNumberOfSteps /  Rotations;//InternalWinderCfg.NumberOfRotationPerPassage;
-    if (Rotations > 6.6)//7.0)
-        Rotations = 6.0;
+  //  if (Rotations > 6.6)//7.0)
+  //      Rotations = 6.0;
     RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     //ROM_IntMasterDisable();
+    tempScrewSpeed = DEFAULT_SCREW_SPEED;
+
     CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+
     temp = SYS_CLK_FREQ;
     temp *= ScrewNumberOfSteps;
     temp /= tempScrewSpeed;
     if ((ScrewRunningTime != temp)||(ScrewSpeed != tempScrewSpeed))
     {
         ScrewSpeed = tempScrewSpeed;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
         ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
         //ROM_IntMasterEnable();
         //usnprintf(TempScrewStr, 100, "Winder: Horizon,Rotation, PPR, RPP{ %d, %d ,%d, %d} ",(int)screw_horizontal_speed,(int)RotationsPerSecond,(int)InternalWinderCfg.NumberOfRotationPerPassage,(int)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround);
-        //usnprintf(ScrewStr, 100, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",(int)ScrewNumberOfSteps,(int)ScrewSpeed,(int)temp,(int)WinderReferenceSpeed);
+        usnprintf(ScrewStr, 100, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",(int)ScrewNumberOfSteps,(int)ScrewSpeed,(int)temp,(int)WinderReferenceSpeed);
         //        Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
         //        #warning PID is now only proportional (above)
         //Report(TempScrewStr,__FILE__,__LINE__,0,RpWarning,0, 0);
-        //Report(ScrewStr,__FILE__,__LINE__,ScrewCurrentDirection,RpWarning,CalculationDirectionChangeCounter, 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);
     }
@@ -353,6 +377,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
         RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
         // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
         ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
 
         //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         usnprintf(ScrewStr, 100, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
@@ -458,20 +483,20 @@ 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)
     {
+        Read_Screw_Encoder();
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
       //  ScrewChangeCounter = 0;
       //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
@@ -481,7 +506,8 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    Rotations+=0.03;
+    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+    //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 91eedeb6b..ead318758 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -28,7 +28,7 @@ void ThreadSetBreakSensorLimit(int limit);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadConfigBreakSensor(void *request);
 
-uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
+double ThreadGetMotorSpeed(threadMotorsEnum MotorId);
 double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 3fb6f3af2..b741f33f3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -22,7 +22,7 @@
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
-int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+double 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};
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 25485290b..a285d4194 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -39,7 +39,7 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 
-uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
+double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
 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};
@@ -48,7 +48,7 @@ uint32_t    SpeedControlId=0xFF;
 uint32_t    PoolerSpeedControlId=0xFF;
 
 double DancerError[NUM_OF_DANCERS] = {0.0};
-int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
+double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 uint32_t JobCounter = 0;
 
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
@@ -186,6 +186,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev);
             SendJobProgress(0.0,0,false, Lenstr);
             Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+            length = 0;
 
         }
 
@@ -275,7 +276,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int index=MAX_THREAD_MOTORS_NUM;
     int32_t i, avreageSampleValue = 0;
     //double tempcalcspeed = 0;
-    uint32_t calculated_speed;
+    double calculated_speed;
     float speed = getSensorSpeedData();
     if (IfIndex>>8 != IfTypeThread)
     {
@@ -297,7 +298,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        if (fabs(calculated_speed-CurrentControlledSpeed[index])>2)
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
@@ -305,7 +306,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
  return OK;
 }
-uint32_t _speed;
+float _speed;
 uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     int index;
@@ -393,7 +394,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
-    uint32_t calculated_speed;
+    double calculated_speed;
     double NormalizedError;
 
     if (ThreadControlActive == false)
@@ -551,17 +552,17 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
 #ifndef TEST_PID_THREAD
-        if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+        if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
 #else
         if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
 #endif
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
-            if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
+            /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
             {
-                Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0);
-            }
+                Report("MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
+            }*/
 #ifdef TEST_PID_THREAD
             int len;
             if ((JobCounter % 2000) == index*100)
@@ -587,7 +588,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 #endif
         }
 		else
+		{
            MotorFailedSample[index]++;
+           //LOG_ERROR(index,"No change in speed");
+		}
 
     }
 
@@ -595,7 +599,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 }
 
 //********************************************************************************************************************
-uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
+double ThreadGetMotorSpeed(threadMotorsEnum MotorId)
 {
     return  CurrentControlledSpeed[MotorId];
 }