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

2 files changed, 86 insertions, 80 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 305c0e767..fd577311e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -28,19 +28,22 @@
 
 bool Winder_ScrewHoming = false;
 
+#define MAX_WINDER_SPEED_CALCULATION 100
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
 
 bool ScrewCurrentDirection = false;  //holds current screw direction
-bool BusyfirstCall = true;           //Ignores first call after activating the screw - too early
+uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
+uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
+uint32_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
+uint8_t WinderMotorSpeedCounter=0;
+uint32_t WinderMotorSpeedRollOver=0;
 double ScrewSpeed = 0;
 double ScrewRunningTime = 0;
-bool ScrewDirection = false;
+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;
 //ScrewSpeed
 uint32_t ScrewControlId = 0xFF;
-uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
-uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw  - will be used to build the cone
 
 InternalWinderConfigStruc InternalWinderCfg = {0};
 void ScrewTimerInterruptInit(void);
@@ -138,7 +141,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewSpeed = 0;
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
-    DirectionChangeCounter = 0;
     REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback");
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
@@ -171,81 +173,68 @@ InternalWinderCfg.segmentoffsetpulses
   
 numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 */
-uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
+uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
-    uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+    uint32_t Steps,i,winderspeed=0;
+    double temp;
+    uint32_t WinderReferenceSpeed = OriginalMotorSpd_2PPS[WINDER_MOTOR];
     float screw_horizontal_speed = 0;
     float RotationsPerSecond;
 
-        if (Counter%InternalWinderCfg.spoolbackingrate == 0)
-        {
-            NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
-            ScrewNumberOfSteps--;
-            screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
-            // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
-            RotationsPerSecond =  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;
+    WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
+    if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
+    {
+        WinderMotorSpeedCounter=0;
+        WinderMotorSpeedRollOver++;
+    }
+
+    if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
+        return OK;
 
-            //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-            //REPORT_MSG(ScrewSpeed, "CalculateNumberOfSteps ScrewSpeed");
-            //REPORT_MSG(CurrentControlledSpeed[WINDER_MOTOR], "CalculateNumberOfSteps Winder speed");
-            CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+    ScrewCurrentDirection = 1-ScrewCurrentDirection;
+    CalculationDirectionChangeCounter++;
 
-        }
-        if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
+    if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
+    {
+        if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
-            if (direction == MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize)//because the direction is already reversed at the interrupt
-            {
-                NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate);
-                ScrewNumberOfSteps++;
-                REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
-            }
+            ScrewNumberOfSteps--;
+            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
 
         }
-    return NumberOfSteps;
-
-}
-uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
-{
-    uint32_t Steps;
-    double temp;
-    if (BusyfirstCall)
-    {
-        BusyfirstCall = false;
-		DirectionChangeCounter++;
-        return OK;
     }
-    //ScrewCurrentDirection: false moves out, true moves home
-    if (BusyFlag == NOTBUSY)
+    else //next time going back
     {
-        CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
-        Steps = ScrewNumberOfSteps;
-/*        if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
+        if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
-            ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
-            Steps += InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
-            REPORT_MSG(LIMIT, "Winder at  right limit");
+            ScrewNumberOfSteps++;
+            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
         }
-        else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT)
+    }
+    if (WinderMotorSpeedRollOver)
+    {
+        for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
         {
-            ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
-            REPORT_MSG(LIMIT, "Winder at left limit");
+            winderspeed+=WinderMotorSpeed[i];
         }
-*/
-        temp = SYS_CLK_FREQ;
-        temp *= Steps;
-        temp /= ScrewSpeed;
-        //LOG_ERROR(Steps,"ScrewDirectionChange - ScrewNumberOfSteps");
-
-        /*if (ScrewRunningTime != temp)
-        {
-            REPORT_MSG(temp, "ScrewDirectionChange ScrewRunningTime changed");
-        }*/
-        ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
-        //MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
+        winderspeed/=MAX_WINDER_SPEED_CALCULATION;
+        LOG_ERROR(winderspeed, "WinderSpeedUpdated");
+        WinderReferenceSpeed = winderspeed;
     }
+    screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
+    RotationsPerSecond =  WinderReferenceSpeed / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
+    ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
+    CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+    temp = SYS_CLK_FREQ;
+    temp *= ScrewNumberOfSteps;
+    temp /= ScrewSpeed;
+    if (ScrewRunningTime != temp)
+    {
+        LOG_ERROR(temp , "new winder speed");
+    }
+    ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
+
+    /********************************************************************************/
     return OK;
 
 }
@@ -308,22 +297,31 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         LOG_ERROR(ScrewSpeed,"Winder pre segment - ScrewSpeed");
         //MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         ScrewsStartControlTimer ();
-    }
+        ScrewControlId = AddControlCallback(Screw100msecDirectionChange, eHundredMillisecond,TemplateDataReadCBFunction,0,0,0);
+        ScrewDirectionChangeCounter = 1;
+        CalculationDirectionChangeCounter = 1;
+        memset (WinderMotorSpeed,0,sizeof(uint32_t) *MAX_WINDER_SPEED_CALCULATION);
+        WinderMotorSpeedCounter=0,WinderMotorSpeedRollOver=0;
+   }
     PreSegmentReady(Module_Winder,ModuleDone);
 
     return OK;
 }
 uint32_t Winder_End(void)
 {
-    int pend;
     //stop screw
     ScrewNumberOfSteps = 0;
     if (ScrewControlId != 0xFF)
-        RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
+    {
+        RemoveControlCallback(ScrewControlId,Screw100msecDirectionChange);
+        ScrewControlId = 0xFF;
+    }
     CurrentControlledSpeed[SCREW_MOTOR] = 0;
     ScrewsStopControlTimer();
+    //move the cart to the edge so the spool can be easily replaced
+    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL);
 
-    return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+    return OK;
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
@@ -374,24 +372,26 @@ void ScrewsStartControlTimer (void)
     Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0);
     return;
 }
+
 void ScrewTimerInterrupt(int ARG0)
 {
-    bool dir = ScrewCurrentDirection;
     ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     ROM_IntMasterDisable();
-    ScrewDirectionChange(0,NOTBUSY);
+    //ScrewDirectionChange(0,NOTBUSY);
+
     if (SCREW_TimerActivated == true)
     {
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        ScrewCurrentDirection = 1-ScrewCurrentDirection;
+//        ScrewCurrentDirection = 1-ScrewCurrentDirection;
         MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
+        ScrewDirectionChangeCounter++;
     }
     else
     {
         TimerDisable(Screw_timerBase, TIMER_A);
     }
-    //Report("ScrewTimerInterrupt duration, speed", __FILE__,__LINE__,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
+    //Report("ScrewTimerInterrupt dir, duration, speed", __FILE__,ScrewCurrentDirection,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
     //
     // Enable all interrupts.
     //
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index e189e2639..5390ed0a7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -582,8 +582,9 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             {
                 if (PoolerSpeedControlId != 0xFF)
                 {
-                    RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction);
-                    PoolerSpeedControlId = 0xFF;
+                    if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
+                        LOG_ERROR(Motor_i,"Remove Control Failed");
+                     PoolerSpeedControlId = 0xFF;
                 }
                 //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
                 PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
@@ -593,7 +594,8 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
-                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
+                        LOG_ERROR(Motor_i,"Remove Control Failed");
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                     CurrentControlledSpeed[Motor_i] = 0;
                 }
@@ -604,7 +606,8 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
-                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
+                        LOG_ERROR(Motor_i,"Remove Control Failed");
                     CurrentControlledSpeed[Motor_i] = 0;
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
@@ -614,7 +617,8 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
-                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
+                        LOG_ERROR(Motor_i,"Remove Control Failed");
                     CurrentControlledSpeed[Motor_i] = 0;
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
@@ -760,16 +764,18 @@ char Endstr[150];
 
      if (SpeedControlId != 0xFF)
      {
-         RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+         if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK)
+             LOG_ERROR(Motor_i,"RemoveControl Failed");
          SpeedControlId = 0xFF;
      }
      if (PoolerSpeedControlId != 0xFF)
      {
-         RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction);
+         if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
+             LOG_ERROR(Motor_i,"RemoveControl Failed");
          PoolerSpeedControlId = 0xFF;
      }
 
-     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
+     for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
      {
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {