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

1 files changed, 30 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 0680e97a4..794559d57 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -168,6 +168,8 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__
 uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 {
     uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+    float screw_speed = 0;
+    float RotationsPerSecond;
 
     if (Counter)
     {
@@ -176,6 +178,15 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
             NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
             REPORT_MSG(ScrewNumberOfSteps, "Head Backing");
             ScrewNumberOfSteps--;
+            screw_speed = InternalWinderCfg.segmentoffsetpulses /  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 = dyeingspeed / (InternalWinderCfg.diameter * PI);
+            RotationsPerSecond =  CurrentControlledSpeed[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_speed*RotationsPerSecond;
+
+            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+
         }
         if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
         {
@@ -193,6 +204,9 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 }
 uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
+    uint32_t Steps;
+
+
     if (BusyfirstCall)
     {
         BusyfirstCall = false;
@@ -202,6 +216,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     if (BusyFlag == NOTBUSY)
     {
         CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
+        Steps = ScrewNumberOfSteps;
         if (ScrewCurrentDirection == false)
         {
             ScrewCurrentDirection = true;
@@ -210,7 +225,19 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         {
             ScrewCurrentDirection = false;
         }
-        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
+/*        if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
+        {
+            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");
+        }
+        else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT)
+        {
+            ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
+            REPORT_MSG(LIMIT, "Winder at left limit");
+        }
+*/
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
     }
 /*
  * calculate new ScrewSpeed and call     MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
@@ -234,7 +261,6 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     float screw_speed = 0;
     float RotationsPerSecond;
 
-    int process_speed = dyeingspeed;
     if (dyeingspeed == 0)
     {
         LOG_ERROR (dyeingspeed," job speed zero");
@@ -251,7 +277,8 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
         screw_speed = InternalWinderCfg.segmentoffsetpulses /  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 = process_speed / (InternalWinderCfg.diameter * PI);
+        RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI);
+        RotationsPerSecond =  CurrentControlledSpeed[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_speed*RotationsPerSecond;