merge

6 files changed, 142 insertions, 512 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index c5fffff60..7e5020336 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -22,10 +22,6 @@
 
 #include "MillisecTask.h"
 #include <driverlib/timer.h>
-#include <time.h>
-#include "Common/Utilities/Utils.h"
-#include "driverlib/hibernate.h"
-
 #include <Drivers/SSI_Comm/SSI_Comm.h>
 #include <Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h>
 #include <Drivers/SSI_Comm/Dancer/Dancer.h>
@@ -45,38 +41,20 @@
 #include "Drivers/I2C_Communication/I2C.h"
 
 #include "modules/thread/thread_ex.h"
-#include "modules/ifs/ifs.h"
 #include "modules/ids/ids_ex.h"
-#include "modules/waste/waste_ex.h"
 #include "modules/Diagnostics/Diagnostics.h"
 #include "Modules/General/MachineStatus.h"
 
+#include "drivers/Flash_Memory/Flash_Memory.h"
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/Flash_ram/FlashProgram.h"
 
+#include "third_party/fatfs/src/ffconf.h"
 #include "Common/SWUpdate/FileSystem.h"
 
 #include "Communication/Connection.h"
 #include "Modules/General/buttons.h"
-#include "Drivers/I2C_Communication/Head_Card/PT100/Head_PT100_ADC.h"
-
-#include <Drivers/I2C_Communication/WHS_Card/D_ADS122_PT100/WHS_PT100_ADC.h>
-#include <Drivers/I2C_Communication/I2C_Task.h>
-#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
-#include "Modules/Heaters/Heaters_ex.h"
-#include <Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.h>
-#include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.h>
-#include <Drivers/I2C_Communication/I2C_Task.h>
-
-#include "drivers/Uart_Comm/WHS_Controller_Comm/WHS_UART/WHS_Uart.h"
-#include "Drivers/Uart_Comm/WHS_Controller_Comm/Shinko/ACS-13AC5E3.h"
-
-#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
-#include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h>
-#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
-#include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
-#include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
-#include "Drivers/Uart_Comm/BTSR/BTSR.h"
+#include "Drivers/I2C_Communication/Head_Card/PT100/PT100_ADC.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -105,7 +83,7 @@ typedef struct MillisecMessage{
 }MillisecMessageStruc;
 
 //uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
-//uint32_t  TemperatureSensor_Data[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0};
+//uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
 //uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
 //uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
 //uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
@@ -125,7 +103,7 @@ MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
-MillisecMotorDataStruc PT100Data[MAX_MAIN_CARD_TEMP_SENS_ID] = {0};
+MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
 Mailbox_Handle          TenMillisecMsgQ = NULL;
@@ -135,6 +113,8 @@ static GateMutex_Handle gateMillisecDB;
 
 uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
 /******************** Functions  ********************************************/
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
+void CalculateVOCAlarms(void);
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
@@ -167,7 +147,7 @@ void MillisecInit(void)
         System_abort("Could not create USB Wait gate");
     }
 
-    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
+    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
     ROM_TimerConfigure(Millisec_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
     //TimerIntRegister(Millisec_timerBase, TIMER_A, Timer0Isr);    // Registering  isr
     ROM_TimerEnable(Millisec_timerBase, TIMER_A);
@@ -246,13 +226,13 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     return ;
 }
 uint32_t PT100Activity = 0;
-
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
 {
-    if (TempSensorId >= MAX_MAIN_CARD_TEMP_SENS_ID) return -1;
+    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
     PT100Activity++; //read request
     PT100Data[TempSensorId].Callback = Callback;
     PT100Data[TempSensorId].Active = true;
+
     return OK;
 }
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
@@ -290,19 +270,19 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
 }
 int MillisecFlushMsgQ(TimerMotors_t MotorId)
 {
-    MillisecMotorDataStruc MotorInfo = {0};
+    MillisecMotorDataStruc MotorData = {0};
     int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
     int i;
     if (pend)
     {
         for (i=0;i<pend;i++)
-            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
+            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
     }
    return pend;
 }
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
-    MillisecMotorDataStruc MotorInfo = {0};
+    MillisecMotorDataStruc MotorData = {0};
     //==========================
     if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
         (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
@@ -319,17 +299,17 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng
     //==========================
     if (MotorId >= NUM_OF_MOTORS) return -1;
     MotorActivity++;
-    MotorInfo.Callback = Callback;
-    MotorInfo.Data = Data;
-    MotorInfo.Length = Length;
-    MotorInfo.DataRequired = false;
+    MotorData.Callback = Callback;
+    MotorData.Data = Data;
+    MotorData.Length = Length;
+    MotorData.DataRequired = false;
     if (MotorsMsgQ[MotorId] != NULL)
-        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
+        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
     else return false;
 }
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
-    MillisecMotorDataStruc MotorInfo = {0};
+    MillisecMotorDataStruc MotorData = {0};
     if (MotorId >= NUM_OF_MOTORS) return -1;
 //==========================
     if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
@@ -347,12 +327,12 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
     //==========================
     MotorActivity++;
     MotorActivity++;
-    MotorInfo.Callback = Callback;
-    MotorInfo.Data = Data;
-    MotorInfo.Length = Length;
-    MotorInfo.DataRequired = true;
+    MotorData.Callback = Callback;
+    MotorData.Data = Data;
+    MotorData.Length = Length;
+    MotorData.DataRequired = true;
     if (MotorsMsgQ[MotorId] != NULL)
-        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
+        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
     else return false;
 }
 
@@ -428,11 +408,10 @@ uint32_t MillisecLoop(uint32_t tick)
         }
    }
     //FPGA_GetTempSensorBusy();
-   //int PT100Busy[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
+   //int PT100Busy[MAX_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
    if (PT100Activity)
    {
-
-       for (Sensor_i = 0;Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID;Sensor_i++)
+       for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            /*if (TempSensorResponse[Sensor_i].Busy == true)
            {
@@ -441,35 +420,19 @@ uint32_t MillisecLoop(uint32_t tick)
            }*/
            if (PT100Data[Sensor_i].Active == true)
            {
-               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
-               {
+               #ifndef  Use_Head_Card
                    TemperatureSendSensorDummyClk(Sensor_i);
-               }
+                #endif
                PT100Data[Sensor_i].Active = false;
                PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
                break; //  one PT100 activitiy per MS 
            }
            else if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
            {
-               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
-               {
+               #ifdef  Use_Head_Card
+                   HeadADCPT100_SendReadDataCommand(Sensor_i);
+               #endif
                    TemperatureSensorReadFromFPGA_Res(Sensor_i); //got the data from the FPGA
-               }
-               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID) - move to the function  control_HeadCard_PT100
-               {
-                   //HeadCard_Toggle_PT100(Sensor_i);
-                   //delay??
-                   //HeadADCPT100_SendReadDataCommand(Sensor_i);
-               }
-               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
-               {
-                   //for WHS...
-               }
-               else
-               {
-                   //TBD
-               }*/
-
                PT100Data[Sensor_i].WaitForData = false;
                PT100Data[Sensor_i].SyncRequired = true;
                if (PT100Data[Sensor_i].Callback)
@@ -478,23 +441,10 @@ uint32_t MillisecLoop(uint32_t tick)
            }
            else if (PT100Data[Sensor_i].SyncRequired == true)
            {
-               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
-               {
-                   TemperatureSensorSync(Sensor_i);
-               }
-               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID)  - NA move to the function  control_HeadCard_PT100
-               {
-                   //HeadADCPT100_StartSync(Sensor_i);
-               }
-               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
-               {
-                   //StartSync WHS...
-               }
-               else
-               {
-                   //TBD...
-               }*/
-
+               #ifdef  Use_Head_Card
+                   HeadADCPT100_StartSync(Sensor_i);
+               #endif
+               TemperatureSensorSync(Sensor_i);
 
                PT100Data[Sensor_i].SyncRequired = false;
                PT100Activity--;
@@ -505,37 +455,21 @@ uint32_t MillisecLoop(uint32_t tick)
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
    SaveLogData();
 #else
-
-    #ifdef test_RTFU_dancer
-       test_dancer_responce_RTFU();
-    #else
-#ifdef FOUR_WINDERS
-        Dancer_Data[HARDWARE_DANCER_0] = Read_Dancer_Position(HARDWARE_DANCER_0);
-        Dancer_Data[HARDWARE_DANCER_1] = Read_Dancer_Position(HARDWARE_DANCER_1);
-        Dancer_Data[HARDWARE_DANCER_2] = Read_Dancer_Position(HARDWARE_DANCER_2);
-        Dancer_Data[HARDWARE_DANCER_3] = Read_Dancer_Position(HARDWARE_DANCER_3);
-        Dancer_Data[HARDWARE_DANCER_4] = Read_Dancer_Position(HARDWARE_DANCER_4);
-#else
-        Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-        Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
-        Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
-#endif
-    #endif
-
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
+    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
 #endif
     return OK;
 }
-int TemperatureSum[MAX_HEAD_CARD_TEMP_SENS_ID];
-int TemperatureMin[MAX_HEAD_CARD_TEMP_SENS_ID];
-int TemperatureMax[MAX_HEAD_CARD_TEMP_SENS_ID];
-int TemperatureCount[MAX_HEAD_CARD_TEMP_SENS_ID];
-int TemperatureCalc[MAX_HEAD_CARD_TEMP_SENS_ID] = {2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1};
+int TemperatureSum[MAX_TEMPERATURE_SENSOR_ID];
+int TemperatureMin[MAX_TEMPERATURE_SENSOR_ID];
+int TemperatureMax[MAX_TEMPERATURE_SENSOR_ID];
+int TemperatureCount[MAX_TEMPERATURE_SENSOR_ID];
+int TemperatureCalc[MAX_TEMPERATURE_SENSOR_ID];
 void MillisecUpdateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId,int temperature)
 {
    //if(TemperatureCount[SensorId]++>=10)
    //    TemperatureCount[SensorId] = 0;
-    if ((temperature>28000)||(temperature<900))
-        return;
     TemperatureCount[SensorId]++;
     if (TemperatureMax[SensorId]<temperature) TemperatureMax[SensorId]=temperature;
     if (TemperatureMin[SensorId]>temperature) TemperatureMin[SensorId]=temperature;
@@ -547,15 +481,7 @@ int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
     TemperatureSum[SensorId]-=TemperatureMax[SensorId];
     TemperatureSum[SensorId]-=TemperatureMin[SensorId];
     calc = TemperatureSum[SensorId] / (TemperatureCount[SensorId]-2);
-    if (TemperatureSum[SensorId]>10)
-    {
 
-        if(abs(TemperatureMax[SensorId]-TemperatureMin[SensorId])>2000)
-        {
-            if ((Head_Type == HEAD_TYPE_FLAT_WITHOUT_CARD)||(SensorId!=TEMP_SENSE_AN_ENCLOSURETEMP3))
-                Report("Millisec Temp spike",__FILE__,SensorId,(int)TemperatureMax[SensorId],RpWarning,(int) TemperatureMin[SensorId],0);
-        }
-    }
     TemperatureSum[SensorId] = 0;
     TemperatureCount[SensorId] = 0;
     TemperatureMin[SensorId] = 30000;
@@ -564,133 +490,48 @@ int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
 }
 int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
 {
-    if (SensorId > WHS_PT100_4_0X82_1)
-        return 0;
     return TemperatureCalc[SensorId];
 }
-bool RapidPressureRead = true;
-float PressureSum[MAX_SYSTEM_DISPENSERS];
-float PressureMin[MAX_SYSTEM_DISPENSERS];
-float PressureMax[MAX_SYSTEM_DISPENSERS];
-float PressureCount[MAX_SYSTEM_DISPENSERS];
-float PressureCalc[MAX_SYSTEM_DISPENSERS];
-void MillisecUpdatePressures (int SensorId,float Pressure)
-{
-   //if(PressureCount[SensorId]++>=10)
-   //    PressureCount[SensorId] = 0;
-    PressureCount[SensorId]++;
-    if (PressureMax[SensorId]<Pressure) PressureMax[SensorId]=Pressure;
-    if (PressureMin[SensorId]>Pressure) PressureMin[SensorId]=Pressure;
-    PressureSum[SensorId]+=Pressure;
-}
-float MillisecCalculatePressures (int SensorId)
-{
-    float calc = 0;
-    PressureSum[SensorId]-=PressureMax[SensorId];
-    PressureSum[SensorId]-=PressureMin[SensorId];
-    calc = PressureSum[SensorId] / (PressureCount[SensorId]-2);
-
-    PressureSum[SensorId] = 0;
-    PressureCount[SensorId] = 0;
-    PressureMin[SensorId] = 30000;
-    PressureMax[SensorId] = -30000;
-    return calc;
-}
-float MillisecGetPressures (int SensorId)
-{
-    if (SensorId > MAX_SYSTEM_DISPENSERS)
-        return 0;
-    return PressureCalc[SensorId];
-}
-
-bool getRapidPressureRead(void)
-{
-    return RapidPressureRead;
-}
+bool RapidPressureRead = false;
 void setRapidPressureRead(bool value)
 {
     RapidPressureRead = value;
-    if (GetDiagnosticMode() == Diagnostic_Extreme_Mode)
-        RapidPressureRead = true;
 }
-uint16_t PumpCounter = 0;
-uint16_t realtimetest[101];
-uint16_t dbgflag[50];
-uint32_t dbttime[50];
-bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,TenMinutes_Tick,OneHourTick,Gradient_Tick;
-bool O700Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick;
 
+uint16_t PumpCounter = 0;
 uint32_t MillisecLowLoop(uint32_t tick)
 {
-    uint8_t Motor_i,Disp_i,temp;
+    uint8_t Motor_i,Disp_i,Heater_i,temp;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
-#ifdef USE_RFID_LOGIC
-    RFID_READER_ID readerID;
-#endif
+
     //static int temp=0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    //bool O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,O900Millisecond_Tick;
+    bool Ten_msTick, Hundred_msTick , m90msecTick, Onesecond_Tick,O900Millisecond_Tick,OneMinute_Tick,OneHourTick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
-    Fifty_msTick      =   (tick%eHundredMillisecond == 40)      ?true:false; //eFiftyMillisecond
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
-    m20msecTick  =   (tick%eHundredMillisecond == 20)  ?true:false;
-    //m70msecTick  =   (tick%eHundredMillisecond == 70)  ?true:false;
     m90msecTick  =   (tick%eHundredMillisecond == 90)  ?true:false;
-    O700Millisecond_Tick  =   (tick%eOneSecond == 700)           ?true:false;
-    O200Millisecond_Tick  =   (tick%eOneSecond == 200)           ?true:false;
-    O400Millisecond_Tick  =   (tick%eOneSecond == 400)           ?true:false;
-    O500Millisecond_Tick  =   (tick%eOneSecond == 500)           ?true:false;
-    O600Millisecond_Tick  =   (tick%eOneSecond == 600)           ?true:false;
-    O800Millisecond_Tick  =   (tick%eOneSecond == 800)           ?true:false;
-    //O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
-    Gradient_Tick  =   (tick%400 == 0)           ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
-    Tensecond_Tick  =   (tick%10000 == 0)           ?true:false;
     OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
-    TenMinutes_Tick  =   (tick%eTenMinutes == 0)           ?true:false;
     OneHourTick  =   (tick%eOneHour == 0)           ?true:false;
-    realtimetest[(tick%1000)/10]++;
-    dbttime[0] = msec_millisecondCounter;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
-    int StartPT100 = 0;
+
     //Screw_ENC_Velocity_to_DAC(); - for testing the screw enc
-    if (Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD)
-        StartPT100 = TEMP_SENSE_ANALOG_DRYER_TEMP1;
+
     if (Ten_msTick)
     {
-        //Speed_Data = Read_Speed_Sensor_TypeII();
+        //Speed_Data = Calculate_Speed_Sensor_Velocity();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
-        //if (Sensor_Read++ >= MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
+        //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
         if(Machine_Idle_Mode == true)
             Machine_Idle_Breathing_Led();
-        dbgflag[1]++;
-        dbttime[1] = msec_millisecondCounter;
-        Trigger_HeaterWriting();
-        dbgflag[2]++;
-        dbttime[2] = msec_millisecondCounter;
-        Read_Dryer_ENC_Position();
-    }
-    if (m20msecTick)
-    {
-        ADC0SS0Handler();
-    }
-    if(Fifty_msTick)
-    {
-        WHS_Read_GPI_Registers();
-        Trigger_PT100_Read();//call every 50mSec (minimum delay 30mSec)
-        //Set_HeadCard_PT100();//call every 50mSec (minimum delay 30mSec)
-
-        #ifdef USE_UART4_FOR_BTSR
-            //call every 50mSec
-            BTSR_State_Machine(RUFeeder1);
-        #endif
     }
     if (m90msecTick)
     {
-        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);
         }
@@ -698,13 +539,9 @@ uint32_t MillisecLowLoop(uint32_t tick)
     if (Hundred_msTick)
     {
         Speed_Data = Calculate_Speed_Sensor_Velocity();
-#ifndef EVALUATION_BOARD
         Read_Buttons_Reg();
-#endif
-        ///////////////////////////////////////////////////////////////////
-
-
-        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+        //Ink_Cart_Led();
+        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
             MillisecUpdateTemperatures (Sensor_i,TemperatureSensorRead(Sensor_i));
         }
@@ -716,65 +553,24 @@ uint32_t MillisecLowLoop(uint32_t tick)
                 //LOG_ERROR (1111, "Control_WD");
             }
         }
-        dbgflag[3]++;
-        dbttime[3] = msec_millisecondCounter;
         if (RapidPressureRead == true)
         {
             for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
             {
-                MillisecUpdatePressures(Disp_i, CalculateDispenserPressure(Disp_i));
+                CalculateDispenserPressure(Disp_i);
             }
             ADC_TriggerCollection();
         }
-
-        Trigger_InputsReading();
-
-        #ifdef USE_RFID_LOGIC
-            //every 100 m sec
-            for(readerID = READER_1; readerID < Max_Readers ; readerID++)
-            {
-                if(DiscoverRFIDTagEvery100mSec[readerID] == true)
-                {
-                    RFIDCallEvery100mSec(readerID);
-                    //break;
-                }
-            }
-        #endif
-    }
-    if (Gradient_Tick)
         DispensersCollectionCall();
-    if (O200Millisecond_Tick)
-    {
-
-        ulocaltime(umktime(&LocalTime)+1,&LocalTime);
-        HibernateCalendarSet(&LocalTime);
-        //ReportWithPackageFilter(ThreadFilter,"Time: ",__FILE__,LocalTime.tm_min,LocalTime.tm_sec,RpWarning,(int) msec_millisecondCounter,0);
 
-        Trigger_WHS_MAX11614_Read_allADC();
-        FPGA_GetAllDispensersValveBusyOCD();
-        Read_Dryer_Fan_Tacho();
-        temp = Read_Fans_Tacho();
-        DrawerFansStatus = temp & 0x1F;
-        SystemFansStatus = temp & 0xE0;
-    }
-    if (O400Millisecond_Tick)
-    {
-        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
-        {
-            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
-                continue;  //
-            if (isMotorConfigured(Motor_i))
-                MotorGetStatusFromFPGA(Motor_i);
-        }
     }
-    if ((O500Millisecond_Tick)&&(RapidPressureRead == false))
+
+    if ((O900Millisecond_Tick)&&(RapidPressureRead == false))
     {
         ADC_TriggerCollection();
     }
-    if (O600Millisecond_Tick)
+    if (Onesecond_Tick)
     {
-        Trigger_WHSReadAllFanTacho ();
-        DrierHeaterVoltageSetup();
         if (RapidPressureRead == false)
         {
             for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
@@ -782,94 +578,40 @@ uint32_t MillisecLowLoop(uint32_t tick)
                 CalculateDispenserPressure(Disp_i);
             }
         }
-    }
-    if (O700Millisecond_Tick)
-    {
-        Trigger_Heater_Current_Read();
-    }
-    if (O800Millisecond_Tick)
-    {
-        AlarmHandling_ControlTrigger(0,0);
-    }
-    if (Onesecond_Tick)
-    {
-        //char Lenstr[160];
-        //static int Counter = 0;
-        //MachineUpdateResponseFunc();
-        KeepAliveOneSecondCall();
-        //TemperatureListString(Lenstr);
-        //ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)Counter++,RpWarning,(int) msec_millisecondCounter,0);
-        dbgflag[4]++;
-        dbttime[4] = msec_millisecondCounter;
-        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+        FPGA_GetAllDispensersValveBusyOCD();
+        temp = Read_Fans_Tacho();
+        DrawerFansStatus = temp & 0x1F;
+        SystemFansStatus = temp & 0xE0;
+        MachineUpdateResponseFunc();
+        //KeepAliveOneSecondCall();
+        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
-            TemperatureCalc[Sensor_i] =  MillisecCalculateTemperatures ( Sensor_i);
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
+                continue;  //
+            if (isMotorConfigured(Motor_i))
+                MotorGetStatusFromFPGA(Motor_i);
         }
-        if (RapidPressureRead == true)
+        if (!(OneMinute_Tick))
         {
-            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+            Trigger_Heater_Current_Read();
+            /*for (Heater_i = 0;Heater_i < NUM_OF_CURRENT_HEATERS;Heater_i++)
             {
-                PressureCalc[Disp_i] = MillisecCalculatePressures(Disp_i);
-            }
+                Read_Heaters_Current(Heater_i);
+            }*/
         }
-        HeaterPrepareFailureCheck();
-        if (WHS_Type == WHS_TYPE_NEW)
+        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
-            //Trigger_WHS_PT100_Read_All();
-            WHS_Blower_Avarege(HEAD_FLOW_METER);
-            WHS_Blower_Avarege(NU_FLOW_METER);
-            WHS_Blower_Avarege(DRIER_FLOW_METER);
-            WHS_Start_Blower_Control_Closed_Loop ();
-
-            if (ReadingVocEverySec == true)//eOneSecond
-            {
-                Trigger_WHS_MAX11614_Read_Gas_Sensor();
-            }
-
-           /* static uint8_t Whs_emptying_cycle = 0;
-
-            // #warning TBD need to define the timing
-            if(Whs_emptying_cycle >= 2)
-            {
-                waste_seq_step1();// include 1Sec delay
-                Whs_emptying_cycle = 0;
-            }
-            else
-            {
-                Whs_emptying_cycle++;
-            }
-            */
-
-            if(Shinko_first_read < 2)//read the two steps once and than in cycle every 10 minutes, also will be used to read after updating the temperature
-            {
-                Shinko_Cycle_Comm();
-            }
-        }
-        if (Head_Type == HEAD_TYPE_ARC) {
-            HeadBlowersControlLoop();
-        }
-        else
-        {
-            FlatHeadAlarms();
+            TemperatureCalc[Sensor_i] =  MillisecCalculateTemperatures ( Sensor_i);
         }
-
-        //call waste state machine
-        Waste_StateMachine_OneSecond_Call();
-
-        //call IFS state machine
-        midTankStateMachine();
-        dbgflag[5]++;
-        dbttime[5] = msec_millisecondCounter;
-    }
-    if (Tensecond_Tick)
-    {
-	#ifdef USE_RFID_LOGIC
-	        RFIDCycleEvery1Sec();
-	#endif
     }
     if (OneMinute_Tick)
     {
-        MachineUpdateResponseFunc();
+        Trigger_MidTank_Pressure_Read();
+        /*for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+        {
+            Read_MidTank_Pressure_Sensor(Disp_i);
+        }*/
+//        MachineUpdateResponseFunc();
         /*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
@@ -878,35 +620,7 @@ uint32_t MillisecLowLoop(uint32_t tick)
                 MotorGetStatusFromFPGA(Motor_i);
         }*/
         midtankDisplay = 1-midtankDisplay;
-        /*if (WHS_Type == WHS_TYPE_UNKNOWN)
-            Gas_PPM_Info = Calculate_Gas_Power_Consumption();*/
-//            ReportWithPackageFilter(ThreadFilter,"waste tank calculate level",__FILE__,__LINE__,(int)(GetWHSWasteTankLevelMiliLiter()*1000),RpWarning,(int) msec_millisecondCounter,0);
-        //Trigger_WHS_MAX11614_Read_allADC();
-        if ((WHS_Type == WHS_TYPE_NEW) && (ReadingVocEverySec == false))//OneMinute_Tick
-        {
-            Trigger_WHS_MAX11614_Read_Gas_Sensor();
-        }
-#ifdef CONTROL_DEBUG
-        ResetControlTime();
-#endif
-    }
-    if (TenMinutes_Tick)
-    {
-        /*if (WHS_Type == WHS_TYPE_NEW)
-        {
-            waste_seq_step1();// include 1Sec delay           <- to open !!!!
-        }*/
-        if (WHS_Type == WHS_TYPE_NEW)
-        {
-            Shinko_Cycle_Comm();
-        }
-#ifndef RUN_AS_MAIN_JIG
-        //Trigger_MidTank_Pressure_Read();
-        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
-        {
-            Read_MidTank_Pressure_Sensor(Disp_i);
-        }
-#endif
+        Gas_PPM_Info = Calculate_Gas_Power_Consumption();
 
     }
     if (OneHourTick)
@@ -918,12 +632,6 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpActivation(900);
             PumpCounter = 0;
         }
-        MidTankReading();
-        if (WHS_Type == WHS_TYPE_NEW)
-        {
-            waste_seq_step1();// include 1Sec delay           <- to open !!!!
-        }
-        //Trigger_WHS_MAX11614_Read_Gas_Sensor();
     }
     //ROM_IntMasterEnable();
 
@@ -999,7 +707,7 @@ void MillisecLowTask(UArg arg0, UArg arg1)
 */
 /*uint32_t getTemperatureSensorData(int SensorId)
 {
-    assert (SensorId < MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID);
+    assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
     return TemperatureSensor_Data[SensorId];
 }*/
 
@@ -1090,7 +798,7 @@ void HundredMicroTimerInterrupt(int ARG0)
     StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer2;
     StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer3;
     /*-----------------------*/
-    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
+    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "\r\n%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
     //StoreBufferCounter+=(len+1);
     /*-----------------------*/
     dancer1sum+=dancer1;
@@ -1139,7 +847,7 @@ void MillisecLogInit(void)
     if (FileHandle == 0)
         Fresult = FR_DENIED;
     else
-        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS|FA_CREATE_ALWAYS);
+        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS);
     ROM_IntMasterEnable();
     return ;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 0d7c42ad6..4edc1a8ed 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -34,9 +34,7 @@ uint32_t getDrawerFansStatus(void);
 uint32_t getSystemFansStatus(void);
 uint8_t getGasReading(void);
 int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId);
-float MillisecGetPressures (int SensorId);
 void setRapidPressureRead(bool value);
-bool getRapidPressureRead(void);
 
 
 void MillisecInit(void);
@@ -50,6 +48,7 @@ void MillisecLogClose(void);
 
 #endif
 
+
 extern bool watchdogCriticalAlarm;
 
 extern Task_Handle Millisecond_Task_Handle;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index eac1f605a..6c1b647be 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -3,9 +3,44 @@
 
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
+	float error;
+	float derivative;
+	float output;
+
+	//Calculate P,I,D
+	error = _setPoint - _mesuredParam;
+
+	//In case of error too small then stop integration
+	if(fabs(error) > params->epsilon)
+	{
+		*_integral = *_integral + error*params->dt;
+	}
+
+	derivative = (error - *_pre_error)/params->dt;
+
+	output = params->Kp*error + params->Ki**_integral + params->Kd*derivative;
+
+	//Saturation Filter
+	if(output > params->MAX)
+	{
+		output = params->MAX;
+	}
+	else if(output < params->MIN)
+	{
+		output = params->MIN;
+	}
+
+	//Update error
+	*_pre_error = error;
+
+	return output;
+}
+
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
+{
     float error;
     float derivative;
-    float output = 0;
+    float output;
 
     //Calculate P,I,D
     error = _setPoint - _mesuredParam;
@@ -19,7 +54,7 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
     derivative = (error - *_pre_error)/params->dt;
 
     output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
-    //}
+
     //Saturation Filter
     if(output > params->MAX)
     {
@@ -35,7 +70,7 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 
     return output;
 }
-/*
+
 float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
     float error;
@@ -46,14 +81,14 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     error = _setPoint - _mesuredParam;
     //ITerm+= (ki * error);
     *_integral = *_integral + (error*params->Ki);
-    /`*  if(ITerm > outMax)
+    /*  if(ITerm > outMax)
       {
           ITerm= outMax;
       }
       else if(ITerm < outMin)
       {
           ITerm= outMin;
-      } *`/
+      } */
     if(*_integral > params->MAX)
     {
         *_integral = params->MAX;
@@ -65,7 +100,7 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     //  double dInput = (input - lastInput);
     derivative = error - *_pre_error;
 
-      /`*Compute PID Output*`/
+      /*Compute PID Output*/
     //  double output = kp * error + ITerm- kd * dInput;
     output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
 
@@ -84,4 +119,3 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
 
     return output;
 }
-*/
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 4f94f3612..4792be9ec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -17,5 +17,6 @@ typedef struct
 
 }PID_Config_Params;
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
 
 #endif /* MODULES_PIDALGO_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index e6da43178..b7d117efe 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -45,29 +45,17 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
+#include "Modules/General/GeneralHardware.h"
 
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 
-#include  <PMR/Diagnostics/EventType.pb-c.h>
-
-#include "drivers/FPGA/Full_Vme/FPGA_Programming_Up.h"
 #include "drivers/adc_sampling/adc.h"
-#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
-#include "drivers/FPGA/FPGA_SPI_Comm.h"
-
 #include "Modules/General/buttons.h"
-#include "Modules/General/GeneralHardware.h"
-#include "Modules/AlarmHandling/AlarmHandling.h"
-
-#include "StateMachines/Printing/PrintingSTM.h"
 
 #include "control.h"
 #include "MillisecTask.h"
-
-
-
-#define DURATION_LIMIT 6
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 /******************** Definitions  ********************************************/
 #define MAX_TANGO_CONTROL_DEVICES 100
 /******************** STRUCTURES AND ENUMs  ********************************************/
@@ -83,7 +71,6 @@ typedef struct
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
   uint32_t ControlTiming;
-  uint32_t LastCalled;
   char *Name;
 }ControlDeviceStruc;
 
@@ -111,7 +98,6 @@ ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
 #define MAX_BACKLOG_SIZE 100
 uint16_t ControlBacklog[MAX_BACKLOG_SIZE]={0};
-uint32_t ControlTime[MAX_TANGO_CONTROL_DEVICES]={0};
 uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
 uint32_t MaxHighDevices = 0xFF;
@@ -171,7 +157,6 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
-int FPGA_ReInit_Count = 0; ///avoid too many reinitializations of motors as happens when FPGA is corrupted
 uint32_t ControlActivityLed( uint32_t Parameter1)
 {
     static bool flag = false;
@@ -184,46 +169,9 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
         ACTIVITY_RED_LED_OFF; // Heaters indication - all the Heaters OFF
         if(FPGA_WD_Occurred == true)
         {
-            if (FPGABurningActive == false)
-            {
-                FPGA_WD_Occurred = false;
-                AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
-                if (JobIsActive())
-                {
-                    Report("Hardware Failure Error - abort job!",__FILE__,__LINE__,EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,RpError, 0,0);
-                    JobEndReason = JOB_MOTOR_ALARM;
-                    usnprintf(AlarmReasonStr, 100, "Hardware Failure Error");
-                    SendJobProgress(0.0,0,false, "Hardware Failure Error");
-                    AbortJob("FPGA Watchdog Error");
-                }
-                if (FPGA_ReInit_Count++<20)
-                {
-                    ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
-
-                    ACTIVITY_GREEN_LED_ON;
-                    MotorConfiguredTimeout = 100;
-
-                    FPGA_SetMotorsInit();
-                    Motor_ReconfigAllMotors();
-                }
-            }
+            ACTIVITY_GREEN_LED_ON;
+            HWConfigurationInit();
         }
-		else
-            ACTIVITY_GREEN_LED_OFF;
-
-        if(power.color == colorOFF) Pannel_Leds(POWER_ON_OFF,MODE_OFF);
-        if(jog.color   == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
-        if(load.color  == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
-        if(cart1.color == colorOFF) Pannel_Leds(CART_1,MODE_OFF);
-        if(cart2.color == colorOFF) Pannel_Leds(CART_2,MODE_OFF);
-        if(cart3.color == colorOFF) Pannel_Leds(CART_3,MODE_OFF);
-
-        if(power.color == colorON) Pannel_Leds(POWER_ON_OFF,MODE_ON);
-        if(jog.color   == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
-        if(load.color  == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
-        if(cart1.color == colorON) Pannel_Leds(CART_1,MODE_ON);
-        if(cart2.color == colorON) Pannel_Leds(CART_2,MODE_ON);
-        if(cart3.color == colorON) Pannel_Leds(CART_3,MODE_ON);
 
         if(power.color == fastBILNK)
             Pannel_Leds(POWER_ON_OFF,MODE_OFF);
@@ -411,7 +359,6 @@ uint32_t  AddControlCallback(char* Name, ControlCBFunction Callback, uint32_t Ct
     if (deviceId == 0xFF)
     {
         LOG_ERROR(deviceId, "Add Callback failed");
-        AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
         return 0xFF;
     }
     key = GateMutex_enter(gateControlDB);
@@ -436,11 +383,11 @@ int SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback
         Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
         if (RemoveControlCallback(GetControlDevice_i(),Callback)==OK)
         {
-            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
         }
         else
         {
-            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
             return ERROR;
         }
     }
@@ -454,11 +401,11 @@ int  SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
         Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
         if (RemoveControlCallback(GetControlLowDevice_i(),Callback)==OK)
         {
-            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
         }
         else
         {
-            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
             return ERROR;
         }
     }
@@ -586,50 +533,22 @@ uint32_t ControlLoop(uint32_t tick)
 
     return OK;
 }
-uint32_t prevtick = 0;
-#ifdef CONTROL_DEBUG
-void ResetControlTime(void)
-{
-    memset(ControlTime,0,sizeof(ControlTime));
-}
-#endif
 uint32_t ControlLowLoop(uint32_t tick)
 {
-    uint32_t skipped_ticks = 0;
-#ifdef CONTROL_DEBUG
-    uint32_t tempp,tempq,delta;
-    uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0;
-#endif
-    if (tick-prevtick>1)
-    {
-        skipped_ticks = tick-prevtick-1;
-        //if (tick-prevtick>10)
-         //   Report("ControlLowLoop skipped",__FILE__,tick,(int)prevtick,RpWarning,(int)skipped_ticks,0);
-    }
-    prevtick = tick;
-
     for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
     {
         if (ControlArray[ControlLowDevice_i].ControlActive)
         {
-            if (tick - ControlArray[ControlLowDevice_i].StartTick<=skipped_ticks)
-                continue;
-            if (tick - ControlArray[ControlLowDevice_i].LastCalled<=skipped_ticks)
+            if (tick == ControlArray[ControlLowDevice_i].StartTick)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
-            if (ControlArray[ControlLowDevice_i].StartTick == tick)
-                continue;
-            ControlArray[ControlLowDevice_i].LastCalled = tick;
-
-            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
+            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)==0) // run the control on exact intervals
             {
                 ControlBacklog[backlogindex]=ControlLowDevice_i;
                 if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                     backlogindex = 0;
-#ifdef CONTROL_DEBUG
-                tempp = HibernateRTCSSGet();
-#endif
+
                 if(ControlArray[ControlLowDevice_i].ControlDataReadPtr)
                     ControlDatalog[ControlLowDevice_i] = ControlArray[ControlLowDevice_i].ControlDataReadPtr( ControlArray[ControlLowDevice_i].Parameter1);
                 else
@@ -638,38 +557,9 @@ uint32_t ControlLowLoop(uint32_t tick)
                     ControlArray[ControlLowDevice_i].ControlCallbackPtr(ControlArray[ControlLowDevice_i].IfIndex, ControlDatalog[ControlLowDevice_i]);
                 else
                     LOG_ERROR (ControlLowDevice_i, "Invalid callback ptr");
-#ifdef CONTROL_DEBUG
-                tempq = HibernateRTCSSGet();
-                if (tempq < tempp)
-                {
-                  delta = (32768 - tempp) + tempq + 1;
-                }
-                else
-                  delta = tempq - tempp;
-                if (ControlTime[ControlLowDevice_i]<delta)
-                {
-                    ControlTime[ControlLowDevice_i] = delta;
-                }
-#endif
             }
         } //if control active
     }  //for
-#ifdef CONTROL_DEBUG
-    sys_ticks_end= msec_millisecondCounter;
-    if (sys_ticks_end-sys_ticks_start > DURATION_LIMIT)
-    {
-        Report("ControlLowLoop long",__FILE__,sys_ticks_end-sys_ticks_start,(int)sys_ticks_end,RpWarning,(int)sys_ticks_start,0);
-        for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
-        {
-            if (ControlTime[ControlLowDevice_i]>max)
-            {
-                max = ControlTime[ControlLowDevice_i];
-                dev = ControlLowDevice_i;
-            }
-        }
-        //Report(ControlArray[dev].Name,__FILE__,__LINE__,dev,RpWarning,max,0);
-    }
-#endif
     //ROM_IntMasterEnable();
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 4b6a80967..4b82aab30 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -17,11 +17,9 @@ typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
     eTenMillisecond = 10,
-    eFiftyMillisecond = 50,
     eHundredMillisecond = 100,
     eOneSecond = 1000,
     eOneMinute = 60000,
-    eTenMinutes = 600000,
     eOneHour = 3600000
 }CTRL_TIMING_ENUM;
 typedef enum