arc head blowers PID

8 files changed, 161 insertions, 27 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c b/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
index e06962ca5..abbfcb4db 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
@@ -469,12 +469,12 @@ uint32_t CalculateTemperatures(TEMPERATURE_SENSOR_ID_ENUM SensorId, uint32_t Dat
             //bits to mvolts
             float temp1 = 3.3;//3.3V ref
             float temp2 = 8388608.0;//2^23 (24bit)
-            float temp3 = 100.0;// V_mult_by_100
+            float temp3 = 10000.0;// V_mult_by_100
 
             temp = Data & 0x00FFFFFF;//24bit
             temp *= temp1;
-            temp /= temp2;
             temp *= temp3;
+            temp /= temp2;
 
             Current_Measurement_C = (int32_t)(temp);///V_mult_by_100
         }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 7e3118c13..f8868a67b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -780,6 +780,9 @@ uint32_t MillisecLowLoop(uint32_t tick)
             }
             */
         }
+        if (Head_Type == HEAD_TYPE_ARC) {
+            HeadBlowersControlLoop();
+        }
 
         //call waste state machine
         Waste_StateMachine_OneSecond_Call();
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 4938035e6..4ff2e546c 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -506,6 +506,10 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
                 status += IDS_DispenserPidRequestMessage(request->pidcontrols[PID_i]);
             else if (request->pidcontrols[PID_i]->hardwarepidcontroltype == HARDWARE_PID_CONTROL_TYPE__WasteControl)
                 WHS_PidRequestMessage(request->pidcontrols[PID_i]);
+            else if (request->pidcontrols[PID_i]->hardwarepidcontroltype == HARDWARE_PID_CONTROL_TYPE__HeadBlower_1)
+                HeadBlowerPidRequestMessage(request->pidcontrols[PID_i], HEAD_FAN_RIGHT);
+            else if (request->pidcontrols[PID_i]->hardwarepidcontroltype == HARDWARE_PID_CONTROL_TYPE__HeadBlower_2)
+                HeadBlowerPidRequestMessage(request->pidcontrols[PID_i], HEAD_FAN_LEFT);
         }
     }
     else
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index d41b3c1a0..7490cef69 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -41,6 +41,7 @@ double dryerairflow = 5.0;
 double pressurebuildup = 0;
 double dryerzone1temp = 0;
 int32_t tableindex = 0;
+double headBlowersFlow[2] = {1.5,1.5};
 
 double dryerbufferMeters = 0;
 double dryerbufferCentimeters = 0;
@@ -303,9 +304,9 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams,bool saveData)
         {
             status |= HeaterCommandRequestMessage(HARDWARE_PID_CONTROL_TYPE__HeadCoverHeater2, false,ProcessParams->lblowertemp);
         }
+        headBlowersFlow[HEAD_FAN_RIGHT] = ProcessParams->rblowerflow;
+        headBlowersFlow[HEAD_FAN_LEFT] = ProcessParams->lblowerflow;
         Trigger_HeaterWriting();
-        Trigger_Head_Fan_Control(HEAD_FAN_RIGHT,ProcessParams->rblowerflow);
-        Trigger_Head_Fan_Control(HEAD_FAN_LEFT,ProcessParams->lblowerflow);
     }
 
     if (status)
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.h b/Software/Embedded_SW/Embedded/Modules/General/process.h
index 7cd592aad..f7f16d11e 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.h
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.h
@@ -19,6 +19,7 @@ extern double headairflow;
 extern double dryerairflow;
 extern double pressurebuildup;
 extern double dryerzone1temp;
+extern double headBlowersFlow[2];
 
 extern int32_t tableindex;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
index 015c5ea83..00d5a2a65 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
@@ -46,3 +46,4 @@ int HeadBlowersGetRPM(uint8_t fanId);
 double PressureSensorInit();
 void HeadBlowersInit();
 void HeadBlowersCfg();
+void HeadBlowersControlLoop ();
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 54e552684..e59d2e223 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -51,6 +51,8 @@
 /******************** Data Structures  ********************************************/
 #define OVERHEAT_COUNT_LIMIT 3
 #define UNDERHEAT_COUNT_LIMIT 3
+#define MIN_ALLOWED_PWM 0
+#define MAX_ALLOWED_PWM 255
 
  int Overheat_Count_Limit = OVERHEAT_COUNT_LIMIT;
  int Underheat_Count_Limit = UNDERHEAT_COUNT_LIMIT;
@@ -84,6 +86,20 @@ typedef struct
 HeaterControlConfig_t  HeaterPIDConfig[HEATER_TYPE_MAX_HEATERS] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 //PID_Config_Params   temp_params;
 
+typedef struct
+{
+    bool                m_isEnabled;
+    float               m_SetParam;
+    float               m_mesuredParam;
+    float               m_preError;
+    float               m_integral;
+    float               m_calculatedError;
+    bool                m_isReady;
+    int                 SamplesFilterSize;
+    PID_Config_Params   m_params;
+}HeadBlowerConfig;
+HeadBlowerConfig HeadBlowerControl[2] = {0,0};
+
 /******************** GLOBAL PARAMETERS  ********************************************/
 HeaterCommand          HeaterCmd[HEATER_TYPE_MAX_HEATERS];
 uint32_t    ControlIdtoHeaterId [HEATER_TYPE_MAX_HEATERS] =    {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
@@ -94,6 +110,7 @@ uint32_t    MainDryerHeaterMaxTempControl = 0xFF;
 uint32_t    SecondDryerHeaterMaxTempControl = 0xFF;
 uint32_t    DisasterControlId = 0xFF;
 double      PressureSensorV0[2] = {0.0, 0.0};
+uint8_t     HeadBlowersCloseLoopTime[2] = {2, 2};
 
 #define DRYER_AIR_PT100  TEMP_SENSE_ANALOG_DRYER_TEMP1
 #define DRYER_MAIN_PT100 TEMP_SENSE_ANALOG_DRYER_TEMP2
@@ -102,6 +119,7 @@ double      PressureSensorV0[2] = {0.0, 0.0};
 #define MAXIMUM_HEATER_READ 283
 #define PRESSURE_SENSOR_CP  0.225
 extern uint16_t Head_Fan_Tach[2];
+extern uint8_t Head_Fan_PWM_Command[2];
 
 bool UseSecondaryDrierHeater = true;
 
@@ -285,6 +303,7 @@ uint32_t HeatersDisasterControl(uint32_t x,uint32_t y);
 
 uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue);
 void HeatersStartControlTimer (void);
+int HeadBlowersGetPWM(uint8_t blowerId);
 
 //**********************************************************************
 //******************** CODE  *******************************************/
@@ -1901,6 +1920,115 @@ void HeadBlowersInit()
 {
     Trigger_Head_Fan_Control(HEAD_FAN_RIGHT, 0);
     Trigger_Head_Fan_Control(HEAD_FAN_LEFT, 0);
+
+    //PID parameters init
+    HeadBlowerControl[0].m_params.MAX = 200;
+    HeadBlowerControl[0].m_params.MIN = -200;
+    HeadBlowerControl[0].m_params.Kd = 0;
+    HeadBlowerControl[0].m_params.Kp = 350;
+    HeadBlowerControl[0].m_params.Ki = 600;
+    HeadBlowerControl[0].m_params.IntegralErrorMultiplier = 100;
+    HeadBlowerControl[0].m_params.ProportionalErrorMultiplier = 100;
+    HeadBlowerControl[0].m_params.epsilon = 0.2;
+    HeadBlowerControl[0].m_params.dt = 2;
+    //HeadBlowerControl[0].m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+    HeadBlowerControl[0].m_calculatedError = 0;
+    HeadBlowerControl[0].m_integral = 0;
+    HeadBlowerControl[0].m_isEnabled = true;
+    HeadBlowerControl[0].m_isReady = true;
+    HeadBlowerControl[0].m_mesuredParam = 0;
+    HeadBlowerControl[0].m_preError = 0;
+    HeadBlowerControl[0].m_SetParam = 5.0;//need to update SetParams on presegment stage
+
+    HeadBlowerControl[1].m_params.MAX = 200;
+    HeadBlowerControl[1].m_params.MIN = -200;
+    HeadBlowerControl[1].m_params.Kd = 0;
+    HeadBlowerControl[1].m_params.Kp = 350;
+    HeadBlowerControl[1].m_params.Ki = 600;
+    HeadBlowerControl[1].m_params.IntegralErrorMultiplier = 100;
+    HeadBlowerControl[1].m_params.ProportionalErrorMultiplier = 100;
+    HeadBlowerControl[1].m_params.epsilon = 0.2;
+    HeadBlowerControl[1].m_params.dt = 2;
+    //HeadBlowerControl[0].m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+    HeadBlowerControl[1].m_calculatedError = 0;
+    HeadBlowerControl[1].m_integral = 0;
+    HeadBlowerControl[1].m_isEnabled = true;
+    HeadBlowerControl[1].m_isReady = true;
+    HeadBlowerControl[1].m_mesuredParam = 0;
+    HeadBlowerControl[1].m_preError = 0;
+    HeadBlowerControl[1].m_SetParam = 5.0;//need to update SetParams on presegment stage
+}
+
+uint32_t HeadBlowerPidRequestMessage(void* request, int BlowerId)
+{
+    HardwarePidControl * PID_Request = request;
+
+    HeadBlowerControl[BlowerId].m_params.MAX = 200;
+    HeadBlowerControl[BlowerId].m_params.MIN = -200;
+    HeadBlowerControl[BlowerId].m_params.Kd = PID_Request->derivativetime;
+    HeadBlowerControl[BlowerId].m_params.Kp = PID_Request->proportionalgain;
+    HeadBlowerControl[BlowerId].m_params.Ki = PID_Request->integraltime;
+    HeadBlowerControl[BlowerId].m_params.IntegralErrorMultiplier = PID_Request->setpointramprateorsoftstartramp;
+    HeadBlowerControl[BlowerId].m_params.ProportionalErrorMultiplier = PID_Request->outputonoffhysteresisvalue;
+    HeadBlowerControl[BlowerId].m_params.epsilon = PID_Request->epsilon;
+    HeadBlowerControl[BlowerId].m_params.dt = PID_Request->controloutputtype;
+    //HeadBlowerControl.m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+    HeadBlowerControl[BlowerId].m_calculatedError = 0;
+    HeadBlowerControl[BlowerId].m_integral = 0;
+    HeadBlowerControl[BlowerId].m_isEnabled = true;
+    HeadBlowerControl[BlowerId].m_isReady = true;
+    HeadBlowerControl[BlowerId].m_mesuredParam = 0;
+    HeadBlowerControl[BlowerId].m_preError = 0;
+    HeadBlowerControl[BlowerId].m_SetParam = PID_Request->outputproportionalcycletime;//need to update SetParams on presegment stage
+    return OK;
+}
+
+uint32_t HeadBlowerPidFunc(double setParam,double measuredParam, int blowerId)
+{
+    int calculatedPwm;
+
+    HeadBlowerControl[blowerId].m_mesuredParam = measuredParam;
+    HeadBlowerControl[blowerId].m_SetParam = setParam;
+    HeadBlowerControl[blowerId].m_calculatedError = PIDAlgorithmCalculation((float)HeadBlowerControl[blowerId].m_SetParam , (float)HeadBlowerControl[blowerId].m_mesuredParam,
+                                                      &HeadBlowerControl[blowerId].m_params,   &HeadBlowerControl[blowerId].m_preError, &HeadBlowerControl[blowerId].m_integral);
+    calculatedPwm = HeadBlowersGetPWM(blowerId) + HeadBlowerControl[blowerId].m_calculatedError;
+
+    HeadBlowersCloseLoopTime[blowerId] = HeadBlowerControl[blowerId].m_params.dt;
+    Trigger_Head_Fan_Control(blowerId, calculatedPwm);
+    if (calculatedPwm < MIN_ALLOWED_PWM)
+        HeadBlowerControl[blowerId].m_integral = 0;
+    if (calculatedPwm > MAX_ALLOWED_PWM)
+        HeadBlowerControl[blowerId].m_integral = 0;
+
+    return OK;
+}
+
+bool HeadBlowerFlowControl(double Q_value, uint8_t blowerId)
+{
+    double currentFlow = 0.0;
+    int sensorId;
+
+    sensorId = (blowerId == HEAD_FAN_RIGHT)?(HEAD_PT100_ZONE_5_0X84_0):(HEAD_PT100_ZONE_7_0X86_0);
+    currentFlow = PressureSensorGetPressure(sensorId);
+    HeadBlowerPidFunc(Q_value,currentFlow, blowerId);
+    return OK;
+}
+
+void HeadBlowersControlLoop ()
+{
+    int blowerId;
+    static int count[2] = {0, 0};
+    if (GetMachineState() < MACHINE_STATE_WAIT_FOR_COOLER)
+        return; //do not start before controller is initialized and running
+
+    for (blowerId = 0; blowerId <= 1; blowerId++) {
+        if (count[blowerId] == HeadBlowersCloseLoopTime[blowerId]) {
+            count[blowerId] = 0;
+            HeadBlowerFlowControl(headBlowersFlow[blowerId], blowerId);
+        } else {
+            count[blowerId] += 1;
+        }
+    }
 }
 
 void HeadBlowersCfg()
@@ -1913,26 +2041,31 @@ void HeadBlowersCfg()
     }
 }
 
-double PressureSensorInit(int fanId)
+double PressureSensorInit(int blowerId)
 {
     int sensorId;
-    sensorId = (fanId == HEAD_FAN_RIGHT)?(HEAD_PT100_ZONE_5_0X84_0):(HEAD_PT100_ZONE_7_0X86_0);
-    PressureSensorV0[fanId] = MillisecGetTemperatures(sensorId);
-    PressureSensorV0[fanId] *= 10;
-    return PressureSensorV0[fanId];
+    sensorId = (blowerId == HEAD_FAN_RIGHT)?(HEAD_PT100_ZONE_5_0X84_0):(HEAD_PT100_ZONE_7_0X86_0);
+    PressureSensorV0[blowerId] = MillisecGetTemperatures(sensorId);
+    PressureSensorV0[blowerId] /= 10;
+    return PressureSensorV0[blowerId];
 }
 
-int HeadBlowersGetRPM(uint8_t fanId)
+int HeadBlowersGetRPM(uint8_t blowerId)
 {
-    Trigger_Head_Read_Tacho(fanId);
+    Trigger_Head_Read_Tacho(blowerId);
 
-    if (Head_Fan_Tach[fanId] == 0x1FFE) {
+    if (Head_Fan_Tach[blowerId] == 0x1FFE) {
         return 0;
     } else {
-        return 7864320/Head_Fan_Tach[fanId];
+        return 7864320/Head_Fan_Tach[blowerId];
     }
 }
 
+int HeadBlowersGetPWM(uint8_t blowerId)
+{
+    return Head_Fan_PWM_Command[blowerId];
+}
+
 double PressureSensorGetPressure(uint8_t SensorId)
 {
     double V0, Vm, Q, Cp;
@@ -1940,7 +2073,7 @@ double PressureSensorGetPressure(uint8_t SensorId)
     Cp = PRESSURE_SENSOR_CP;
     V0 = (SensorId == HEAD_PT100_ZONE_5_0X84_0)?(PressureSensorV0[0]):(PressureSensorV0[1]);
     Vm = MillisecGetTemperatures(SensorId);
-    Vm *= 10;
+    Vm /= 10;
     Q = sqrt(Vm - V0 + 22) * Cp;
     return Q;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index d983144f3..853f6913a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -814,8 +814,7 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         Task_sleep(2000);
 
         //response.progress = Fan_Click_Info.Product_ID;
-        //response.progress = Head_Fan_Tach[0];
-        response.progress = HeadBlowersGetRPM(HEAD_FAN_RIGHT);
+        response.progress = Head_Fan_Tach[0];
         response.has_progress = true;
 
     }
@@ -824,27 +823,19 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
     {
         Trigger_Head_Fan_Control(HEAD_FAN_LEFT, request->delay & 0xFF);
         Task_sleep(2000);
-        //response.progress = Head_Fan_Tach[1];
-        response.progress = HeadBlowersGetRPM(HEAD_FAN_LEFT);
+        response.progress = Head_Fan_Tach[1];
         response.has_progress = true;
     }
     else
     if ((request->amount & 0x0000FFF0) == 0xFAC0) //WHS Fan control
     {
         uint16_t fan_tacho = 0;
-        WHS_fan_num Fan_Id;
-        uint8_t Fan_Speed;
 
         if (WHS_Type == WHS_TYPE_NEW)
         {
-            Fan_Id = (request->amount & 0x0000000F)-2;//0-5
-            Fan_Speed = request->delay & 0x000000FF;
-            Trigger_SetWHSFanSpeed(Fan_Id, Fan_Speed);
+            Trigger_SetWHSFanSpeed(((request->amount & 0x0000000F)-2) , request->delay & 0x000000FF);
             Task_sleep(2000);
-            Trigger_WHSReadAllFanTacho();
-            Task_sleep(1000);
-            //fan_tacho = WHS_Get_fan_tach( (request->amount & 0x0000000F)-2);
-            fan_tacho = WHS_Fan_Tach_RPM[Fan_Id];
+            fan_tacho = WHS_Get_fan_tach( (request->amount & 0x0000000F)-2);
         }
         response.progress = fan_tacho;
         response.has_progress = true;