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

4 files changed, 184 insertions, 69 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 23560bd06..cfcae2da6 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -20,7 +20,9 @@
 #include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "drivers/FPGA/FPGA.h"
+#include "PMR/Hardware/HardwareDancerType.pb-c.h"
 
 #include "modules/thread/thread_ex.h"
 #include  <PMR/Diagnostics/StartDiagnosticsRequest.pb-c.h>
@@ -222,13 +224,105 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
 void SendDiagnostics(void)
 {
     MessageContainer responseContainer;
-
+    int i;
     StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT;
-
+    DiagnosticsMonitors DiagnosticsMonitor;
     if (DiagnosticsActive == false)
         return;
+    DiagnosticsMonitor.n_dancer1angle =  1;
+    DiagnosticsMonitor.n_dancer2angle =  1;
+    DiagnosticsMonitor.n_dancer3angle =  1;
+    DiagnosticsMonitor.n_feedermotorfrequency =  1;
+    DiagnosticsMonitor.n_dryermotor =  1;
+    DiagnosticsMonitor.n_pollermotor =  1;
+    DiagnosticsMonitor.n_windermotor =  1;
+    DiagnosticsMonitor.n_screwmotor = 1;
+    DiagnosticsMonitor.n_mixertemperature = 1;
+    DiagnosticsMonitor.n_headzone1temperature = 1;
+    DiagnosticsMonitor.n_headzone2temperature = 1;
+    DiagnosticsMonitor.n_headzone3temperature = 1;
+    DiagnosticsMonitor.n_dryerzone1temperature = 1;
+    DiagnosticsMonitor.n_dryerzone2temperature = 1;
+    DiagnosticsMonitor.n_dryerzone3temperature = 1;
+/*
+    DiagnosticsMonitor.dancer1angle =  Control_Read_Dancer_Position(WINDER_DANCER, 0,0);
+    DiagnosticsMonitor.dancer2angle =  Control_Read_Dancer_Position(POOLER_DANCER, 0,0);
+    DiagnosticsMonitor.dancer3angle =  Control_Read_Dancer_Position(FEEDER_DANCER, 0,0);
+
+    DiagnosticsMonitor.feedermotorfrequency = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+    DiagnosticsMonitor.dryermotor = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+    DiagnosticsMonitor.pollermotor = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+    DiagnosticsMonitor.windermotor = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+    DiagnosticsMonitor.screwmotor = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    DiagnosticsMonitor.mixertemperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_MIXCHIP_TEMP);
+    DiagnosticsMonitor.headzone1temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1);
+    DiagnosticsMonitor.headzone2temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP2);
+    DiagnosticsMonitor.headzone3temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP3);
+    DiagnosticsMonitor.dryerzone1temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1);
+    DiagnosticsMonitor.dryerzone2temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2);
+    DiagnosticsMonitor.dryerzone3temperature = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3);
 
+    DiagnosticsMonitor.n_dispenser1motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser2motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser3motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser4motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser5motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser6motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser7motorfrequency = 1;
+    DiagnosticsMonitor.n_dispenser8motorfrequency = 1;
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7);
+    DiagnosticsMonitor.dispenser1motorfrequency = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8);
+*/
+    DigitalPin DigitalPinArray[MAX_GPI];
+    for (i=0;i<MAX_GPI;i++)
+    {
+        DigitalPinArray[i].has_port = true;
+        DigitalPinArray[i].port = i;
+        DigitalPinArray[i].has_value = true;
+        DigitalPinArray[i].value = FPGA_Read_limit_Switches(i);
+    }
+    response.monitors = &DiagnosticsMonitor;
+    response.n_digitalpins = MAX_GPI;
+    response.digitalpins = &DigitalPinArray;
+    response.n_events = 0;
+    response.n_componentsstates = 0;
     /*DiagnosticsMonitors *monitors;
+     struct  _DiagnosticsMonitors
+{
+  size_t n_threadspeed;
+  double *threadspeed;
+  size_t n_headairflow;
+  double *headairflow;
+  size_t n_feedertension;
+  double *feedertension;
+  size_t n_pullertension;
+  double *pullertension;
+  size_t n_dryerairflow;
+  double *dryerairflow;
+  size_t n_windertension;
+  double *windertension;
+  size_t n_dispensersmotorsfrequency;
+  DoubleArray **dispensersmotorsfrequency;
+  size_t n_dispensersangularencoders;
+  DoubleArray **dispensersangularencoders;
+  size_t n_dispenserslinearpositions;
+  DoubleArray **dispenserslinearpositions;
+  size_t n_dispenserspressure;
+  DoubleArray **dispenserspressure;
+  size_t n_midtankpressure;
+  double *midtankpressure;
+  size_t n_filterdeltapressure;
+  double *filterdeltapressure;
+  size_t n_chillertemperature;
+  double *chillertemperature;
+};
+
     size_t n_digitalpins;
     DigitalPin **digitalpins;
     extern bool FPGA_Gpi_Buf[MAX_GPI];
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
index 757fa0412..7282974e6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters.h
@@ -65,7 +65,6 @@ typedef struct
 extern HeaterPIDControlConfig HeaterControl[MAX_HEATERS_NUM];
 extern HeaterControlConfig_t  HeaterPIDConfig[MAX_HEATERS_NUM];
 extern HeaterCommand          HeaterCmd[MAX_HEATERS_NUM];
-extern uint32_t HeaterId2PT100Id[MAX_HEATERS_NUM];
 extern uint32_t  Heater_timerBase;
 
 uint32_t HeaterRecalculateSharedHeatersParams(uint32_t deviceId, uint32_t new_outputproportionalpowerlimit);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index 315e4c1e7..1388c72f9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -47,7 +47,6 @@ typedef enum {
 /******************** GLOBAL PARAMETERS  ********************************************/
 HeaterPIDControlConfig HeaterControl[MAX_HEATERS_NUM] = {0,0,0,0,0,0,0,0,0,0,0,0};
 HeaterControlConfig_t  HeaterPIDConfig[MAX_HEATERS_NUM] = {0,0,0,0,0,0,0,0,0,0,0,0};
-uint32_t HeaterId2PT100Id[MAX_HEATERS_NUM] = {ANALOG_DRYER_TEMP1,ANALOG_DRYER_TEMP2,ANALOG_DRYER_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP1,TEMP_SENSE_ANALOG_DYEINGH_TEMP2,TEMP_SENSE_ANALOG_DYEINGH_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP4,ANALOG_MIXCHIP_TEMP};
 bool AcHeaterConfigured[MAX_AC_HEATERS] = {0,0,0};
 
 int      NumberOFSlicesInUse = 0;
@@ -352,12 +351,13 @@ uint32_t HeaterRecalculateSharedHeatersParams(uint32_t deviceId, uint32_t new_ou
    Heater1000Slices = HeaterControl[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].outputproportionalpowerlimit * NumberOFSlicesInUse / 100;
    Heater200aSlices = HeaterControl[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].outputproportionalpowerlimit * NumberOFSlicesInUse / 100;
 
-   if ((Heater1000Slices + Heater200aSlices +2)>NumberOFSlicesInUse)
+#warning temporary disable time slice control as we are working with one heater in run time
+/*   if ((Heater1000Slices + Heater200aSlices +2)>NumberOFSlicesInUse)
    {
        LOG_ERROR (NumberOFSlicesInUse, "proportional time slices too high too high");
        return ERROR;//NumberOFSlicesInUse = MAX_TIMESLICES;
    }
-
+*/
    //mark the time slices  for heaters operation as empty / Heater1000 / Heater 200
    memset (TimeSliceAllocation,0xFF,sizeof(TimeSliceAllocation));
    for (Slice_i = 0; Slice_i < Heater1000Slices;Slice_i++ ) TimeSliceAllocation[Slice_i] = HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w;
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index eb5736c1d..110d1918d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -45,10 +45,14 @@ typedef struct HeatersControlMessage{
 HeaterCommand          HeaterCmd[MAX_HEATERS_NUM];
 uint32_t    ControlIdtoHeaterId [MAX_HEATERS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
 uint32_t    DryerHeaterMaxTempControl = 0xFF;
-uint32_t    DryerInternalPT100Id = ANALOG_DRYER_TEMP2;
-
+#warning the PT100 id for the dryer control is number2 - dryer internal ambient sensor
+uint32_t HeaterId2PT100Id[MAX_HEATERS_NUM] = {TEMP_SENSE_ANALOG_DRYER_TEMP1,TEMP_SENSE_ANALOG_DRYER_TEMP2,TEMP_SENSE_ANALOG_DRYER_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP1,TEMP_SENSE_ANALOG_DYEINGH_TEMP2,TEMP_SENSE_ANALOG_DYEINGH_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP4,TEMP_SENSE_ANALOG_MIXCHIP_TEMP};
+uint32_t    DryerInternalPT100Id = TEMP_SENSE_ANALOG_DRYER_TEMP1;
 bool HeatersRestart = false;
 
+bool HeaterMaxTempFlag[MAX_HEATERS_NUM] = {false,false,false,false,false,false,false,false};
+bool HeaterControlFlag[MAX_HEATERS_NUM] = {true,true,true,true,true,true,true,true};
+
 uint32_t OutputProportionalSingleStep = 0;  //A/C Heaters step size from one decision point to another - in cpu clocks. 120000 = 1 millisecod
 uint32_t  Heater_timerBase = TIMER2_BASE;        //Timer handle
 uint32_t OutputProportionalCycleTime = 0; //A/C Heaters Cycle time in milliseconds - one for all heaters
@@ -236,7 +240,7 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
 
     //start thread control for all motors
     HeaterPIDConfig[HeaterId].m_params.MAX = HeaterControl[HeaterId].outputproportionalpowerlimit;
-    HeaterPIDConfig[HeaterId].m_params.MIN = .0;
+    HeaterPIDConfig[HeaterId].m_params.MIN = 0.0;
     HeaterPIDConfig[HeaterId].m_params.Kd = HeaterControl[HeaterId].kd;
     HeaterPIDConfig[HeaterId].m_params.Kp = HeaterControl[HeaterId].kp;
     HeaterPIDConfig[HeaterId].m_params.Ki = HeaterControl[HeaterId].ki;
@@ -299,19 +303,30 @@ uint32_t HeaterMaxTempCBFunction(uint32_t IfIndex, uint32_t readValue)
         LOG_ERROR (IfIndex, "Wrong  Interface ");
         return 0xFFFFFFFF;
     }
-    if (readValue >= HeaterControl[index].sensormaxvalue)
+    static int ttt=0;
+    ttt++;
+    if (ttt >=21)
+    {
+        HeatingTestSendResonse(0,false,GetHeaterState(index), InitialHeating,index, readValue,HeaterControl[index].sensormaxvalue*100,
+                               HeaterControl[index].sensorminvalue*100,"AC MaxTemp");
+        ttt=0;
+    }
+    if ((readValue/100) >= HeaterControl[index].sensormaxvalue)
     {
         LOG_ERROR (readValue, "Heater Over the max temperature, turned off");
         DeActivateHeater(index);
+        HeaterMaxTempFlag[index] = true;
         return OK;
     }
-    if (readValue <= (HeaterControl[index].sensormaxvalue-HeaterControl[index].sensorminvalue))
+    if ((readValue/100) <= (HeaterControl[index].sensormaxvalue-HeaterControl[index].sensorminvalue))
     {
         if (HeaterControl[index].sensorminvalue > 0)
         {
-            ActivateHeater(index);
+            HeaterMaxTempFlag[index] = false;
+            if (HeaterControlFlag[index] == true)
+                ActivateHeater(index);
         }
-        LOG_ERROR (readValue, "Heater Cooled Off max temperature, turned on");
+        LOG_ERROR ((readValue/100), "Heater Cooled Off max temperature, turned on");
         return OK;
     }
     return ERROR;
@@ -319,6 +334,7 @@ uint32_t HeaterMaxTempCBFunction(uint32_t IfIndex, uint32_t readValue)
 uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
+    double temperror = 0.0, correction = 0.0;
     /*char str[100];
     uint8_t len = 0;*/
     if (IfIndex>>8 != IfTypeHeaters)
@@ -344,11 +360,20 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
                            Temperature[0],Temperature[1],
                            HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"Standard");
 */
+    static int tt=0;
+    tt++;
+    if (tt >=21)
+    {
+        HeatingTestSendResonse(0,false,GetHeaterState(index), InitialHeating,index, readValue,correction,
+                           HeaterPIDConfig[index].m_calculatedError,"Standard AC");
+        tt=0;
+    }
     // check if the read value is within the proportional band
     if (InitialHeating)
     {
         if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
                 {
+                    HeaterControlFlag[index] = false;
                     DeActivateHeater(index);
                     //Heaters OFF until coming into the proportional band
                     /*
@@ -385,20 +410,18 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         HeaterPIDConfig[index].m_mesuredParam = readValue;
         if (readValue < ((HeaterCmd[index].targettemperatue * (100-HeaterControl[index].outputproportionalband))/100)) //below proportional band
         {
-            HeaterPIDConfig[index].m_calculatedError = 1.0;
-            HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+            HeaterRecalculateSharedHeatersParams(index, 100);
         }
         else if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
         {
-            HeaterPIDConfig[index].m_calculatedError = 0.0;
-            HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+            HeaterRecalculateSharedHeatersParams(index, 0);
         }
 //check only for the proportional band limits
-//        HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-//                                                                              &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-//        HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
-
-        //SetMotorFreq (index, HeaterControl[index].m_calculatedError);
+        temperror = (double)readValue/10000;
+        HeaterPIDConfig[index].m_mesuredParam = temperror;
+        HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
+                                                                              &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
+        correction = HeaterPIDConfig[index].m_calculatedError;
     }
 
 
@@ -413,6 +436,7 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
+    double temperror = 0.0, correction = 0.0;
     /*char str[100];
     uint8_t len = 0;*/
     if (IfIndex>>8 != IfTypeHeaters)
@@ -431,32 +455,14 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         LOG_ERROR (0, "unconfigured");
         return ERROR;
     }
-    /*static uint32_t Temperature[2] = {0};
-    if (index<2)
-        Temperature[index] =  readValue;
+/*
     HeatingTestSendResonse(0, false,GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w),GetHeaterState(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1),
                            Temperature[0],Temperature[1],
                            HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"Standard");
 */
-/*
- Data, 189, 2730, 0, False, True, 50, 0, Standard DC
-{
-Data, 428, 80, 2641, False, False, 3, 4000, Standard DC
-{
-  "HeaterGroupId": 0,
-  "Zone1Temp": 80,
-  "Zone2Temp": 2641,
-  "Heater1Active": false,
-  "Heater2Active": false,
-  "Heater1Percentage": 3,
-  "Heater2Percentage": 4000,
-  "InfoMessage": "Standard DC"
-}  void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message)
-
-}*/
     static int t=0;
     t++;
-    if (t >=11)
+    if (t >=22)
     {
         HeatingTestSendResonse(0,false,GetHeaterState(index), DCInitialHeating[index],index, readValue,DCTimeSliceAllocation[index],
                            HeaterPIDConfig[index].m_calculatedError,"Standard DC");
@@ -468,6 +474,7 @@ Data, 428, 80, 2641, False, False, 3, 4000, Standard DC
         //int temp = ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100);
         if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
                 {
+                    HeaterControlFlag[index] = false;
                     DeActivateHeater(index);
                     //Heaters OFF until coming into the proportional band
                     return OK;
@@ -489,23 +496,22 @@ Data, 428, 80, 2641, False, False, 3, 4000, Standard DC
     }
     if(HeaterPIDConfig[index].m_isEnabled && (HeaterPIDConfig[index].m_SetParam != 0))
     {
-        HeaterPIDConfig[index].m_mesuredParam = readValue;
         if (readValue < ((HeaterCmd[index].targettemperatue * (100-HeaterControl[index].outputproportionalband))/100)) //below proportional band
         {
-            HeaterPIDConfig[index].m_calculatedError = 1.0;
-            HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+            HeaterRecalculateHeaterParams(index, 100);
         }
         else if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
         {
-            HeaterPIDConfig[index].m_calculatedError = 0.0;
-            HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+            HeaterRecalculateHeaterParams(index, 0);
         }
+
 //check only for the proportional band limits
-//        HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-//                                                                              &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
+        temperror = (double)readValue/10000;
+        HeaterPIDConfig[index].m_mesuredParam = temperror;
+        HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
+                                                                              &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
+        correction = HeaterPIDConfig[index].m_calculatedError;
 //        HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
-
-        //SetMotorFreq (index, HeaterControl[index].m_calculatedError);
     }
 
 
@@ -567,29 +573,35 @@ uint32_t HeatersControlLoop(uint32_t tick)
     }
     if (TimeSliceAllocation[SliceCounter] == HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w)
     {
-        //If HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w should be active
-        //Activate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
-        ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
-        //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
+        if (HeaterMaxTempFlag[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w] == false)
+        {
+            //If HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w should be active
+            //Activate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
+            ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
+            //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
+        }
     }
     else if (TimeSliceAllocation[SliceCounter] == HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1)
     {
-        //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
-        //If HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1 should be active
-        //Activate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
-        ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
-        ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
+        if (HeaterMaxTempFlag[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1] == false)
+        {
+            //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
+            //If HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1 should be active
+            //Activate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
+            ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
+            ActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
+        }
     }
     else
     {
-        //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
-        //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
-        DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
+            //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w);
+            //DeActivate HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1);
+            DeActivateHeater (HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2);
     }
 
     for ( DcHeaterId = HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1; DcHeaterId<= HARDWARE_PID_CONTROL_TYPE__MixerHeater;DcHeaterId++)
@@ -599,12 +611,22 @@ uint32_t HeatersControlLoop(uint32_t tick)
         if (DCTimeSliceAllocation[DcHeaterId] > 0) //heater active
         {
             if (SliceCounter == 0)
-                ActivateHeater (DcHeaterId);
+                if (HeaterMaxTempFlag[DcHeaterId] == false)
+                {
+                    HeaterControlFlag[DcHeaterId] = true;
+                    ActivateHeater (DcHeaterId);
+                }
             else if (SliceCounter >= DCTimeSliceAllocation[DcHeaterId]) //turn off
+            {
+                HeaterControlFlag[DcHeaterId] = false;
                 DeActivateHeater (DcHeaterId);
+            }
         }
         else
+        {
+            HeaterControlFlag[DcHeaterId] = false;
             DeActivateHeater (DcHeaterId);
+        }
     }
     //handle the time sharing module
     SliceCounter++;