updates for the 4 winders demo

1 files changed, 75 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 2bdf1b17f..849d54a67 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -728,6 +728,14 @@ 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
             {
@@ -841,13 +849,14 @@ uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag)
 uint32_t Release_Right_TFU_Tension()
 {
     uint32_t status = OK;
+#ifndef FOUR_WINDERS
     if (RTFU_Up == true)
     {
         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* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000);
     }
-
+#endif
     return status;
 }
 int SecondFeederCorrection = 4;
@@ -886,6 +895,7 @@ uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
 uint32_t Adjust_Right_TFU_Tension(double tension)
 {
     uint32_t status = OK;
+#ifndef FOUR_WINDERS
     if (tension > 0.5) //0 = lower position, 1 = high position
     {
         if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
@@ -895,7 +905,7 @@ uint32_t Adjust_Right_TFU_Tension(double tension)
             Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0);
         }
     }
-
+#endif
     return status;
 }
 uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension)
@@ -1120,6 +1130,7 @@ uint32_t ThreadPrepareState(void *JobDetails)
 /////////////////////////////////////////////////////
         MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
+#ifndef FOUR_WINDERS
         if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
         {
             ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
@@ -1132,6 +1143,7 @@ uint32_t ThreadPrepareState(void *JobDetails)
             LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
             SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
         }
+#endif
         if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
         {
             ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
@@ -1145,6 +1157,7 @@ uint32_t ThreadPrepareState(void *JobDetails)
             PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
             PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
         }
+#ifndef FOUR_WINDERS
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
@@ -1159,6 +1172,7 @@ uint32_t ThreadPrepareState(void *JobDetails)
                 //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
 #endif
             }
+#endif
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
@@ -1196,7 +1210,57 @@ uint32_t ThreadPrepareState(void *JobDetails)
     //set 3 dancers to the profile positions
     return OK;
 }
+uint32_t UpdatePidDuringRun(HardwarePidControl *request)
+{
+    int Motor_i = MAX_THREAD_MOTORS_NUM,i;
+    double temp_dt;
 
+    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+    {
+        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
+        {
+            Motor_i = i;
+            break;
+        }
+    }
+    if (Motor_i == MAX_THREAD_MOTORS_NUM)
+        return ERROR;
+
+
+    if (request->derivativetime == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0);
+    }
+    if (request->proportionalgain == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0);
+    }
+    if (request->integraltime == true)
+    {
+        MotorControlConfig[Motor_i].m_params.Ki = request->integraltime;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0);
+    }
+    if (request->epsilon == true)
+    {
+        MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon;
+        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0);
+    }
+    if (request->has_controloutputtype == true)
+    {
+        MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype;
+        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,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0);
+    }
+//////////////////////////////////////////////////
+    return OK;
+}
 void SetOriginMotorSpeed(float process_speed)
 {
     int Motor_i, HW_Motor_Id;
@@ -1451,6 +1515,15 @@ char Endstr[150];
      for ( Motor_i = 0;Motor_i <= WINDER_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);
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);