handle heater off command, improves screw speed, improve diagnostics

3 files changed, 34 insertions, 6 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 5084114c6..3a0b25ba8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -40,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..30cbd4db3 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,18 @@ 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 +260,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 +276,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_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index b6c1fea57..75d54ef02 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -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;
     }
 }