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

16 files changed, 309 insertions, 38 deletions

diff --git a/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c b/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
index 60873243c..c311e9787 100644
--- a/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
+++ b/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
@@ -428,6 +428,7 @@ uint32_t    DeleteRequestFunc(MessageContainer* requestContainer)
 
     Fresult = f_unlink(request->path);
 
+    REPORT_MSG(Fresult, "File Delete");
     responseContainer = createContainer(MESSAGE_TYPE__DeleteResponse,  requestContainer->token, false, &response, &delete_response__pack, &delete_response__get_packed_size);
     if (Fresult!= OK)
     {
diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
index de3f9288d..12d6bdeb6 100644
--- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
+++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
@@ -20,7 +20,7 @@ typedef struct
 } TangoVersion_t;
 
 
-TangoVersion_t _gTangoVersion = {1,3,7,7};
+TangoVersion_t _gTangoVersion = {1,3,7,8};
 #define BUILD_DATE __DATE__
 char Dat[50] = BUILD_DATE; 
  char _gTangoName [MAX_STRING_LEN] = "Tango01 ";//d
diff --git a/Software/Embedded_SW/Embedded/DataDef.h b/Software/Embedded_SW/Embedded/DataDef.h
index ec2def5a7..cf137a00a 100644
--- a/Software/Embedded_SW/Embedded/DataDef.h
+++ b/Software/Embedded_SW/Embedded/DataDef.h
@@ -16,6 +16,7 @@
 #define  HIZ_TIMEOUT 1000
 //#define DEMO_TEMPERATURE
 //#define TEST_LONGER_PID_THREAD
+//#define HUNDRED_MICROSECONDS_DANCER_READ
 
 #define MAX_STRING_LEN              255 //Embedded  version + filter.c
 
@@ -48,8 +49,9 @@ enum
 #define Cartridge_MidTank_OFF 0
 #define Atm_MidTank_ON        1
 #define Atm_MidTank_OFF       0
-#define Mixer_Head            0
-#define Mixer_Waste           1
+//waste valve reversed
+#define Mixer_Head            1
+#define Mixer_Waste           0
 
 
 #define NEW  0
diff --git a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c
index 3cc8d0d9e..6c692ed10 100644
--- a/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c
+++ b/Software/Embedded_SW/Embedded/Drivers/SSI_Comm/Dancer/Dancer.c
@@ -41,13 +41,13 @@ typedef struct  // 24bit
 
 SSI_ENC SSI_enc;
 
+#ifndef HUNDRED_MICROSECONDS_DANCER_READ
 uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1)
 {
     return DANCER_ENC[DancerId].Position;
     //return SSI_enc.Position;
 }
-
-
+#endif
 uint32_t Read_Dancer_Position (HardwareDancerType DancerId)
 {
     #ifdef EVALUATION_BOARD
diff --git a/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.c b/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.c
index c2b228ffb..af7f13a00 100644
--- a/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.c
+++ b/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.c
@@ -57,10 +57,10 @@ uint32_t ReadAppAndProgram(uint32_t ui32FlashStart,uint32_t ui32FileSize,void* b
     // Enter a loop to erase all the requested flash pages beginning at the
     // application start address (above the USB stick updater).
     //
-    for(ui32Idx = ui32FlashStart; ui32Idx < ui32FlashEnd; ui32Idx += 1024)
+    /*for(ui32Idx = ui32FlashStart; ui32Idx < ui32FlashEnd; ui32Idx += 1024)
     {
         ROM_FlashErase(ui32Idx);
-    }
+    }*/
 
     //
     // Enter a loop to read sectors from the application image file and
@@ -108,4 +108,27 @@ uint32_t ReadAppAndProgram(uint32_t ui32FlashStart,uint32_t ui32FileSize,void* b
     //
     return(1);
 }
+#warning flash erase is now done only in init. should consider adding before config changes
+uint32_t EraseFlashSection(uint32_t ui32FlashStart,uint32_t ui32FileSize)
+{
+    volatile uint32_t ui32Idx;
+    for(ui32Idx = ui32FlashStart; ui32Idx < ui32FlashStart+ui32FileSize; ui32Idx += 1024)
+    {
+        ROM_FlashErase(ui32Idx);
+    }
+    return OK;
+
+}
+//Each word may not be subject to more than a specific number of programming cycles before an
+//erase cycle is required. In other words, for any given word, FlashProgram can only be called
+//twice before FlashErase is called.
+void FlashInit(void)
+{
+    volatile uint32_t ui32Idx;
+    //FlashUsecSet(120);
+    for(ui32Idx = FLASH_RAM_BASE; ui32Idx < FLASH_RAM_BASE+FLASH_SIZE; ui32Idx += 1024)
+    {
+        ROM_FlashErase(ui32Idx);
+    }
+}
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.h b/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.h
index a21afd52b..411f812c1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.h
+++ b/Software/Embedded_SW/Embedded/Drivers/flash_ram/FlashProgram.h
@@ -15,7 +15,7 @@
 #define JOB_MAP_IN_FLASH   FLASH_RAM_BASE+0X4000
 #define GENHWCFG_MAP_IN_FLASH FLASH_RAM_BASE+0X10000
 #define OTHER_MAP_IN_FLASH FLASH_RAM_BASE+0X12000
-#define FLASH_SIZE 0X50000
+#define FLASH_SIZE 0X30000
 
 //*****************************************************************************
 //
@@ -26,6 +26,8 @@
 //
 //*****************************************************************************
 uint32_t ReadAppAndProgram(uint32_t ui32FlashStart,uint32_t ui32FileSize,void* buffer);
+void FlashInit(void);
+uint32_t EraseFlashSection(uint32_t ui32FlashStart,uint32_t ui32FileSize);
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Embedded.cfg b/Software/Embedded_SW/Embedded/Embedded.cfg
index b88b47cf0..ea7a370b8 100644
--- a/Software/Embedded_SW/Embedded/Embedded.cfg
+++ b/Software/Embedded_SW/Embedded/Embedded.cfg
@@ -55,7 +55,7 @@ Program.global.usb0 = Hwi.create(58, "&USBCDCD_hwiHandler", hwi5Params);*/
 
 var hwi6Params = new Hwi.Params();
 hwi6Params.instance.name = "timer1";
-Program.global.timer1 = Hwi.create(37, "&OneMilliSecondMillisecInterrupt", hwi6Params);
+Program.global.timer1 = Hwi.create(37, "&MillisecInterrupt", hwi6Params);
 
 var hwi17Params = new Hwi.Params();
 hwi17Params.instance.name = "timer3";
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index b23cf3fa1..f14ef8ff1 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -27,6 +27,8 @@
 
 
 #include <drivers/Flash_Memory/Flash_Memory.h>
+#include "drivers/Flash_ram/FlashProgram.h"
+
 #include <DataDef.h>
 #include "Sys_PinOut_Config/MCU_MAIN_pinout.h"
 
@@ -139,6 +141,8 @@ int main(void)
     ROM_SysTickIntEnable();
     ROM_SysTickEnable();
 
+//    FlashInit();
+
     Calculateinit();
 
 	PortFunctionInit();
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index a6399efbb..94c7614d1 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -124,8 +124,8 @@ AlarmStatusItem  AlarmState[MAX_SYSTEM_ALARMS];
 * or static variable whose declaration follows the pragma directive (only if using it without #define for example: #pragma location = MOTOR_MAP_IN_FLASH)
 * The variables must be declared either __no_init or const!
 */
-//AlarmHandlingItemStruc  *AlarmItem;
-const AlarmHandlingItemStruc  AlarmItem[MAX_SYSTEM_ALARMS]={
+AlarmHandlingItemStruc  *AlarmItem;
+const AlarmHandlingItemStruc  HardCodedAlarmItem[MAX_SYSTEM_ALARMS]={
  {eHundredMillisecond,LimitSwitchAlarm,LimitSwitchAlarmEmpty_1,0,false,true,DEBUG_LOG_CATEGORY__Error,0xFF,0,EVENT_TYPE__DISPENSER_1_EMPTY,"Dispenser 1 Empty"},
  {eHundredMillisecond,LimitSwitchAlarm,LimitSwitchAlarmEmpty_2,1,false,true,DEBUG_LOG_CATEGORY__Error,0xFF,0,EVENT_TYPE__DISPENSER_2_EMPTY,"Dispenser 2 Empty"},
  {eHundredMillisecond,LimitSwitchAlarm,LimitSwitchAlarmEmpty_3,2,false,true,DEBUG_LOG_CATEGORY__Error,0xFF,0,EVENT_TYPE__DISPENSER_3_EMPTY,"Dispenser 3 Empty"},
@@ -270,15 +270,15 @@ const AlarmHandlingItemStruc  AlarmItem[MAX_SYSTEM_ALARMS]={
  { eOneSecond, MotorAlarm, HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH    , HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH    , x_STATUS_UVLO         , FALSE , DEBUG_LOG_CATEGORY__Error , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_CLEANING_MECHANISM_MOTOR_UNDERVOLTAGE                ," MotorDyeingHeadCleaningUnderVoltage "        },                                           /* 5038 */
  { eOneSecond, MotorAlarm, HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD    , HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD    , x_STATUS_UVLO         , FALSE , DEBUG_LOG_CATEGORY__Error , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_CLEANING_HEAD_MOTOR_UNDERVOLTAGE           ," MotorDyeingHeadCleaningHeadUnderVoltage "    },                                           /* 5042 */
 
- { eOneSecond, CurrentAlarm, HEAD_ZONE_1  , HEAD_ZONE_1  , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_1_CURRENT_OUT_OF_RANGE           ," Head Zone 1 Heater Current Out Of Range "    },                                           /* 5018 */
- { eOneSecond, CurrentAlarm, HEAD_ZONE_2    , HEAD_ZONE_2    , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_2_CURRENT_OUT_OF_RANGE           ," Head Zone 2 Heater Current Out Of Range "    },                                           /* 5019 */
- { eOneSecond, CurrentAlarm, HEAD_ZONE_3    , HEAD_ZONE_3    , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_3_CURRENT_OUT_OF_RANGE           ," Head Zone 3 Heater Current Out Of Range "    },                                           /* 5020 */
- { eOneSecond, CurrentAlarm, HEAD_ZONE_4    , HEAD_ZONE_4    , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_4_CURRENT_OUT_OF_RANGE           ," Head Zone 4 Heater Current Out Of Range "    },                                           /* 5021 */
- { eOneSecond, CurrentAlarm, HEAD_ZONE_5_6    , HEAD_ZONE_5_6    , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_5_6_CURRENT_OUT_OF_RANGE         ," Head Zone 5-6 Heater Current Out Of Range "    },                                           /* 5022 */
- { eOneSecond, CurrentAlarm, MIXCHIP        , MIXCHIP        , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__MIXER_CURRENT_OUT_OF_RANGE                        ," Mixer Heater Current Out Of Range "    },                                           /* 6004 */
- { eOneSecond, CurrentAlarm, DRYER_CURRENT_1, DRYER_CURRENT_1, 1         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DRYER_HEATERS_ZONE_1_CURRENT_OUT_OF_RANGE         ," Drier Heater 1 Current Out Of Range "    },                                           /* 6004 */
- { eOneSecond, CurrentAlarm, DRYER_CURRENT_2, DRYER_CURRENT_2, 1         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DRYER_HEATERS_ZONE_2_CURRENT_OUT_OF_RANGE         ," Drier Heater 2 Current Out Of Range "    },                                           /* 6004 */
- {eOneSecond,DoNotPollAlarm,0,0,0,0,DEBUG_LOG_CATEGORY__Safety,0xFF,0,EVENT_TYPE__DYEING_HEAD_THERMAL_CUTOFF,"Dyeing head Thermal Cut-Off"},
+// { eOneSecond, CurrentAlarm, HEAD_ZONE_1  , HEAD_ZONE_1  , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_1_CURRENT_OUT_OF_RANGE           ," Head Zone 1 Heater Current Out Of Range "    },                                           /* 5018 */
+// { eOneSecond, CurrentAlarm, HEAD_ZONE_2    , HEAD_ZONE_2    , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_2_CURRENT_OUT_OF_RANGE           ," Head Zone 2 Heater Current Out Of Range "    },                                           /* 5019 */
+// { eOneSecond, CurrentAlarm, HEAD_ZONE_3    , HEAD_ZONE_3    , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_3_CURRENT_OUT_OF_RANGE           ," Head Zone 3 Heater Current Out Of Range "    },                                           /* 5020 */
+// { eOneSecond, CurrentAlarm, HEAD_ZONE_4    , HEAD_ZONE_4    , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_4_CURRENT_OUT_OF_RANGE           ," Head Zone 4 Heater Current Out Of Range "    },                                           /* 5021 */
+// { eOneSecond, CurrentAlarm, HEAD_ZONE_5_6    , HEAD_ZONE_5_6    , 312         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DYEING_HEAD_ZONE_5_6_CURRENT_OUT_OF_RANGE         ," Head Zone 5-6 Heater Current Out Of Range "    },                                           /* 5022 */
+// { eOneSecond, CurrentAlarm, MIXCHIP        , MIXCHIP        , 187         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__MIXER_CURRENT_OUT_OF_RANGE                        ," Mixer Heater Current Out Of Range "    },                                           /* 6004 */
+// { eOneSecond, CurrentAlarm, DRYER_CURRENT_1, DRYER_CURRENT_1, 1         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DRYER_HEATERS_ZONE_1_CURRENT_OUT_OF_RANGE         ," Drier Heater 1 Current Out Of Range "    },                                           /* 6004 */
+// { eOneSecond, CurrentAlarm, DRYER_CURRENT_2, DRYER_CURRENT_2, 1         , FALSE , DEBUG_LOG_CATEGORY__Warning , 0xFF , 2 , EVENT_TYPE__DRYER_HEATERS_ZONE_2_CURRENT_OUT_OF_RANGE         ," Drier Heater 2 Current Out Of Range "    },                                           /* 6004 */
+// {eOneSecond,DoNotPollAlarm,0,0,0,0,DEBUG_LOG_CATEGORY__Safety,0xFF,0,EVENT_TYPE__DYEING_HEAD_THERMAL_CUTOFF,"Dyeing head Thermal Cut-Off"},
 
  {eOneSecond,DoNotPollAlarm,0,0,0,0,DEBUG_LOG_CATEGORY__Warning,0xFF,0,EVENT_TYPE__THREAD_BREAK,"Thread Break"},
  {eOneSecond,DoNotPollAlarm,0,0,0,0,DEBUG_LOG_CATEGORY__Warning,0xFF,0,EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,"Thread Feeder Tension Control Failure"},
@@ -330,9 +330,10 @@ void AlarmHandlingInit(void)
         AlarmState[Alarm_i].EventPtr = NULL;
     }
 
-    //AlarmItem = (AlarmHandlingItemStruc *)MOTOR_MAP_IN_FLASH;
-    //ReadAppAndProgram(AlarmItem, sizeof(AlarmHandlingItemStruc)*MAX_SYSTEM_ALARMS, HardCodedAlarmItem);
-    //LOG_ERROR(AlarmItem[2].EventName,"Flash Test");
+    AlarmItem = (AlarmHandlingItemStruc *)ALARM_MAP_IN_FLASH;
+    EraseFlashSection(ALARM_MAP_IN_FLASH,sizeof(AlarmHandlingItemStruc)*MAX_SYSTEM_ALARMS);
+    ReadAppAndProgram(AlarmItem, sizeof(AlarmHandlingItemStruc)*MAX_SYSTEM_ALARMS, HardCodedAlarmItem);
+    LOG_ERROR(AlarmItem[2].EventName,"Flash Test");
 
     return;
 }
@@ -394,7 +395,7 @@ uint32_t AlarmHandlingConsequentActions(uint32_t AlarmId, DebugLogCategory Sever
     case DEBUG_LOG_CATEGORY__Critical:
     case DEBUG_LOG_CATEGORY__Safety:
 
-        watchdogCriticalAlarm = true;
+//        watchdogCriticalAlarm = true;
         LOG_ERROR(AlarmId,"Critical Alarm - Watchdog activated to reset  hardware");
 /*
         //stop all dispensers
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 968e52d7d..a580dce02 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -40,6 +40,13 @@
 #include "modules/thread/thread_ex.h"
 #include "modules/ids/ids_ex.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"
 
 Task_Handle Millisecond_Task_Handle;
@@ -147,7 +154,11 @@ void MillisecStop(void)
     MillisecRestart = false;
     ADCAcquireStop();
 }
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+#define HIGH_TASK_FREQUENCY 12000
+#else
 #define HIGH_TASK_FREQUENCY 120000
+#endif
 void MillisecStart(void)
 {
     MillisecRestart = true;
@@ -164,6 +175,8 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc MillisecMessage;
     uint32_t Tenmsec_millisecondCounter = 0;
+#ifndef HUNDRED_MICROSECONDS_DANCER_READ
+
     ROM_IntMasterDisable();
     ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     if (MillisecRestart == true)
@@ -176,6 +189,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 	    ROM_IntMasterEnable();
 		return;
 	}
+#endif
     if (msec_millisecondCounter%10 == 0)
     {
         Tenmsec_millisecondCounter = msec_millisecondCounter;
@@ -420,10 +434,13 @@ uint32_t MillisecLoop(uint32_t tick)
            }
        }
    }
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+   SaveLogData();
+#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
     return OK;
 }
 uint32_t MillisecLowLoop(uint32_t tick)
@@ -489,10 +506,10 @@ uint32_t MillisecLowLoop(uint32_t tick)
             if (isMotorConfigured(Motor_i))
                 MotorGetStatusFromFPGA(Motor_i);
         }
-       /* for (Heater_i = 0;Heater_i < NUM_OF_HEATERS;Heater_i++)
+        for (Heater_i = 0;Heater_i < NUM_OF_HEATERS;Heater_i++)
         {
             Read_Head_MixChip_Heaters_Current(Heater_i);
-        }*/
+        }
     }
     if (OneMinute_Tick)
     {
@@ -600,3 +617,147 @@ uint32_t getDrawerFansStatus(void)
 {
     return DrawerFansStatus;
 }
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+uint32_t DancerData[NUM_OF_DANCERS];
+uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
+{
+    return DancerData[DancerId];
+}
+uint32_t dancer1;
+uint32_t dancer2;
+uint32_t dancer3;
+uint32_t dancer1sum;
+uint32_t dancer2sum;
+uint32_t dancer3sum;
+uint32_t dancer_count;
+/*-----------------------*/
+uint32_t StoreBuffer[2][128];
+//char * StoreBuffer[2][512];
+/*-----------------------*/
+int  StoreBufferId = 0;
+int  StoreBufferCounter=0;
+uint32_t  BufferCounter=0xEEEEEEEE;
+bool storeData=false;
+uint8_t len=0;
+void HundredMicroTimerInterrupt(int ARG0)
+{
+    ROM_IntMasterDisable();
+    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
+    if (MillisecRestart == true)
+    {
+        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
+    }
+    else
+    {
+        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
+        ROM_IntMasterEnable();
+        return;
+    }
+    dancer1 = Read_Dancer_Position(WINDER_DANCER);
+    dancer2 = Read_Dancer_Position(POOLER_DANCER);
+    dancer3 = Read_Dancer_Position(FEEDER_DANCER);
+//data store - logging
+//double buffer switch
+    /*-----------------------*/
+    if (StoreBufferCounter>125)
+    //if (StoreBufferCounter>490)
+        /*-----------------------*/
+    {
+        StoreBufferId = 1-StoreBufferId;//switch buffer
+        StoreBufferCounter=0;
+        storeData = true;
+    }
+//double buffer initialize
+    /*-----------------------*/
+    if (StoreBufferCounter==0)
+    {
+        StoreBuffer[StoreBufferId][StoreBufferCounter] = BufferCounter++;
+        StoreBufferCounter++;
+    }
+    /*-----------------------*/
+//store data
+    /*-----------------------*/
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer1;
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer2;
+    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer3;
+    /*-----------------------*/
+    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "\r\n%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
+    //StoreBufferCounter+=(len+1);
+    /*-----------------------*/
+    dancer1sum+=dancer1;
+    dancer2sum+=dancer2;
+    dancer3sum+=dancer3;
+    dancer_count++;
+
+
+    if (dancer_count == 10)
+    {
+        DancerData[WINDER_DANCER] = dancer1sum/dancer_count;
+        DancerData[POOLER_DANCER] = dancer2sum/dancer_count;
+        DancerData[FEEDER_DANCER] = dancer3sum/dancer_count;
+        dancer_count = 0;
+        dancer1sum = 0;
+        dancer2sum = 0;
+        dancer3sum = 0;
+        OneMilliSecondMillisecInterrupt(ARG0);
+    }
+    ROM_IntMasterEnable();
+    return ;
+
+}
+char MillisecPath[50] = "0://SysInfo//Millisec.txt";
+FIL *FileHandle;
+void SaveLogData(void)
+{
+    uint32_t WrittenBytes = 0;
+    int BufferID = 1- StoreBufferId;
+
+    if (storeData == true)
+    {
+        if (FileHandle)
+        {
+            f_write(FileHandle,StoreBuffer[BufferID],512,&WrittenBytes );
+            storeData = false;
+        }
+    }
+}
+void MillisecLogInit(void)
+{
+    FRESULT Fresult = FR_OK;
+    ROM_IntMasterDisable();
+    BufferCounter = 0;
+    FileHandle = my_malloc(sizeof(FIL));
+    if (FileHandle == 0)
+        Fresult = FR_DENIED;
+    else
+        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS);
+    ROM_IntMasterEnable();
+    return ;
+
+}
+void MillisecLogClose(void)
+{
+    FRESULT Fresult = FR_OK;
+    if (FileHandle == 0)
+        Fresult = FR_DENIED;
+    else
+    {
+        storeData = false;
+        ROM_IntMasterDisable();
+        Fresult = f_close(FileHandle);
+        my_free(FileHandle);
+        FileHandle = 0;
+        ROM_IntMasterEnable();
+    }
+    return ;
+
+}
+#endif
+void MillisecInterrupt(UArg arg0)
+{
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+    HundredMicroTimerInterrupt(arg0);
+#else
+    OneMilliSecondMillisecInterrupt(arg0);
+#endif
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 517716c08..61c7df7ee 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -36,6 +36,14 @@ void MillisecInit(void);
 void MillisecStop(void);
 void MillisecStart(void);
 
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+void SaveLogData(void);
+void MillisecLogInit(void);
+void MillisecLogClose(void);
+
+#endif
+
+
 extern bool watchdogCriticalAlarm;
 
 extern Task_Handle Millisecond_Task_Handle;
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 580648842..97bb2e245 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -60,7 +60,9 @@ uint32_t HWConfigurationInit(void)
     uint32_t status = OK;
     UploadHardwareConfigurationRequest* UploadRequest;
 
+    GeneralHwReady = false;
     Control_WD(ENABLE,250);
+
     Fresult = f_mkdir(SW_INFO_DIR);
     if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
     {
@@ -85,6 +87,7 @@ uint32_t HWConfigurationInit(void)
            if (status ==OK)
            {
                upload_hardware_configuration_request__free_unpacked(UploadRequest,NULL);
+               EraseFlashSection(GENHWCFG_MAP_IN_FLASH,Bytes);
                ReadAppAndProgram(GENHWCFG_MAP_IN_FLASH, Bytes, buffer);
            }
            else
@@ -232,9 +235,11 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     if (request->n_winders == 1)
         status += InternalWinderConfigMessage(request->winders);
     status += MotorsInit();
-
+ 
     if (request->n_motors < NUM_OF_MOTORS)
     {
+       //EraseFlashSection(GENHWCFG_MAP_IN_FLASH + 0x2000,sizeof(HardwareMotor)*request->n_motors);
+
         for (Motor_i = 0; Motor_i < request->n_motors ; Motor_i++)
         status += MotorsConfigMessage(request->motors[Motor_i]);
     }
@@ -259,6 +264,8 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     //StoreDancerConfigMessage();
     if (request->n_pidcontrols <= HARDWARE_PID_CONTROL_TYPE__Dispenser8)
     {
+        IDS_DispenserControlInit();
+
         for (PID_i = 0; PID_i < request->n_pidcontrols ; PID_i++)
         {
             if (request->pidcontrols[PID_i]->hardwarepidcontroltype <= HARDWARE_PID_CONTROL_TYPE__MixerHeater)
@@ -338,6 +345,9 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
 
     UploadHardwareConfigurationRequest* UploadRequest = upload_hardware_configuration_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
+    GeneralHwReady = false;
+    Control_WD(ENABLE,250);
+
     status = HWConfiguration(UploadRequest);
 
     responseContainer = createContainer(MESSAGE_TYPE__UploadHardwareConfigurationResponse, requestContainer->token, true, &response, &upload_hardware_configuration_response__pack, &upload_hardware_configuration_response__get_packed_size);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 0a9210379..bc5448e7b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -23,6 +23,7 @@
 #include "../control/control.h"
 #include "../control/pidalgo.h"
 
+#include <ti/sysbios/knl/Task.h>
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 #include <inc/hw_memmap.h>
@@ -308,6 +309,30 @@ void HeatersStartControlTimer (void)
     Report("HeatersStartControlTimer ", __FILE__,__LINE__,0, RpMessage, 0, 0);
     return;
 }
+/***************************************************************************************************/
+int GetFilteredHeaterRead(int HeaterId)
+{
+#define MAX_FILTER 12
+    int i,maxread = -999999,minread = 999999,sum=0;
+    int FilteredRead[MAX_FILTER];
+    for (i = 0; i < MAX_FILTER;i++)
+    {
+        FilteredRead[i] = TemperatureSensorRead(HeaterId2PT100Id[HeaterId]);
+        Task_sleep(1);
+    }
+    for (i = 0; i < MAX_FILTER;i++)
+    {
+        if (FilteredRead[i]<minread)
+            minread = FilteredRead[i];
+        if (FilteredRead[i]>maxread)
+            maxread = FilteredRead[i];
+        sum+=FilteredRead[i];
+    }
+    sum -= minread;
+    sum -= maxread;
+    Report("GetFilteredHeaterRead ", __FILE__,HeaterId,sum, minread, maxread, 0);
+    return (sum/(MAX_FILTER-2));
+}
 /**************************************************************************************
  * HeaterCommandRequestMessage
  * called by: Communication from host
@@ -368,6 +393,11 @@ uint32_t  HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature)
         {
             PrepareHeater(HeaterId,Temperature);  //prepare the heaters control info
         }
+        else
+        {
+            HeaterPreviousRead[HeaterId] = GetFilteredHeaterRead(HeaterId);//
+            LOG_ERROR(HeaterId,"ReRead Heater Temp");
+        }
         HeaterPIDConfig[HeaterId].m_SetParam = HeaterCmd[HeaterId].targettemperatue;
         // if the heater is not on return (?).
         //set the target operation temperature
@@ -402,7 +432,7 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
         return ERROR;
     }
 
-    Report("PrepareHeater ", __FILE__,__LINE__,HeaterId, SetTemperatue, 0, 0);
+    //Report("PrepareHeater ", __FILE__,__LINE__,HeaterId, SetTemperatue, 0, 0);
 
     //start thread control for all motors
     HeaterPIDConfig[HeaterId].m_params.MAX = HeaterControl[HeaterId].outputproportionalpowerlimit*100;
@@ -441,10 +471,11 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
         //HeaterPIDConfig[HeaterId].m_params.dt *=10;
         //DCInitialHeating[HeaterId] = true;
         HeaterReady[HeaterId] = false;
+        HeaterRecalculateHeaterParams(HeaterId, 0);
 
         Enable_Reading_Heaters_Current(HeaterId2CurrentId[HeaterId]);
 
-        HeaterPreviousRead[HeaterId] = TemperatureSensorRead(HeaterId2PT100Id[HeaterId]);
+        HeaterPreviousRead[HeaterId] = GetFilteredHeaterRead(HeaterId);//TemperatureSensorRead(HeaterId2PT100Id[HeaterId]);
         if((abs(HeaterPreviousRead[HeaterId]-MINIMUM_HEATER_READ)<1)||(abs(MAXIMUM_HEATER_READ-HeaterPreviousRead[HeaterId])<10))
         {
             LOG_ERROR (HeaterId,"PT100 not working properly");
@@ -460,10 +491,11 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
         if (ControlIdtoHeaterId [HeaterId] == 0xFF)
 		{
 	        ControlIdtoHeaterId [HeaterId] = AddControlCallback( HeaterControlCBFunction, Frequency/*eOneSecond*/,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),DryerInternalPT100Id,0);
-            HeaterPreviousRead[HeaterId] = TemperatureSensorRead(DryerInternalPT100Id);
+            //HeaterPreviousRead[HeaterId] = TemperatureSensorRead(DryerInternalPT100Id);
+            HeaterPreviousRead[HeaterId] = GetFilteredHeaterRead(HeaterId);//
             Report("PrepareHeater Read", __FILE__,__LINE__,HeaterId, SetTemperatue, HeaterPreviousRead[HeaterId], 0);
-            HeaterPreviousRead[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain] = TemperatureSensorRead(HeaterId2PT100Id[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain]);
-            HeaterPreviousRead[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary] = TemperatureSensorRead(HeaterId2PT100Id[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary]);
+            HeaterPreviousRead[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain] =  GetFilteredHeaterRead(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain);//TemperatureSensorRead(HeaterId2PT100Id[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain]);
+            HeaterPreviousRead[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary]  = GetFilteredHeaterRead(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary);// = TemperatureSensorRead(HeaterId2PT100Id[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary]);
 		}
         if (ControlIdtoMaxHeaterId [HeaterId] == 0xFF)
             ControlIdtoMaxHeaterId [HeaterId] = AddControlCallback( DcHeaterMaxTempCBFunction, eOneSecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),HeaterId2PT100Id[HeaterId],0);
@@ -471,6 +503,8 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
         Enable_Reading_Heaters_Current(HeaterId2CurrentId[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain]);
         Enable_Reading_Heaters_Current(HeaterId2CurrentId[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary]);
 
+        HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, 0);
+        HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary, 0);
         if (MainDryerHeaterMaxTempControl == 0xFF)
             MainDryerHeaterMaxTempControl = AddControlCallback( HeaterMaxTempCBFunction, eHundredMillisecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain),HeaterId2PT100Id[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain],0);
         if (SecondDryerHeaterMaxTempControl == 0xFF)
@@ -711,6 +745,10 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
     if (abs(readValue - HeaterPreviousRead[index])>2000)
     {
         Report("Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,readValue, index);
+        if (readValue > HeaterCmd[index].targettemperatue)
+        {
+            DeActivateHeater(index);
+        }
         return ERROR;
     }
     HeaterPreviousRead[index] = readValue;
@@ -834,6 +872,11 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
     if (abs(readValue - HeaterPreviousRead[index])>2000)
     {
         Report("Temperature Spike",__FILE__,__LINE__,HeaterPreviousRead[index],RpWarning,readValue, index);
+        if (readValue > HeaterCmd[index].targettemperatue)
+        {
+            DeActivateHeater(index);
+            HeaterRecalculateHeaterParams(index, 0);
+        }
         return ERROR;
     }
     HeaterPreviousRead[index] = readValue;
@@ -844,6 +887,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
                 {
                     DeActivateHeater(index);
+                    HeaterRecalculateHeaterParams(index, 0);
                     //Heaters OFF until coming into the proportional band
                     //Report("DC HEATER Over temperature ", __FILE__,__LINE__,index, RpMessage, readValue, 0);
                     return OK;
@@ -851,6 +895,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         if (readValue < ((HeaterCmd[index].targettemperatue * (100-HeaterControl[index].outputproportionalband))/100))
                 {
                     ActivateHeater(index);
+                    //HeaterRecalculateHeaterParams(index, 100);
                     //Heaters OFF until coming into the proportional band
                     //Report("DC HEATER Under temperature ", __FILE__,__LINE__,index, RpMessage, readValue, 0);
                     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index 8f58bac3e..bf73ed7fa 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -40,6 +40,7 @@ uint32_t IDSDistanceToSpoolState(void);
 uint32_t IDSEndState(void *JobDetails);
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request);
+uint32_t IDS_DispenserControlInit();
 
 uint32_t IDS_HomeDispenser (uint32_t deviceID, uint32_t speed , callback_fptr callback);
 uint32_t IDS_StopHomeDispenser (uint32_t deviceID);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index d77d6f082..2be233962 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -30,7 +30,7 @@ typedef struct
     bool                m_isReady;
     PID_Config_Params   m_params;
 }DispenserControlConfig_t;
-HardwarePidControl DispensersControl[MAX_SYSTEM_DISPENSERS] = {0};
+HardwarePidControl *DispensersControl;// = (HardwarePidControl *)GENHWCFG_MAP_IN_FLASH + 0x4000;
 
 int32_t DispenserSamples[MAX_SYSTEM_DISPENSERS][MAX_CONTROL_SAMPLES] = {0};
 int DispenserSamplePointer[MAX_SYSTEM_DISPENSERS] = {0};
@@ -55,6 +55,13 @@ bool         DispenserUsedInJob[MAX_SYSTEM_DISPENSERS] = {false,false,false,fals
 bool         DispensersAlarmState[ MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false,false,false};
 int JobBrushStopId = 0;
 
+uint32_t IDS_DispenserControlInit()
+{
+    DispensersControl = (void *)(GENHWCFG_MAP_IN_FLASH + 0x4000);
+    EraseFlashSection(DispensersControl,sizeof(HardwarePidControl)*MAX_SYSTEM_DISPENSERS);
+    return OK;
+}
+
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 {
     int Dispenser_i,i;
@@ -67,9 +74,10 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
             break;
         }
     }
-    memcpy (&DispensersControl[Dispenser_i],request,sizeof(HardwarePidControl));
-    if (DispensersControl[Dispenser_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
-        DispensersControl[Dispenser_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+    if (request->pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
+        request->pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
+    ReadAppAndProgram(&DispensersControl[Dispenser_i], sizeof(HardwarePidControl), request);
+
     for (i = 0;i < DispensersControl[Dispenser_i].pvinputfilterfactormode; i++)
         DispenserSamples[Dispenser_i][i] = 0;  //reset the samples value for control beginning
     /*DispenserNormalizedErrorCoEfficient[Dispenser_i] = (2*PI*DancersCfg[ThreadDispenserIdToDancerId[Dispenser_i]].armlength);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index ee49d56f3..394e2ae13 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -694,6 +694,9 @@ uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
         PrepareState = false;
         // set the new speed in the dryer motor to the speed of the new segment
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+     MillisecLogInit();
+#endif
 
         if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0)
         {
@@ -775,7 +778,9 @@ char Endstr[150];
 
      ThreadUpdateProcessLength (0.0,(void *)NULL);
     SetOriginMotorSpeed(0);
-
+#ifdef HUNDRED_MICROSECONDS_DANCER_READ
+     MillisecLogClose();
+#endif
      if (SpeedControlId != 0xFF)
      {
          if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK)