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

1 files changed, 18 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 45ab2cb19..26616c141 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -41,11 +41,7 @@
 
 double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
-#ifndef DRIER_CONTROL_TEST
-TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
-#else
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
-#endif
 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF};
 uint32_t    SpeedControlId=0xFF;
@@ -121,8 +117,8 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 
   if (Current_Read < Previous_Read)
   {
-    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
     ReportWithPackageFilter(ThreadFilter,"Length rollover",__FILE__,__LINE__,(int)Current_Read,RpWarning,(int)Previous_Read,0);
+    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
   }
   else
     Time_Pass = Current_Read - Previous_Read;
@@ -500,6 +496,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }
         KeepReadValue = ReadValue;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+        if (abs(TranslatedReadValue) > 0x2000)
+        {
+            TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment
+        }
         if (index == POOLER_MOTOR)
         {
 			//pooler dancer is right sided: data is opposite
@@ -608,14 +608,14 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         DancerError[DancerId] = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
-        if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+        if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit
         {
             MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
         }
-        else
+        /*else
         {
             //KeepNormalizedError = NormalizedError;
-        }
+        }*/
         /*if ((JobCounter % 100) == 0)
         {
             //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit
@@ -742,6 +742,16 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     EnableIntersegment = JobTicket->enableintersegment;
     IntersegmentLength = JobTicket->intersegmentlength;
 
+    if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
+    {
+        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
+        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING;
+    }
+    else
+    {
+        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ;
+        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
+    }
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop);
 
     if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)