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

11 files changed, 375 insertions, 164 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 23560bd06..433f6210f 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -20,20 +20,16 @@
 #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>
-#include  <PMR/Diagnostics/StartDiagnosticsResponse.pb-c.h>
-#include  <PMR/Diagnostics/StopDiagnosticsRequest.pb-c.h>
-#include  <PMR/Diagnostics/StopDiagnosticsResponse.pb-c.h>
 #include  <PMR/Diagnostics/Event.pb-c.h>
 #include  <PMR/Diagnostics/EventType.pb-c.h>
 #include "PMR/debugging/DebugLogCategory.pb-c.h"
 
 
-char DiagnosticsToken[36] = {0};
-bool DiagnosticsActive = false;
 
 
 Task_Handle AlarmHandling_Task_Handle;
@@ -55,44 +51,6 @@ typedef struct AlarmHandlingMessage{
     uint8_t messageData[20];
 }AlarmHandlingMessageStruc;
 
-/******************** CODE  ********************************************/
-//**********************************************************************
-uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer)
-{
-    uint32_t status = NOT_SUPPORTED;
-    //MessageContainer responseContainer;
-
-//    ReportInitParams InitParams;
-
-    StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
-    strcpy (DiagnosticsToken, requestContainer->token);
-    DiagnosticsActive = true;
-
-    start_diagnostics_request__free_unpacked(request,NULL);
-    return status;
-}
-uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer)
-{
-
-    MessageContainer responseContainer;
-
-    StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
-
-    StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT;
-
-    DiagnosticsActive = false;
-
-    responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse,  requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_response__get_packed_size);
-    responseContainer.continuous = false;
-    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
-    size_t container_size = message_container__pack(&responseContainer, container_buffer);
-    free(responseContainer.data.data);
-    //USBCDCD_sendData(container_buffer, container_size,10);
-    SendChars(container_buffer, container_size);
-    //free (container_buffer);
-
-return OK;
-}
 
 void AlarmHandlingInit(void)
 {
@@ -121,7 +79,7 @@ void AlarmHandlingIterate(UArg arg0)
 
     return ;
 }
-void AlarmHandler_SendDiagnostics(void)
+uint32_t AlarmHandler_SendDiagnostics(uint32_t IfIndex, uint32_t ReadValue)
 {
     AlarmHandlingMessageStruc Message;
 
@@ -133,7 +91,7 @@ void AlarmHandler_SendDiagnostics(void)
         Mailbox_post(AlarmHandlingMsgQ , &Message, BIOS_NO_WAIT);
 
 
-    return ;
+    return OK;
 }
 /*typedef enum _EventType {
   EVENT_TYPE__ThreadBreak = 0,
@@ -219,40 +177,6 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
 
     return OK;
 }
-void SendDiagnostics(void)
-{
-    MessageContainer responseContainer;
-
-    StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT;
-
-    if (DiagnosticsActive == false)
-        return;
-
-    /*DiagnosticsMonitors *monitors;
-    size_t n_digitalpins;
-    DigitalPin **digitalpins;
-    extern bool FPGA_Gpi_Buf[MAX_GPI];
-
-void Read_FPGA_GPI_Rgisters();
-bool FPGA_Read_limit_Switches(FPGA_GPI_ENUM Limit_Switch);
-
-    size_t n_componentsstates;
-    ValueComponentState **componentsstates;
-    size_t n_events;
-    Event **events;*/
-
-
-    responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
-    responseContainer.continuous = true;
-
-    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
-    size_t container_size = message_container__pack(&responseContainer, container_buffer);
-    free(responseContainer.data.data);
-    //USBCDCD_sendData(container_buffer, container_size,10);
-//    SendChars(container_buffer, container_size);
-    //free (container_buffer);
-
-}
 /******************************************************************************
  *  ======== messageTsk ========
  *  Task for this function is created statically. See the project's .cfg file.
@@ -276,7 +200,7 @@ void AlarmHandlingTask(UArg arg0, UArg arg1)
                 AlarmHandlingLoop(Message.tick);
                 break;
             case AlarmHandlingSendDiagnostics:
-                SendDiagnostics();
+                DiagnosticCollection();
             default:
                 break;
         }
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
index 9e1be07e2..31adcd328 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
@@ -9,7 +9,15 @@
 #define MODULES_ALARMHANDLING_ALARMHANDLING_H_
 
 
-void AlarmHandler_SendDiagnostics(void);
+//void AlarmHandler_SendDiagnostics(void);
 
+extern char DiagnosticsToken[36];
+uint32_t AlarmHandler_SendDiagnostics(uint32_t IfIndex, uint32_t ReadValue);
+
+uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer);
+uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer);
+void SendDiagnostics(void);
+void DiagnosticsReset(void);
+void DiagnosticCollection(void);
 
 #endif /* MODULES_ALARMHANDLING_ALARMHANDLING_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandlingDiagnostics.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandlingDiagnostics.c
new file mode 100644
index 000000000..60a093216
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandlingDiagnostics.c
@@ -0,0 +1,245 @@
+/*
+ * AlarmHandlingDiagnostics.c
+ *
+ *  Created on: 26 june 2018
+ *      Author: shlomo
+ */
+#include "include.h"
+#include "Modules/General/GeneralHardware.h"
+
+#include "AlarmHandling.h"
+#include <driverlib/timer.h>
+#include <inc/hw_ints.h>
+
+#include "drivers/adc_sampling/adc.h"
+#include "Control/control.h"
+
+#include "drivers/Motors/Motor.h"
+#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/ids/ids_ex.h"
+
+#include "modules/thread/thread_ex.h"
+#include  <PMR/Diagnostics/StartDiagnosticsRequest.pb-c.h>
+#include  <PMR/Diagnostics/StartDiagnosticsResponse.pb-c.h>
+#include  <PMR/Diagnostics/StopDiagnosticsRequest.pb-c.h>
+#include  <PMR/Diagnostics/StopDiagnosticsResponse.pb-c.h>
+#include  <PMR/Diagnostics/Event.pb-c.h>
+#include  <PMR/Diagnostics/EventType.pb-c.h>
+#include "PMR/debugging/DebugLogCategory.pb-c.h"
+
+
+char DiagnosticsToken[36] = {0};
+bool DiagnosticsActive = false;
+
+
+
+/******************** CODE  ********************************************/
+//**********************************************************************
+#define DIAGNOSTICS_LIMIT 10
+int DiagnosticsIndex = 0;
+int DiagnosticCollectionLimit = 3;  //number of data samples to collect before sending to the host
+int DiagnosticLimit = eTenMillisecond; //frequency of data collection
+DigitalPin DigitalPinArray[1][DIAGNOSTICS_LIMIT];
+double dancer1angle[DIAGNOSTICS_LIMIT];
+double dancer2angle[DIAGNOSTICS_LIMIT];
+double dancer3angle[DIAGNOSTICS_LIMIT];
+
+double feedermotorfrequency[DIAGNOSTICS_LIMIT];
+double dryermotor[DIAGNOSTICS_LIMIT];
+double pollermotor[DIAGNOSTICS_LIMIT];
+double windermotor[DIAGNOSTICS_LIMIT];
+double screwmotor[DIAGNOSTICS_LIMIT];
+double mixertemperature[DIAGNOSTICS_LIMIT];
+double headzone1temperature[DIAGNOSTICS_LIMIT];
+double headzone2temperature[DIAGNOSTICS_LIMIT];
+double headzone3temperature[DIAGNOSTICS_LIMIT];
+double dryerzone1temperature[DIAGNOSTICS_LIMIT];
+double dryerzone2temperature[DIAGNOSTICS_LIMIT];
+double dryerzone3temperature[DIAGNOSTICS_LIMIT];
+
+double dispensermotorfrequency[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS];
+double dispenser1motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser2motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser3motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser4motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser5motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser6motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser7motorfrequency[DIAGNOSTICS_LIMIT];
+double dispenser8motorfrequency[DIAGNOSTICS_LIMIT];
+
+void DiagnosticCollection(void)
+{
+    int i;
+    if (DiagnosticsActive == false)
+        return;
+    dancer1angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(WINDER_DANCER, 0,0);
+    dancer2angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(POOLER_DANCER, 0,0);
+    dancer3angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(FEEDER_DANCER, 0,0);
+
+    feedermotorfrequency[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+    dryermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
+    pollermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
+    windermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER);
+    screwmotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    mixertemperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_MIXCHIP_TEMP);
+    headzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1);
+    headzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP2);
+    headzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP3);
+    dryerzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1);
+    dryerzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2);
+    dryerzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3);
+
+    for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
+    {
+        dispensermotorfrequency[DiagnosticsIndex][i] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1+i);
+    }
+     dispenser1motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
+     dispenser2motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2);
+     dispenser3motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3);
+     dispenser4motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4);
+     dispenser5motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5);
+     dispenser6motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6);
+     dispenser7motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7);
+     dispenser8motorfrequency[DiagnosticsIndex] = MotorGetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8);
+
+    for (i=0;i<1;i++)
+    {
+        DigitalPinArray[i][DiagnosticsIndex].has_port = true;
+        DigitalPinArray[i][DiagnosticsIndex].port = i;
+        DigitalPinArray[i][DiagnosticsIndex].has_value = true;
+        DigitalPinArray[i][DiagnosticsIndex].value = FPGA_Read_limit_Switches(i);
+    }
+
+    DiagnosticsIndex++;
+    if (DiagnosticsIndex >= DIAGNOSTICS_LIMIT) DiagnosticsIndex = DIAGNOSTICS_LIMIT;
+    if (DiagnosticsIndex >= DiagnosticCollectionLimit)
+        SendDiagnostics();
+
+}
+void DiagnosticsReset(void)
+{
+    DiagnosticsIndex = 0;
+
+}
+void SendDiagnostics(void)
+{
+    MessageContainer responseContainer;
+    StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT;
+    DiagnosticsMonitors DiagnosticsMonitor;
+    //if (DiagnosticsActive == false)
+        return;
+
+
+    DiagnosticsMonitor.n_dancer1angle =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_dancer2angle =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_dancer3angle =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_feedermotorfrequency =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_dryermotor =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_pollermotor =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_windermotor =  DiagnosticsIndex;
+    DiagnosticsMonitor.n_screwmotor = DiagnosticsIndex;
+    DiagnosticsMonitor.n_mixertemperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_headzone1temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_headzone2temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_headzone3temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_dryerzone1temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_dryerzone2temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.n_dryerzone3temperature = DiagnosticsIndex;
+    DiagnosticsMonitor.dancer1angle = dancer1angle;
+    DiagnosticsMonitor.dancer2angle = dancer2angle;
+    DiagnosticsMonitor.dancer3angle = dancer3angle;
+
+    DiagnosticsMonitor.feedermotorfrequency = feedermotorfrequency;
+    DiagnosticsMonitor.dryermotor = dryermotor;
+    DiagnosticsMonitor.pollermotor = pollermotor;
+    DiagnosticsMonitor.windermotor = windermotor;
+    DiagnosticsMonitor.screwmotor = screwmotor;
+    DiagnosticsMonitor.mixertemperature = mixertemperature;
+    DiagnosticsMonitor.headzone1temperature = headzone1temperature;
+    DiagnosticsMonitor.headzone2temperature = headzone2temperature;
+    DiagnosticsMonitor.headzone3temperature = headzone3temperature;
+    DiagnosticsMonitor.dryerzone1temperature = dryerzone1temperature;
+    DiagnosticsMonitor.dryerzone2temperature = dryerzone2temperature;
+    DiagnosticsMonitor.dryerzone3temperature = dryerzone3temperature;
+
+    DiagnosticsMonitor.dispensersmotorsfrequency = dispensermotorfrequency;
+
+    DiagnosticsMonitor.dispenser1motorfrequency =   dispenser1motorfrequency;
+    DiagnosticsMonitor.dispenser2motorfrequency =   dispenser2motorfrequency;
+    DiagnosticsMonitor.dispenser3motorfrequency =   dispenser3motorfrequency;
+    DiagnosticsMonitor.dispenser4motorfrequency =   dispenser4motorfrequency;
+    DiagnosticsMonitor.dispenser5motorfrequency =   dispenser5motorfrequency;
+    DiagnosticsMonitor.dispenser6motorfrequency =   dispenser6motorfrequency;
+    DiagnosticsMonitor.dispenser7motorfrequency =   dispenser7motorfrequency;
+    DiagnosticsMonitor.dispenser8motorfrequency =   dispenser8motorfrequency;
+
+
+    response.monitors = &DiagnosticsMonitor;
+    response.n_digitalpins = 0/**MAX_GPI*/;
+    //response.digitalpins = DigitalPinArray;
+    response.n_events = 0;
+    response.n_componentsstates = 0;
+
+    responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
+    responseContainer.continuous = true;
+
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+
+    DiagnosticsReset();
+    //USBCDCD_sendData(container_buffer, container_size,10);
+//    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+}
+uint32_t AlarmHandlingControlId = 0xff;
+uint32_t AlarmHandlingEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
+
+uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer)
+{
+    uint32_t status = NOT_SUPPORTED;
+    //MessageContainer responseContainer;
+
+//    ReportInitParams InitParams;
+
+    StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    strcpy (DiagnosticsToken, requestContainer->token);
+    DiagnosticsActive = true;
+    AlarmHandlingControlId = AddControlCallback(AlarmHandler_SendDiagnostics,DiagnosticLimit,AlarmHandlingEmptyCBFunction,0,0,0);
+    start_diagnostics_request__free_unpacked(request,NULL);
+    return status;
+}
+uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT;
+
+    DiagnosticsActive = false;
+    RemoveControlCallback(AlarmHandlingControlId,AlarmHandler_SendDiagnostics);
+    AlarmHandlingControlId = 0xFF;
+    responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse,  requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    //USBCDCD_sendData(container_buffer, container_size,10);
+    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+return OK;
+}
+
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index a00c23739..4b0c42412 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -266,7 +266,7 @@ uint32_t MillisecLoop(uint32_t tick)
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
     bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick;
-    Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
+    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
@@ -380,7 +380,6 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
-        AlarmHandler_SendDiagnostics();
     }
 
     if (O900Millisecond_Tick)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 8e999a668..1e087eda3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -227,6 +227,8 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
 }
 int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
 {
+    if (deviceId == 0xFF)
+        return ERROR;
     assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
     unsigned int key;
 
@@ -280,7 +282,7 @@ uint32_t ControlLoop(uint32_t tick)
     //check all callback units (state machine waiting for completion of a change)
     uint32_t Device_i;
     bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
-    Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
+    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
@@ -305,7 +307,7 @@ uint32_t ControlLoop(uint32_t tick)
                     else
                         LOG_ERROR (Device_i, "Invalid callback ptr");
                     break;
-                case eTenMilliSecond:
+                case eTenMillisecond:
                     if (Ten_msTick)
                     {
                         if(ControlArray[Device_i].ControlDataReadPtr)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 853393739..97b0e16a1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -16,7 +16,7 @@ typedef  uint32_t (* DataReadCBFunction)( uint32_t Parameter1, uint32_t Paramete
 typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
-    eTenMilliSecond = 10,
+    eTenMillisecond = 10,
     eHundredMillisecond = 100,
     eOneSecond = 1000
 }CTRL_TIMING_ENUM;
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_ex.h b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
index 4cee9b196..962f9b651 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_ex.h
@@ -10,5 +10,7 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
 void HeatingTestRequest(MessageContainer* requestContainer);
 void HeatingTestPollRequest(MessageContainer* requestContainer);
 
+bool HeaterGetOverTemperatureState(uint8_t HeaterId);
+
 uint32_t HeatersEnd(void);
 uint32_t Heaters_Init(void);
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..32509dfea 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -45,10 +45,15 @@ 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.
+#warning the PT100 for head 4 is number 5
+uint32_t HeaterId2PT100Id[MAX_HEATERS_NUM] = {TEMP_SENSE_ANALOG_DRYER_TEMP2,TEMP_SENSE_ANALOG_DRYER_TEMP1,TEMP_SENSE_ANALOG_DRYER_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP1,TEMP_SENSE_ANALOG_DYEINGH_TEMP2,TEMP_SENSE_ANALOG_DYEINGH_TEMP3,TEMP_SENSE_ANALOG_DYEINGH_TEMP5,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
@@ -131,7 +136,7 @@ uint32_t HeatersEnd(void)
             }
         }
         if (i == HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w) //DC Heaters
-            if (ControlIdtoHeaterId [i]!=0xFF)
+            if (DryerHeaterMaxTempControl)
             {
                 status |=RemoveControlCallback(DryerHeaterMaxTempControl ,HeaterMaxTempCBFunction);
                 ControlIdtoHeaterId [i]=0xFF;
@@ -236,7 +241,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;
@@ -282,7 +287,11 @@ void HeaterPrepareReady(void)
  * called by: Communication from host
  * initialized all global data
  */
-//DryerHeaterMaxTempControl = AddControlCallback( HeaterMaxTempCBFunction
+bool HeaterGetOverTemperatureState(uint8_t HeaterId)
+{
+    if (HeaterId > MAX_HEATERS_NUM) return false;
+    return HeaterMaxTempFlag[HeaterId];
+}
 uint32_t HeaterMaxTempCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
@@ -299,19 +308,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 +339,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 +365,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 +415,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 +441,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 +460,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 >=23)
     {
         HeatingTestSendResonse(0,false,GetHeaterState(index), DCInitialHeating[index],index, readValue,DCTimeSliceAllocation[index],
                            HeaterPIDConfig[index].m_calculatedError,"Standard DC");
@@ -468,6 +479,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 +501,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);
     }
 
 
@@ -565,33 +576,41 @@ uint32_t HeatersControlLoop(uint32_t tick)
                            0,0, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w].m_calculatedError, HeaterPIDConfig[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1].m_calculatedError,"First Control");
 
     }
-    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);
-    }
-    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);
-    }
-    else
+    if (InitialHeating == false)
     {
-        //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);
+        if (TimeSliceAllocation[SliceCounter] == HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w)
+        {
+            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)
+        {
+            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);
+        }
     }
-
     for ( DcHeaterId = HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1; DcHeaterId<= HARDWARE_PID_CONTROL_TYPE__MixerHeater;DcHeaterId++)
     {
         if (HeaterReady[DcHeaterId] == false)
@@ -599,12 +618,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++;
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 4b7648e96..092fbb252 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -16,6 +16,8 @@
 #include "drivers/motors/motor.h"
 #include "drivers/valves/valve.h"
 
+#include "modules/heaters/heaters.h"
+
 
 typedef struct
 {
@@ -252,6 +254,7 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
                  {
                      Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                      MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
+                     HeatingTestSendResonse(0,false,0, Dispenser_Mixer,DispenserId, segmentfirst_speed,SegmentId,JobBrushStopId,"Dispenser !!!!!!!!!!!");
                  }
          }
      }