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

1 files changed, 51 insertions, 26 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index eb1b751bb..a88ce61a5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -259,32 +259,38 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         return 0xFFFFFFFF;
     }
     PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
-//    if (CurrentPosition == 0)
-//        return OK; //unusable data
-    if (Poolerinitialpos == 0xFFFF)
-    {
+    //if (PoolerCurrentPosition != 0)
+    //{
+        if (Poolerinitialpos == 0xFFFF)
+        {
+            PoolerPreviousPosition = PoolerCurrentPosition;
+            Poolerinitialpos = 0;
+        }
+        prevprev = PoolerPreviousPosition;
+        positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition);
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
         PoolerPreviousPosition = PoolerCurrentPosition;
-        Poolerinitialpos = 0;
-    }
-    prevprev = PreviousPosition;
-    positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition);
-    //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
-    PoolerPreviousPosition = PoolerCurrentPosition;
 
-    length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;
-    //if (InitialProcess == false)
-    {
-        PoolerTotalProcessedLength+= (length/100);
-        TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
-    }
-    if (length > 1000)
-    {
-        usnprintf(Lenstr, 100, "pooler length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev);
-        SendJobProgress(0.0,0,false, Lenstr);
-        Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
-        length = 0;
+        // total length = (position diff / full cycle) * pulley perimeter
+        //(positionDiff/pulseperround)*((2*PI*motor_Radius)
 
-    }
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+        length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;
+
+        if (length > 1000)
+        {
+            usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev);
+            SendJobProgress(0.0,0,false, Lenstr);
+            Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+            length = 0;
+
+        }
+
+    //}
+
+    length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;
+    PoolerTotalProcessedLength+= (length/100);
+    TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
 #ifndef FEEDER_LENGTH_CALCULATION
     CurrentProcessedLength+=length;
 
@@ -539,6 +545,25 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                         }
                         BreakSensorCounter = 0;
                     }
+                    if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
+                    {
+                        BreakSensorCounter++;
+                        BreakSensorLatchCounter++;
+                        if (BreakSensorCounter>=BreakSensordebouncetimemilli)
+                        {
+	                        //consider applying the debouce parameters later
+	                        usnprintf(TMessage, 60, "thread speed too low");
+	                        JobEndReason = JOB_THREAD_BREAK;
+	                        ThreadControlActive = false;
+	                        SendJobProgress(0.0,0,false, TMessage);
+	                        SendSegmentFail();
+	                        //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
+	                        //EndState(CurrentJob,"ReadBreakSensor Error" );
+	                        LOG_ERROR(index, "thread speed too low Error");
+	                        return OK;
+						}
+                    }
+
                 }
             }
         }
@@ -895,8 +920,8 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
     }
     /*else if (InitialProcess==true)
     {
-        ThreadUpdateProcessLength (dryerbufferlength,(void *)ThreadPreSegmentEnded);
-        REPORT_MSG (dryerbufferlength," ThreadPreSegmentState DTS length (sample)");
+        ThreadUpdateProcessLength (dryerbufferMeters,(void *)ThreadPreSegmentEnded);
+        REPORT_MSG (dryerbufferCentimeters," ThreadPreSegmentState DTS length (sample)");
         SegmentState = false;
         PreSegmentState = true;
         DTSState = false;
@@ -958,7 +983,7 @@ uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId)
 //********************************************************************************************************************
 uint32_t ThreadDistanceToSpoolState(void )
 {
-    seglength = dryerbufferlength;
+    seglength = dryerbufferMeters;
     REPORT_MSG (seglength,"ThreadDistanceToSpoolState");
 //#ifdef FEEDER_LENGTH_CALCULATION
     ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded);