updates for the 4 winders demo

4 files changed, 134 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index c912bca21..aea0dd93a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -42,6 +42,15 @@ typedef struct
     PID_Config_Params   m_params;
 }MotorControlConfig_t;
 
+#ifdef FOUR_WINDERS
+#define SCREW_2_Motor  HARDWARE_MOTOR_TYPE__MOTO_SPARE2_1
+#define SCREW_3_Motor  HARDWARE_MOTOR_TYPE__MOTO_SPARE1_1
+#define SCREW_4_Motor  HARDWARE_MOTOR_TYPE__MOTO_RDANCER
+#define Winder_2_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE2_2
+#define Winder_3_Motor HARDWARE_MOTOR_TYPE__MOTO_SPARE1_2
+#define Winder_4_Motor HARDWARE_MOTOR_TYPE__MOTO_RLOADARM
+#endif
+
 
 #define MAX_THREAD_FEED_MOTORS (WINDER_MOTOR+1)
 #define MAX_SYSTEM_DANCERS (HARDWARE_DANCER_TYPE__RightDancer+1)
@@ -84,6 +93,7 @@ uint32_t DancerConfigMessage(HardwareDancer * request);
 uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
 
 void SetOriginMotorSpeed(float process_speed);
+uint32_t UpdatePidDuringRun(HardwarePidControl *hardwarepidcontrol);
 
 uint32_t ThreadPrepare_Tension (int DancerId, double tension);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index c7ec8a58c..151886016 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -1344,10 +1344,16 @@ void ThreadLoadRequest(MessageContainer* requestContainer)
     responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size);
     container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
 
-
     int LoadStage = (int)request->dryerzone1temp;
-    LoadStages = LoadStage;
-    ThreadLoadStateMachine(LoadStage);
+    if (request->hardwarepidcontrol1!=NULL)
+    {
+        UpdatePidDuringRun(request->hardwarepidcontrol1);
+    }
+    else
+    {
+        LoadStages = LoadStage;
+        ThreadLoadStateMachine(LoadStage);
+    }
 
     /*if (status)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 420141d5b..bb838a5bd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -200,6 +200,11 @@ uint32_t Winder_Prepare(void *JobDetails)
         Winder_ScrewHoming = true;
         //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit");
         status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2,2000);
+#ifdef FOUR_WINDERS
+        status = MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, 5/*LS_SPARE2_2*/, NULL,2000);
+        status = MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, 5/*LS_SPARE1_2*/, NULL,2000);
+        status = MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, NULL,2000);
+#endif
     }
     return status;
 }
@@ -239,6 +244,11 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
     else
     {
         status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000);
+#ifdef FOUR_WINDERS
+        status |= MotorMoveWithCallback (SCREW_2_Motor,(1-MotorsCfg[SCREW_2_Motor].directionthreadwize), numOfSteps,  NULL,1000);
+        status |= MotorMoveWithCallback (SCREW_3_Motor,(1-MotorsCfg[SCREW_3_Motor].directionthreadwize), numOfSteps,  NULL,1000);
+        status |= MotorMoveWithCallback (SCREW_4_Motor,(1-MotorsCfg[SCREW_4_Motor].directionthreadwize), numOfSteps,  NULL,1000);
+#endif
     }
      //set motor location 0 here
         return status;
@@ -250,6 +260,11 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
 
     MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
+#ifdef FOUR_WINDERS
+    MotorStop(SCREW_2_Motor, Hard_Hiz);
+    MotorStop(SCREW_3_Motor, Hard_Hiz);
+    MotorStop(SCREW_4_Motor, Hard_Hiz);
+#endif
     Task_sleep(5);
 #ifdef READ_SCREW_ENCODER
     Reset_Screw_Encoder();
@@ -261,6 +276,11 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
 #endif
     SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+#ifdef FOUR_WINDERS
+    SetMotHome(SCREW_2_Motor);
+    SetMotHome(SCREW_3_Motor);
+    SetMotHome(SCREW_4_Motor);
+#endif
 
     ScrewCurrentDirection = false;
 #ifdef READ_SCREW_ENCODER
@@ -565,6 +585,11 @@ uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag)
     //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
     //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
     MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, WinderDistanceToSpoolEnded,2000);
+#ifdef FOUR_WINDERS
+    MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, NULL,2000);
+    MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, NULL,2000);
+    MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, NULL,2000);
+#endif
 
     return OK;
 }
@@ -594,6 +619,11 @@ uint32_t Winder_End(void)
     //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
     //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
     MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //bug #2709
+#ifdef FOUR_WINDERS
+    MotorAbortMovetoLimitSwitch (SCREW_2_Motor);
+    MotorAbortMovetoLimitSwitch (SCREW_3_Motor);
+    MotorAbortMovetoLimitSwitch (SCREW_4_Motor);
+#endif
     return OK;
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
@@ -655,7 +685,17 @@ void ScrewTimerInterrupt(int ARG0)
     {
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
+#ifdef FOUR_WINDERS
+        MotorSetDirection (SCREW_2_Motor, ScrewCurrentDirection);
+        MotorSetDirection (SCREW_3_Motor, ScrewCurrentDirection);
+        MotorSetDirection (SCREW_4_Motor, ScrewCurrentDirection);
+#endif
         MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+#ifdef FOUR_WINDERS
+        MotorSetSpeedDirect (SCREW_2_Motor, ScrewSpeed);
+        MotorSetSpeedDirect (SCREW_3_Motor, ScrewSpeed);
+        MotorSetSpeedDirect (SCREW_4_Motor, ScrewSpeed);
+#endif
 #ifdef READ_SCREW_ENCODER
         Read_Screw_Encoder();
         ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
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);