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

1 files changed, 93 insertions, 33 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 79832120b..7ec0ed5e5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -46,13 +46,15 @@
 
 double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
-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};
 #ifdef FOUR_WINDERS
-Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
+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,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
+Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS};
+uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
 #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};
 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
-#endif
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF};
+#endif
 uint32_t    SpeedControlId=0xFF;
 uint32_t    PoolerSpeedControlId=0xFF;
 
@@ -432,7 +434,10 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
 uint16_t BreakSensorCounter = 0;
 uint16_t BreakSensorLatchCounter = 0;
 char TMessage[150];
-
+#ifdef FOUR_WINDERS
+char ATMessage[MAX_THREAD_MOTORS_NUM][150];
+int c = 0;
+#endif
 uint32_t checkBreakSensor(uint32_t index)
 {
     if (BreakSensorenabled == true)
@@ -550,7 +555,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 //#define MAX_CONTROL_SAMPLES 6
 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
-
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     int len;
@@ -621,6 +625,29 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 			//pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
         }
+#ifdef FOUR_WINDERS
+        if (index == WINDER_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_2_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_3_MOTOR)
+        {
+            c++;
+        }
+        if (index == WINDER_4_MOTOR)
+        {
+            c++;
+        }
+        if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR))
+        {
+            //pooler dancer is right sided: data is opposite
+            TranslatedReadValue = (-1*TranslatedReadValue);
+        }
+#endif
         if (index == POOLER_MOTOR)
         {
             //pooler dancer is right sided: data is opposite
@@ -650,6 +677,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             ThreadControlActive = false;
             //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
             JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
+#ifdef FOUR_WINDERS
+            if (DancerId>HARDWARE_DANCER_2)
+                JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0;
+#endif
             SendJobProgress(0.0,0,false, TMessage);
             //EndState(CurrentJob,TMessage );
             SendSegmentFail();
@@ -702,24 +733,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             }*`/
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (index != WINDER_MOTOR)
+        if (index < WINDER_MOTOR)
             calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
-        if (0)//(JobCounter % 1000 == 0)
-        //if (JobCounter < 100)//(FirstCalcInJob == true)
+        //if (0)//(JobCounter % 1000 == 0)
+#ifdef FOUR_WINDERS
+        if (JobCounter % 500 < 4)//(FirstCalcInJob == true)
         {
-            if (index == FEEDER_MOTOR)
+            if (index >= WINDER_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 %d",
+                len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index,
                                 TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
                                 (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));
-                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);
+                ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);
 
             }
+            JobCounter++;
         }
-
+#endif
 #ifndef TEST_PID_THREAD
         if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
 #else
@@ -735,14 +768,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 }*/
                 CurrentControlledSpeed[index] = calculated_speed;
                 MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
-#ifdef FOUR_WINDERS
-                if (index == WINDER_MOTOR)
-                {
-                    MotorSetSpeed(Winder_2_Motor, calculated_speed);
-                    MotorSetSpeed(Winder_3_Motor, calculated_speed);
-                    MotorSetSpeed(Winder_4_Motor, calculated_speed);
-                }
-#endif
             }
             else
             {
@@ -832,11 +857,32 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd;
 
     if(ThreadParams->winderp)
+    {
         MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp;
+#endif
+    }
     if(ThreadParams->winderi)
+    {
         MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi;
+#endif
+    }
     if(ThreadParams->winderd)
+    {
         MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd;
+#ifdef FOUR_WINDERS
+        MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd;
+        MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd;
+        MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd;
+#endif
+    }
     ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0);
     if(ThreadParams->bypassrockers)
         Thread_Rockers_Bypass = true;
@@ -1032,13 +1078,14 @@ uint32_t ThreadPrepareState(void *JobDetails)
     IntersegmentLength = JobTicket->intersegmentlength;
 
     PrepareWaitCount = 0;
+#ifndef FOUR_WINDERS
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
     ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0);
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
     ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0);
     status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension);
     ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0);
-
+#endif
     FirstCalcInJob = true;
     if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
     {
@@ -1098,6 +1145,11 @@ uint32_t ThreadPrepareState(void *JobDetails)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
+#ifdef FOUR_WINDERS
+        if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR;
+        if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR;
+        if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR;
+#endif
         MotorControlConfig[Motor_i].m_params.MAX = 1;
         MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
         MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
@@ -1116,7 +1168,6 @@ uint32_t ThreadPrepareState(void *JobDetails)
         MotorControlConfig[Motor_i].m_preError = 0;
         MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
-        HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
         temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
         MotorTiming[Motor_i] = (int)temp_dt;
@@ -1210,7 +1261,24 @@ uint32_t ThreadPrepareState(void *JobDetails)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
+#ifdef FOUR_WINDERS
+            if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
+            {
+                ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0);
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
+                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
+                    CurrentControlledSpeed[Motor_i] = 0;
+                    ControlIdtoMotorId[Motor_i] = 0xFF;
+                }
+#ifndef TEST_PID_THREAD
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+#endif
+#endif
+            }
     }
+    HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
 #ifdef TEST_PID_THREAD
     testDancersControl();
@@ -1274,7 +1342,7 @@ uint32_t UpdatePidDuringRun(HardwarePidControl *request)
 void SetOriginMotorSpeed(float process_speed)
 {
     int Motor_i, HW_Motor_Id;
-    for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++)
+    for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         //(Speed*uStep*PPR)/((2*PI*motor_Radius)
@@ -1509,7 +1577,7 @@ char Endstr[150];
          PoolerSpeedControlId = 0xFF;
      }
 
-     for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
      {
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {
@@ -1522,17 +1590,9 @@ char Endstr[150];
 
      }
 	 Task_sleep(100);
-     for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
      {
         MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
-#ifdef FOUR_WINDERS
-        if (Motor_i == WINDER_MOTOR)
-        {
-            MotorStop(Winder_2_Motor, Hard_Hiz);
-            MotorStop(Winder_3_Motor, Hard_Hiz);
-            MotorStop(Winder_4_Motor, Hard_Hiz);
-        }
-#endif
 
      }
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);