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

4 files changed, 48 insertions, 19 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 7780287fb..3a0b25ba8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -29,16 +29,6 @@ typedef struct
 }InternalWinderConfigStruc;
 
 
-typedef enum threadMotorsEnum
-{
-    FEEDER_MOTOR,
-    DRYER_MOTOR,
-    POOLER_MOTOR,
-    WINDER_MOTOR,
-    SCREW_MOTOR,
-    MAX_THREAD_MOTORS_NUM
-}threadMotorsEnum;
-
 #define MAX_THREAD_FEED_MOTORS WINDER_MOTOR+1
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 
@@ -50,6 +40,7 @@ extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
 extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
 //extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
+extern uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
 
 #define MAX_CONTROL_SAMPLES 10
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
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;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index df88da8c8..7405a91d9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -11,6 +11,16 @@
 #define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer
 #define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 //} DANCER_ENUM;
+typedef enum threadMotorsEnum
+{
+    FEEDER_MOTOR,
+    DRYER_MOTOR,
+    POOLER_MOTOR,
+    WINDER_MOTOR,
+    SCREW_MOTOR,
+    MAX_THREAD_MOTORS_NUM
+}threadMotorsEnum;
+
 extern double TotalProcessedLength;
 
 uint32_t InternalWindingConfigMessage(JobSpool* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index b6c1fea57..8d1ea20fa 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -127,8 +127,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
     initialpos = 0xFFFF;
 }
-double MotorSentData[100] = {0};
-uint32_t PosDif[100] = {0};
+double MotorSentData[102] = {0};
+uint32_t PosDif[102] = {0};
 
 
 int MotorDataIndex = 0;
@@ -181,7 +181,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     MotorDataIndex+=1;
-    if (MotorDataIndex == 99) MotorDataIndex = 0;
+    if (MotorDataIndex >= 97) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
     TotalProcessedLength+= (length/100);
@@ -360,7 +360,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 if (ReadBreakSensor()==ERROR)
                 {
                     //LOG_ERROR(index, "ReadBreakSensor Error");
-                    JobEndReason = JOB_DANCER_FAIL;
+                    JobEndReason = JOB_THREAD_BREAK;
                     //SendJobProgress(0.0,0,false, "ReadBreakSensor Error");
                    EndState(CurrentJob,"ReadBreakSensor Error" );
                 }
@@ -372,7 +372,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
             JobAbortedByUser = true;
-            JobEndReason = JOB_THREAD_BREAK;
+            JobEndReason = JOB_DANCER_FAIL;
             EndState(CurrentJob,Message );
         }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
@@ -404,7 +404,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 KeepReadValue = TranslatedReadValue;
             }
         }*/
-        if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
@@ -533,6 +533,7 @@ void SetOriginMotorSpeed(float process_speed)
                 / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius);
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
+        CurrentControlledSpeed[Motor_i] = (int) motor_speed;
     }
 }