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

1 files changed, 50 insertions, 18 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 7bc38c936..1eb49ef6d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -28,6 +28,8 @@
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
 DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 
+int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
+
 typedef struct
 {
     bool                m_isEnabled;
@@ -81,25 +83,42 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 
  return OK;
 }
-uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
+uint32_t ThreadControlCBFunction(uint32_t DancerId, 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;
-    uint32_t TranslatedReadValue = ReadValue - 8740;
+    int Pid_Id;
+    int32_t TranslatedReadValue, avreageSampleValue = 0;
+    double NormalizedError;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ThreadMotorIdToDancerId[i] == deviceID)
+        if (ThreadMotorIdToDancerId[i] == DancerId)
         {
             index = i;
             break;
         }
     if (index==MAX_THREAD_MOTORS_NUM)
     {
-        LOG_ERROR (deviceID, "No motor  for device");
+        LOG_ERROR (DancerId, "No motor  for device");
         return 0xFFFFFFFF;
     }
+
     if(MotorControlConfig[index].m_isEnabled )
     {
-        MotorControlConfig[index].m_mesuredParam = TranslatedReadValue;
+        Pid_Id = ThreadMotorIdToControlId[index];
+        TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;
+        MotorSamplePointer[index]++;
+        if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
+        for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+            avreageSampleValue += MotorSamples[index][i];
+        avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+        NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
+
+        MotorControlConfig[index].m_mesuredParam = avreageSampleValue;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
@@ -111,7 +130,7 @@ uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }
 
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].minmicrostep);
+        MotorSetSpeed(ThreadMotorIdToMotorId[index], MotorControlConfig[index].m_calculatedError,  MotorsCfg[index].microstep);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
 
@@ -140,16 +159,17 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
-    int Motor_i, HW_Motor_Id;
+    int Motor_i, HW_Motor_Id, Pid_Id;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
-            MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq;
-            MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq;
-            MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd;
-            MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp;
-            MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].ki;
+        Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
+            MotorControlConfig[Motor_i].m_params.MAX = MotorsControl[Pid_Id].outputproportionalpowerlimit*OriginalMotorSpd_2PPS[Motor_i];
+            MotorControlConfig[Motor_i].m_params.MIN = 0;
+            MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
+            MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
+            MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
             MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
@@ -158,12 +178,14 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
             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
+
+            MotorSetDirection(HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
     #ifdef DEBUG_TEST_FUNCTIONS
-            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
                 AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
-            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
+            else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
                 AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
     #else
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
@@ -184,13 +206,23 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-//    int process_speed = JobTicket->processparameters->dyeingspeed;
-    int process_speed = 500;
+    int Motor_i, HW_Motor_Id;
+
+    int process_speed = JobTicket->processparameters->dyeingspeed;
+
+    process_speed = 10; //debug
 
-    int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here
+    for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
+    {
+        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
+        //(Speed*uStep*PPR)/((2*PI*motor_Radius)
+        double motor_speed = (process_speed *  MotorsCfg[HW_Motor_Id].pulseperround *  MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
+
+        OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed;
+    }
 
     // set the new speed in the dryer motor to the speed of the new segment
-    MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed,  MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep);
+    MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[MOTOR_DRYER_DRIVING].microstep);
     MotorSetSpeed(MOTOR_RLOADING, 1, 1);
     MotorSetSpeed(MOTOR_LLOADING, 1,1);