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

1 files changed, 14 insertions, 3 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index a93387e0a..741fed7af 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -66,6 +66,7 @@ InternalWinderConfigStruc InternalWinderCfg = {0};
 #ifdef READ_SCREW_ENCODER
 uint32_t ScrewLocationLimitSwitch = 0,ScrewLocationStart = 0;
 uint32_t ScrewLocationRun[3];
+uint32_t ScrewLocationPrev[3];
 float WinderRunAverage = 0.0,WinderRunSum = 0.0;
 int WinderRunSamples = 0;
 #endif
@@ -305,7 +306,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 //            WinderMotorSpeedCounter=0;
         }
 //    }
-    if (flipflop == 0)
+    /*if (flipflop == 0)
     {
         MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
@@ -314,11 +315,11 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
     flipflop ++;
-
+    */
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
         return OK;
     //deley TODO
-    flipflop = 0;
+    //flipflop = 0;
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
@@ -342,6 +343,16 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
              Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0);
          }
      }
+     if (abs(ScrewLocationPrev[1]-ScrewLocationRun[1])>21)
+     {
+         Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[1],RpWarning,ScrewLocationRun[1], 0);
+     }
+     ScrewLocationPrev[1] = ScrewLocationRun[1];
+     if (abs(ScrewLocationPrev[0]-ScrewLocationRun[0])>21)
+     {
+         Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[0],RpWarning,ScrewLocationRun[0], 0);
+     }
+     ScrewLocationPrev[0] = ScrewLocationRun[0];
      //else
      //    Report("winder run error value" ,__FILE__,WinderRun,ScrewLocationRun[0],RpWarning,ScrewLocationRun[1], 0);