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

1 files changed, 51 insertions, 7 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index efcbd5030..e7a333cc2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -58,6 +58,10 @@ uint32_t JobCounter = 0;
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 
+int MotorTiming[MAX_THREAD_MOTORS_NUM];
+int MotorTimer[MAX_THREAD_MOTORS_NUM];
+
+
 uint32_t PreviousPosition = 0, CurrentPosition = 0;
 double CurrentRequestedLength = 0.0;
 double CurrentProcessedLength = 0.0;
@@ -506,6 +510,18 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     index = IfIndex&0xFF;
 
+    if (MotorTiming[index]>1)
+    {
+        MotorTimer[index]++;
+        if (MotorTimer[index]>=MotorTiming[index])
+        {
+            MotorTimer[index]=0;
+        }
+        else
+        {
+            return OK;
+        }
+    }
     if(MotorControlConfig[index].m_isEnabled )
     {
         //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
@@ -674,9 +690,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
+        //if (JobCounter % eHundredMillisecond == 50)
         if (FirstCalcInJob == true)
         {
-            if (index == FEEDER_MOTOR)
+            if (index == POOLER_MOTOR)
             {
                 FirstCalcInJob = false;
                 len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
@@ -833,7 +850,7 @@ uint32_t Release_Right_TFU_Tension()
     {
         Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0);
         RTFU_Up = false;
-        status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4, Release_Right_TFU_TensionCallback,1000);
+        status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/2, Release_Right_TFU_TensionCallback,1000);
     }
 
     return status;
@@ -859,6 +876,22 @@ uint32_t Adjust_Right_TFU_Tension(double tension)
 
     return status;
 }
+int PrepareWaitCount = 0;
+uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
+{
+    if (PrepareWaitCount)
+    {
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
+        PrepareWaitCount--;
+    }
+    if (PrepareWaitCount == 0)
+    {
+        PrepareState = false;
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
+        PrepareReady(Module_Thread,ModuleDone);
+    }
+    return OK;
+}
 uint32_t ThreadPrepare_Tension (int DancerId, double tension)
 {
     int current, request = (int)tension,movement;
@@ -892,6 +925,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
         return status;
     else
     {
+        PrepareWaitCount++;
         if (current < request) //go down
         {
             direction = MotorsCfg[HW_Motor_Id].directionthreadwize;
@@ -903,12 +937,12 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             movement = current - request;
         }
         MotorSetMaxSpeed (HW_Motor_Id, 500);
-        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000);
+        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000);
         status |= MCU_E2PromProgram(address,request);
     }
     usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d",
               DancerId,request,current,movement,direction,HW_Motor_Id,address);
-    ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0);
+    ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,PrepareWaitCount,0);
 
     return status;
 }
@@ -919,7 +953,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     JobTicket* JobTicket = JobDetails;
     uint32_t status = OK;
     CurrentSegmentId = 0;
-
+    float temp_dt = 0;
     JobCounter = 0;
     TotalProcessedLength = 0.0;
     PoolerTotalProcessedLength = 0.0;
@@ -1018,6 +1052,14 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             MotorControlConfig[Motor_i].m_mesuredParam = 0;
             MotorControlConfig[Motor_i].m_preError = 0;
             MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
+
+            temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+            MotorTiming[Motor_i] = (int)temp_dt;
+            if (MotorTiming[Motor_i])
+            {
+                MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+                ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
+            }
 //////////////////////////////////////////////////
             for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
             {
@@ -1107,7 +1149,8 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
 #ifdef TEST_PID_THREAD
     testDancersControl();
 #endif
-    PrepareReady(Module_Thread,ModuleDone);
+    if (PrepareWaitCount == 0)
+        PrepareReady(Module_Thread,ModuleDone);
     //set 3 dancers to the profile positions
     return OK;
 }
@@ -1188,7 +1231,8 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
     {
         SetOriginMotorSpeed(process_speed);
         ThreadControlActive = true;
-        PrepareState = false;
+        if (PrepareWaitCount == 0)
+            PrepareState = false;
         PullerSpeedIndex = 0;
         FeederSpeedIndex = 0;