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

19 files changed, 539 insertions, 443 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 1c2b6662d..650d9be24 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -893,7 +893,7 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
 
         if (AlarmItem[Alarm_i].AlarmSource == ALARM_SOURCE_TYPE__CoversAlarm)
         {
-            if (AlarmState[Alarm_i].Status == true)
+            if ((AlarmState[Alarm_i].Status == true)&&(AlarmItem[Alarm_i].Severity == DEBUG_LOG_CATEGORY__Error))
             {
                 FoundReason = JOB_TAMPER_ALARM;
                 ReportWithPackageFilter(AlarmFilter,"tamper alarm preventing job", __FILE__,__LINE__,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
@@ -977,6 +977,7 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
                 case EVENT_TYPE__INSUFFICIENT_AIR_FLOW:
                 case EVENT_TYPE__VOC_SENSOR_ALARM_TIME:
                 case EVENT_TYPE__VOC_SENSOR_ALARM_SLOPE:
+                //case EVENT_TYPE__POWER_UP_BIT_FAILURE:
                     FoundReason = JOB_UNSPECIFIED_ERROR;
                     AlarmId = Alarm_i;
                     ReportWithPackageFilter(AlarmFilter,"an alarm preventing job", __FILE__,__LINE__,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
@@ -1001,7 +1002,7 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
         //AbortJob(AlarmReasonStr);
         //PrepareReady(Module_Alarms,ModuleFail);
         ReportWithPackageFilter(AlarmFilter,AlarmReasonStr, __FILE__,__LINE__,AlarmId, RpMessage, DEBUG_LOG_CATEGORY__Error, 0);
-		return (JobEndReason);
+		return (FoundReason);
     }
     return JOB_OK;
 }
@@ -1314,7 +1315,7 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
                  }
                  break;
             case ALARM_SOURCE_TYPE__MotorAlarm:
-                if (0)//(CheckMotorAlarms)
+                if (CheckMotorAlarms)
                 {
                     if (FPGA_WD_Occurred == true)
                     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4ad5336d9..27fc8a707 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -594,7 +594,7 @@ uint32_t MillisecLowLoop(uint32_t tick)
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m70msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,OneHourTick,Gradient_Tick;
+    bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m70msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,TenMinutes_Tick,OneHourTick,Gradient_Tick;
     bool O700Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick;
     //bool O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
@@ -614,6 +614,7 @@ uint32_t MillisecLowLoop(uint32_t tick)
     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]++;
     //gather Motor data from FPGA
@@ -766,8 +767,12 @@ uint32_t MillisecLowLoop(uint32_t tick)
             }
             */
         }
+
         //call waste state machine
         Waste_StateMachine_OneSecond_Call();
+
+        //call IFS state machine
+        midTankStateMachine();
     }
     if (Tensecond_Tick)
     {
@@ -797,6 +802,13 @@ uint32_t MillisecLowLoop(uint32_t tick)
         ResetControlTime();
 #endif
     }
+    if (TenMinutes_Tick)
+    {
+        if (WHS_Type == WHS_TYPE_NEW)
+        {
+            waste_seq_step1();// include 1Sec delay           <- to open !!!!
+        }
+    }
     if (OneHourTick)
     {
 #define PUMP_LIMIT 4
@@ -807,10 +819,10 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpCounter = 0;
         }
         MidTankReading();
-        if (WHS_Type == WHS_TYPE_NEW)
+        /*if (WHS_Type == WHS_TYPE_NEW)
         {
             waste_seq_step1();// include 1Sec delay           <- to open !!!!
-        }
+        }*/
         //Trigger_WHS_MAX11614_Read_Gas_Sensor();
     }
     //ROM_IntMasterEnable();
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 78e24f495..4b6a80967 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -21,6 +21,7 @@ typedef enum {
     eHundredMillisecond = 100,
     eOneSecond = 1000,
     eOneMinute = 60000,
+    eTenMinutes = 600000,
     eOneHour = 3600000
 }CTRL_TIMING_ENUM;
 typedef enum
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 8e01fe18e..421206b45 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -864,10 +864,10 @@ void DiagnosticOneSecCollection(void)
             tempFlow = HeadFlowMeter;
         }*/
         WasteLevel = GetWHSWasteTankLevelMiliLiter()/1000;//change from ml to litter
-        static double InitCounter = 0;
-        if (GetMachineState()<MACHINE_STATE_NO_PROCESS_PARAMS)
+        static double InitCounter = 60.0;
+        if ((GetMachineState()<MACHINE_STATE_NO_PROCESS_PARAMS) &&(GetMachineState()>=MACHINE_STATE_WAIT_FOR_COOLER))
         {
-            InitCounter+=1.0;
+            InitCounter-=1.0;
             DiagnosticsMonitor.chillertemperature = &InitCounter;
 
         }
@@ -1058,7 +1058,8 @@ void SendDiagnostics(void)
     DiagnosticsMonitor.dancer2angle = dancer2angle;
     DiagnosticsMonitor.dancer3angle = dancer3angle;
 */
-    if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode))
+//    if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode))
+    if (DiagnosticMode >= Diagnostic_Extended_Mode)
     {
         DiagnosticsMonitor.n_dancer1angle =  DancerCounterIndex[0];
         DiagnosticsMonitor.n_dancer2angle =  DancerCounterIndex[1];
diff --git a/Software/Embedded_SW/Embedded/Modules/General/Safety.c b/Software/Embedded_SW/Embedded/Modules/General/Safety.c
index 89ac8d020..099ac1769 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/Safety.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/Safety.c
@@ -46,7 +46,7 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
    bool mDrierDoorAlarmState = false;
    bool mAirFlowAlarmState = false;
    bool mAirFilterAlarmState = false;
-   //bool mWasteOverflowAlarmState = false;
+   bool mWasteOverflowAlarmState = false;
 #ifdef CONTROL_DEBUG
     uint32_t tempp,tempq,delta;
     uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0;
@@ -91,6 +91,16 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
                    mDrierDoorAlarmState = true;
                    DrierDoorAlarmState = true;
                }
+               else
+               {
+                   //if (WHS_GPI_WASTE_OVERFULL()) - cannot read this switch
+                   {
+                       //report and handle waste overflow
+                       AlarmHandlingSetAlarm(EVENT_TYPE__CHILLER_DRY_CONTACT, true);
+                       mWasteOverflowAlarmState = true;
+                       WasteOverflowAlarmState = true;
+                   }
+               }
            }
        }
 
@@ -224,6 +234,12 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
        AlarmHandlingSetAlarm(EVENT_TYPE__AIR_FILTER_NOT_INSTALLED, false);
        AirFilterAlarmState = mAirFilterAlarmState;
    }
+   if  ((mWasteOverflowAlarmState != WasteOverflowAlarmState)|| (mWasteOverflowAlarmState == false))
+   {
+      //alarm went off
+       AlarmHandlingSetAlarm(EVENT_TYPE__WASTE_CONTAINER_OVERFLOW, false);
+       WasteOverflowAlarmState = mWasteOverflowAlarmState;
+   }
 #ifdef CONTROL_DEBUG
                 tempq = HibernateRTCSSGet();
                 if (tempq < tempp)
diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.h b/Software/Embedded_SW/Embedded/Modules/General/buttons.h
index 646861bc7..6a15d3765 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/buttons.h
+++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.h
@@ -59,7 +59,6 @@ uint32_t Buttons_Init(void);
 uint32_t Button_load_Init(void);
 uint32_t Button_JOG_Init(void);
 bool SetPowerMachineState(PBmachineState state);
-void Ink_Cart_Led();
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index bf7d761dd..30a99ac86 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -119,7 +119,7 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams,bool saveData)
     }
     if (saveData == true)
     {
-        if ((ProcessParams->dryerzone1temp > 0.1)&&(ProcessParams->headzone2temp > 0.1)&&(ProcessParams->headzone3temp > 0.1)&&(ProcessParams->headzone4temp > 0.1))//NOT turning off heaters
+        if ((ProcessParams->dryerzone1temp > 0.1)&&(ProcessParams->headzone2temp > 0.1)&&(ProcessParams->headzone3temp > 0.1)&&(ProcessParams->headzone1temp > 0.1))//NOT turning off heaters
         {
             Bytes = sizeof(ProcessParameters);
             FileWrite(ProcessParams,Bytes,ProcessParamsConfigPath,BIOS_WAIT_FOREVER);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
index 8f10a0e5d..0be1e7ebe 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
@@ -55,17 +55,20 @@ uint32_t IDS_Cleaning_Move_Actuators()
        return OK;
    if (CleaningStageCounter == 1)
    {
-       Trigger_Head_Actuators_Control(ACTIN, LOW,true);
+       Trigger_Head_Actuators_Stub(ACTIN, true, true);
+       //Trigger_Head_Actuators_Control(ACTIN, LOW,true);
        Report("IDS_Cleaning_Move_ActuatorUp", __FILE__, __LINE__, 1, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tup)
    {
-       Trigger_Head_Actuators_Disable();
+       Trigger_Head_Actuators_Stub(ACTIN, false, false);
+       //Trigger_Head_Actuators_Disable();
        Report("IDS_Cleaning_Stop_Actuator", __FILE__, __LINE__, Tup, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tdelay1)
    {
-       Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+       Trigger_Head_Actuators_Stub(ACTIN, true, false);
+       //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
        Report("IDS_Cleaning_MoveDown", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tdelay2)
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c
index 3798e9aee..7693d1520 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_dispenser.c
@@ -347,19 +347,20 @@ void IDS_Dispenser_Content_Calculation (char DispenserId)
             != DispenserIdToMotorId[DispenserId])
         return;
 
+    /********* THIS FUNCTION IS NOW CALLED EVERY 400 MILISECONDS!!!!!!!!!! ***************************/
     //int dir = (IDS_Dispenser_Data[DispenserId].direction==1)?1:-1;
     //double consumedintimeframe = (double)(CurrentDispenserSpeed[DispenserId])*(double)(IDS_Dispenser_Data[DispenserId].microsteps)*
     //        IDS_Dispenser_Data[DispenserId].nanolitterperpulse ;//* dir;
     //double consumedintimeframe = (double)(CurrentDispenserSpeed[DispenserId]); //pulses only
     //consumedintimeframe = 10+DispenserId;
-    IDS_Dispenser_Data[DispenserId].consumedinnanolitter += (CurrentDispenserSpeed[DispenserId]/10);//100 milliseconds ==> speed/10
+    IDS_Dispenser_Data[DispenserId].consumedinnanolitter += (CurrentDispenserSpeed[DispenserId]*4/10);//100 milliseconds ==> speed/10
     if (IDS_Dispenser_Data[DispenserId].consumedinnanolitter<0)
         IDS_Dispenser_Data[DispenserId].consumedinnanolitter = 0;
     if (CurrentDispenserSpeed[DispenserId]>0.1)
     {
         DispenserDataUpdated = true;
         IDS_Dispenser_Data[DispenserId].totalconsumedinnanolitter += (CurrentDispenserSpeed[DispenserId]/10);
-        if (seconds_counter%6000 == 0)
+        if (seconds_counter%1500 == 0) //1500*400MILI = 600000
         {
             Report("IDS_Dispenser_Data ",__FILE__,DispenserId,(int)IDS_Dispenser_Data[DispenserId].consumedinnanolitter,RpWarning,(int)CurrentDispenserSpeed[DispenserId],0);
         }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index b4442c996..ac9bd0deb 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -43,6 +43,8 @@ typedef struct
 HardwarePidControl *DispensersControl;
 HardwarePidControl DispensersCtrl[MAX_SYSTEM_DISPENSERS];
 #define MAX_DYE_DISPENSERS 6
+#define IDS_PRESEGMENT_TIME_STEP 50
+
 int32_t DispenserSamples[MAX_SYSTEM_DISPENSERS][MAX_CONTROL_SAMPLES] = {0};
 int DispenserSamplePointer[MAX_SYSTEM_DISPENSERS] = {0};
 double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0};
@@ -117,8 +119,9 @@ void IDS_Dispenser_SetPreSegmentWFCFValues(double dispenserpresegmentwfcf, doubl
          LeftRockerSpeed = ids_leftcleaningmotorspeed;
      if ( ids_rightcleaningmotorspeed)
          RightRockerSpeed = ids_rightcleaningmotorspeed;
-     Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,CleaningDispenserSpeed,RpWarning,(int)LeftRockerSpeed,0);
+     Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,RightRockerSpeed,RpWarning,(int)LeftRockerSpeed,0);
      Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,InterSegmentStartSprayCleaner,RpWarning,(int)InterSegmentCenterRockers,0);
+     Report("IDS_Dispenser actuator times ",__FILE__,(int)LeftRockerSpeed/100*IDS_PRESEGMENT_TIME_STEP,(int)LeftRockerSpeed%100*IDS_PRESEGMENT_TIME_STEP,RpWarning,(int)RightRockerSpeed*IDS_PRESEGMENT_TIME_STEP,0);
 
  }
 
@@ -590,7 +593,7 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
              Report("pressureReady = true  or timeout;",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
          }
      }
-     else if (endOfPrepareWCF == false)
+     else if ((endOfPrepareWCF == false)&&(NumOfActiveDispensers))
      {
          if ((DispenserBuildTimeCounter == PRESSURE_READ_TIME_GAP)&&(FirstBrushStop))
          {
@@ -683,6 +686,13 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
 
          }
      }
+     else if (NumOfActiveDispensers == 0)
+     {
+         endOfPrepareWCF = true;
+         FreeBrushStopFileData(FirstBrushStop);
+         FirstBrushStop =NULL;
+         Report("endOfPrepareWCF = true;",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
+     }
      //start new stage
      if (endOfPrepareWCF == true)
      {
@@ -1027,7 +1037,6 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
 
 
 //********************************************************************************************************************
-
  uint32_t IDSPreSegmentStateCallbackRunner(uint32_t IfIndex, uint32_t ReadValue)
  {
      JobDispenser **Dispensers;
@@ -1065,7 +1074,7 @@ uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr
 uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
 */
      //InterSegmentStepsLimit = lInterSegmentLength*10;//100 millisec steps
-     InterSegmentStepsCount+=100;
+     InterSegmentStepsCount+=IDS_PRESEGMENT_TIME_STEP;
      if (InterSegmentStepsCount >= lInterSegmentLength)
      {
          //IDS_Valve_PresegmentReady(1,0);
@@ -1259,7 +1268,7 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
             lInterSegmentLength = ((IntersegmentLength*100)*1000/dyeingspeed);
             lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
             InterSegmentStepsCount = 0;
-            DispenserPreSegmentControlId = AddControlCallback(NULL,IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+            DispenserPreSegmentControlId = AddControlCallback(NULL,IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
             if (DispenserPreSegmentControlId == 0xFF)
             {
                 Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
@@ -1292,7 +1301,7 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
         lInterSegmentLength = (dryerbufferCentimeters*1000/dyeingspeed);
         lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
         InterSegmentStepsCount = 0;
-        DispenserPreSegmentControlId = AddControlCallback( IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+        DispenserPreSegmentControlId = AddControlCallback( IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
         if (DispenserPreSegmentControlId == 0xFF)
         {
             Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
@@ -1695,7 +1704,7 @@ uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId)
          lInterSegmentLength = (dryerbufferCentimeters*1000/dyeingspeed);
          lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
          InterSegmentStepsCount = 0;
-         DispenserPreSegmentControlId = AddControlCallback("IDS DTS", IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+         DispenserPreSegmentControlId = AddControlCallback("IDS DTS", IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
          if (DispenserPreSegmentControlId == 0xFF)
          {
              Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
index 5af90c3d1..974b022d7 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
@@ -8,14 +8,15 @@
  *  @author Shai
  */
 
+#include <DataDef.h>
 #include "PMR/Diagnostics/CartridgeSlot.pb-c.h"
 #include "PMR/Diagnostics/CartridgeTagContent.pb-c.h"
 #include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
-
-#include <DataDef.h>
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"//#include "FPGA_GPIO.h"  //  use for FPGA IO
+#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
+#include "StateMachines/Initialization/PowerOffSequence.h"
 #include "include.h"
 
-#include "Modules/Waste/Waste.h"
 #include "Modules/IDS/ids_ex.h"
 #include "Modules/IFS/ifs.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
@@ -23,256 +24,147 @@
 #include "CartridgeValidationResponse.pb-c.h"
 #include  <PMR/Diagnostics/EventType.pb-c.h>
 #include "drivers/Valves/Valve.h"
-#include "Modules/Waste/Waste.h"
 #include "../control/control.h"
+#include "Modules/General/buttons.h"
+
+#define CARTRIDGE_EMPTY_QUANTITY_LIMIT 10
+#define CARTRIDGE_EMPTY_CALCULATION_TIME 12
+#define FIVE_SECONDS 5
+
+//#define IFS_DEBUG
+
+int CartridgeInkTimeout = CARTRIDGE_INK_TIMEOUT;
+int CartridgeCheckTime = FIVE_SECONDS;
 
-bool IFS_TimeOutAlarm(bool status);
-bool ColorMatch();
-bool MidTankValvesAction(bool action);
+double midTankCapacity;
+MidTank_t midTankCartColor;
+bool prevDoorState;
+bool doorState;
+uint8_t inkAuthenticationPass;
+uint32_t midTankTimeoutCounter = 0;
+double MidTankEmptyLimit = MidTank_Pressure_EMPTY;
+MidTankStateEnum midTankState = 0;
+
+char CartridgeRequestToken[36+1];
+int  CartridgeRequestId = 0x01010101;
+
+bool RdInkCartridgeSensor()
+{
+    return Is_Cartridge_Present(CART_1);
+}
 
 bool IFS_CartridgeLowerPresent()
 {
-    return IFS_info.IFS_Ink_precense_sensor;
+    return RdInkCartridgeSensor();
 }
 
 bool IFS_MidTankFilling(void)
 {
-    return (IFS_info.Ink.time_out > 0);
+    return (midTankTimeoutCounter > 0);
 }
+
 MidTank_t IFS_MidTankIsActive(void)
 {
-    return (IFS_info.Ink.cart_color);
+    return midTankCartColor;
 }
 
-NFC_Tag NFCTag[MAX_CARTRIDGES];
+uint32_t MidTankReading(void)
+{
+    if (midTankTimeoutCounter > 0)
+        return ERROR;
+    return OK;
+}
 
-void IFS_Init(void)
+bool MidTankValvesAction(bool action)
 {
-    int i;
-    for (i=0;i<MAX_CARTRIDGES;i++)
-    {
-        //TagInitialize(&TagContent[i]);
-        memset(NFCTag[i].Buf,0,sizeof(NFCTag[i]));
+    bool ret =  true;
+    if (action == Cartridge_MidTank_ON) {
+        Enable_MidTank_Pressure_Reading(midTankCartColor);
+    } else {
+        Disable_MidTank_Pressure_Reading(midTankCartColor);
+    }
+    if (IDS_IsHomingActive(midTankCartColor)==false) {
+        Valve_Set(IDS_Id_to_AirValve[midTankCartColor], action);
     }
+    Valve_Set(IDS_Id_to_CartrideValve[midTankCartColor], action);
+    return ret;
 }
 
-/**
- *
- * @brief detect cartridge INK Empty
- *  this function detect changes in IFS INK cartridge
- *  when MidTank average is bigger the 98% of midtank pressure
- *  the INK cartridge is empty
- * @return bool ret OK = 0 , CartridgeFinished = 1
- */
-#define CARTRIDGE_EMPTY_QUANTITY_LIMIT 10
-#define CARTRIDGE_EMPTY_CALCULATION_TIME 12
 bool DetectIfCartridgeFinished(int int_MidTank_Pressure_1000)
 {
-
-#define CartridgeFinished 1
-
-    bool ret = OK;
+    bool ret = false;
     static int measure_midtank[CARTRIDGE_EMPTY_CALCULATION_TIME] ;
     static int count_measure_midtank = 0;
     int i=0;
     int sum = 0;
     int average = 0;
 
-    if ( count_measure_midtank == CARTRIDGE_EMPTY_CALCULATION_TIME)
+    if (count_measure_midtank == CARTRIDGE_EMPTY_CALCULATION_TIME)
         count_measure_midtank = 0;
     measure_midtank[count_measure_midtank] = int_MidTank_Pressure_1000;
     count_measure_midtank += 1;
     for (i=0;i<CARTRIDGE_EMPTY_CALCULATION_TIME;i++)
         sum += measure_midtank[i];
     average = sum/CARTRIDGE_EMPTY_CALCULATION_TIME;
-    if (abs(average - int_MidTank_Pressure_1000)<CARTRIDGE_EMPTY_QUANTITY_LIMIT)  ret = CartridgeFinished;
-    //if (average > (int_MidTank_Pressure_1000 * 0.98) )  ret = CartridgeFinished;
-
+    if (abs(average - int_MidTank_Pressure_1000)<CARTRIDGE_EMPTY_QUANTITY_LIMIT)
+        ret = true;
     return ret;
 }
 
-
-
-/**
- *
- * @brief detect changes in cartridge INK presence
- *  this function detect changes in IFS presence cartridge
- *  when changes are happens it update the presence status
- *  and the correct event
- * @return IFS_INK_presence_sensor or WHS_no_event event
- */
-
-WHS_sensor CartridgeInkCallBackFunction()
+#ifdef IFS_DEBUG
+bool CartridgeInkFull()
 {
-     WHS_sensor ret = WHS_no_event;
-
-    if (IFS_info.IFS_Ink_precense_sensor != RdInkCartridgeSensor())
-    {
-        WHS_info.event = IFS_INK_presence_sensor;
-        IFS_info.IFS_Ink_precense_sensor = !IFS_info.IFS_Ink_precense_sensor;
-        Report("------------ find  : IFS_Ink_presence_sensor event  -----------------", __FILE__,__LINE__, IFS_info.IFS_Ink_precense_sensor, RpMessage, 0, 0);
-        ret = IFS_INK_presence_sensor;
+    static int count = 1;
+    if((count++ % 60) == 0) {
+        return true;
+    } else {
+        return false;
     }
-    return ret;
 }
+#else
+int CartridgeFinishedCount = 0;
 
-/**
- *
- * @brief detect TimeOut
- * we have 2 way to stop filling:
- * 1. time out
- * 2. detect Midtank is full.
- *
- * 1. time out process is:
- *      1. detect timeout after 5 minutes
- *      2. close and open the valves
- *      3. count 5 minutes again
- *2. measure MidTank capacity
- *  if MidTank is full stop count
- *
- * @return IFS_INK_presence_sensor or WHS_no_event event
- */
-WHS_sensor CartridgeInkTimeOutCallBackFunction()
+bool CartridgeInkFull()
 {
-    WHS_sensor ret = WHS_no_event;
     double MidTank_Pressure = 0;
     int int_MidTank_Pressure_1000 = 0;
 
-
-    //Enable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
-    MidTank_Pressure = Read_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);// Get_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);
-    //Disable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+    MidTank_Pressure = Read_MidTank_Pressure_Sensor(midTankCartColor);
     int_MidTank_Pressure_1000 =  ((int)(MidTank_Pressure*1000));
-    Report("------------ IFS_info.Ink.time_out and Pressure  -----------------", __FILE__,__LINE__,(IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-
-    if (IFS_info.Ink.time_out) IFS_info.Ink.time_out += 1; /*** counter  ***/
-
 
     /*** detect the MidTank capacity is full  ***/
-    //Read_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);
-    if (MidTank_Pressure >= IFS_info.MidTank_capacity  + CARTRIDGE_CAPATICY)
-    {
-        IFS_info.Ink.time_out = 0;
-        WHS_info.event = IFS_MidTankFull;
-        return IFS_MidTankFull;
-    }
-
-    /**** detect if the MidTank_Pressure is the same, 5 times   ****/
-    if (DetectIfCartridgeFinished(int_MidTank_Pressure_1000))
-    {
-        Report("------------ INK Cartrigde is empty (by average)  -----------------", __FILE__,__LINE__,(IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        IFS_info.Ink.time_out = 0;
-        WHS_info.event = IFS_MidTankFull;
-        return IFS_MidTankFull;
-    }
-
-
-    int timeout = CARTRIDGE_INK_TIMEOUT;
-    int secondtimeout = CARTRIDGE_INK_TIMEOUTx2;
-    if (IFS_info.Ink.cart_color == MIDTANK_8)//lubricant
-    {
-        timeout = CARTRIDGE_INK_TIMEOUT*3;
-        secondtimeout = CARTRIDGE_INK_TIMEOUTx2 * 3;
-    }
-    /*** first TimeOut  ***/
-    if (IFS_info.Ink.time_out == timeout)
-    {
-        Report("------------ first IFS_info.Ink.time_out, close valves  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        //IFS_info.Ink.time_out = 0;
-        //return IFS_info.event = IFS_Timeout;
+    //Read_MidTank_Pressure_Sensor(IFS_info.Ink.midTankCartColor);
+    if (MidTank_Pressure >= midTankCapacity  + CARTRIDGE_CAPATICY)
+        return true;
 
-        // try again :
-        // start timeout
-        // close and open the correct valves
-
-        IFS_info.Ink.time_out = 0;
-        MidTankValvesAction(Cartridge_MidTank_OFF);
-        IFS_info.Ink.time_out = timeout;
+    if (CartridgeFinishedCount++>=CartridgeCheckTime ) {
+        CartridgeFinishedCount = 0;
+        Report("IFS_filling ", __FILE__,__LINE__,int_MidTank_Pressure_1000, RpMessage, int_MidTank_Pressure_1000, 0);
+        /**** detect if the MidTank_Pressure is the same, 5 times   ****/
+        if (DetectIfCartridgeFinished(int_MidTank_Pressure_1000))
+            return true;
     }
-
-    if (IFS_info.Ink.time_out == (timeout + 1)) //need to open the valve for second chance
-        {
-        Report("------------ IFS_Timeout open valves for second timeout  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        MidTankValvesAction(Cartridge_MidTank_ON);
-        IFS_info.Ink.time_out = timeout;
-        }
-
-    if (IFS_info.Ink.time_out >= (secondtimeout + 1))
-        {
-            Report("------------ IFS_Timeout_Second_Time  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-            IFS_info.Ink.time_out = 0;
-            MidTankValvesAction(Cartridge_MidTank_OFF);
-            WHS_info.event = IFS_Timeout_Second;
-            return IFS_Timeout_Second;
-        }
-    return ret;
+    return false;
 }
+#endif
 
-/**
- *
- * @brief IFS Time Out Alarm
- *
- * @return OK
- */
-bool IFS_TimeOutAlarm(bool status)
+bool midTankCart_door()
 {
-    bool ret =  OK;
-
-        AlarmHandlingSetAlarm( EVENT_TYPE__MID_TANK_1_FILL_TIMEOUT+IFS_info.Ink.cart_color,  status);
-
-
-        return ret;
-}
+    bool door;
 
-/**
- *
- * @brief MidTank Valves Action
- * the MidTank have 2 Valves:
- * 1. Ink Color Valve to get INK from the INK Cartridge to Midtank
- * 2. Air valve to get INK in the Midtank we have to open the Air valve so the air can go out
- *
- * @return action to open the Valve use: Cartridge_MidTank_ON/Atm_MidTank_ON
- *                to close the Valve use : Cartridge_MidTank_OFF/Atm_MidTank_OFF
- */
-bool MidTankValvesAction(bool action) //Cartridge_MidTank_ON of Cartridge_MidTank_OFF
-{
-    bool ret =  false;
-    if (action == Cartridge_MidTank_ON)
+    if(Get_COVER_1_State(CartridgesDoor))//to check the polarity
     {
-        Enable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+        door = OPEN;
     }
     else
     {
-        Disable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+        door = CLOSE;
     }
-    if (IDS_IsHomingActive(IFS_info.Ink.cart_color)==false)
-        Valve_Set(IDS_Id_to_AirValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
-    Valve_Set(IDS_Id_to_CartrideValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
-
-    ret = true;
-    return ret;
-}
-
-bool ColorMatch()
-{
-    bool ret =  false;
-
-    ret = true;
-    return ret;
+    return door;
 }
 
-char CartridgeRequestToken[36+1];
-int  CartridgeRequestId = 0x01010101;
-
 
-/**
- *
- * @brief create message to PPC
- *  temporary message RFID will replace that message.
- *  send a request to PPC to know the MidTank number
- * @return OK
- */
-uint32_t ReadCartridgeData(cartridge_name cart_name)
+uint32_t ReadInkCartridgeData()
 {
     //this function triggers reading and validation of the cartridge information
     // for now it sends a request to the tablet to inquire for the ink color/midtank id
@@ -306,16 +198,9 @@ uint32_t ReadCartridgeData(cartridge_name cart_name)
     return OK;
 }
 
-/**
- *
- * @brief PPC answers the MidTank number and call WasteTankCBFunction() function.
- *  temporary message RFID will replace that message.
- *
- * @return OK
- */
 bool CartridgeValidationResponseFunc(MessageContainer* requestContainer)
 {
-    bool ret = notOK;
+    bool ret = ERROR;
 
     CartridgeValidationResponse* response = cartridge_validation_response__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
@@ -324,118 +209,150 @@ bool CartridgeValidationResponseFunc(MessageContainer* requestContainer)
     // 2. the index is out of rang
     if (response == NULL)
     {
-        ReportWithPackageFilter(WHS_filling,"------------ IFS_filling message from PPC is null!!! -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+        Report("IFS_filling message from PPC is null!!!", __FILE__,__LINE__,0, RpMessage, 0, 0);
         return ret;
     }
 
     if (response->index < NUM_OF_MIDTANKS)
     {
-        IFS_info.Ink.cart_color = response->index; // midtank 1-8
-        ReportWithPackageFilter(WHS_filling,"------------ IFS get message response->index is  -----------------", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
-        WHS_info.event = IFS_INK_authentication_pass;
+        midTankCartColor = response->index; // midtank 1-8
+        Report("IFS get message response->index is", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
+        inkAuthenticationPass = true;
     }
     else
     {
-        ReportWithPackageFilter(WHS_filling,"------------ IFS get message response->index is out of rang -----------------", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
-        WHS_info.event = IFS_INK_authentication_fail;
+        Report("IFS get message response->index is out of rang", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
+        inkAuthenticationPass = false;
     }
-
-    WasteTankCBFunction();//call cart is ok
-    ret = OK;
-
-return ret;
+    return OK;
 }
 
-
-/**
- *
- * @brief that response is from STUB. replace the PPC
- *  use stub : Poweroff_progress.cs from P:\Users - Public\Shai_Frieder
- *  ProgressRequest progressRequest = new ProgressRequest();
- *  progressRequest.Amount = 0xCC; //porgress id is 0xCC
- *  progressRequest.Delay = 1; //midtank number from 0-7
- *
- *
- * @return OK
- */
 void ResponseDemo(int MidtankId)
 {
-    WHS_info.event = IFS_INK_authentication_pass;
-    //global mid tank id = MidtankId;
-    IFS_info.Ink.cart_color = MidtankId;
-    WasteTankCBFunction();//call cart is ok
+    inkAuthenticationPass = true;
+    midTankCartColor = MidtankId;
 }
-uint32_t MidTankReadControlId = 0xFF;
-int MidTankCounter = 0;
-uint32_t MidTankCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
+
+void midTankStateMachine(void)
 {
-    int MidTankOpenAir = 8;
-    int MidTankReadPressure = 16;
-    int MidTankCloseAir = 40;
-    int MidTankEnd = 48;
-    int portId;
+    static int init = 1;
+    static int authenticationTimeOutCounter = 0;
 
-    if (MidTankCounter >= MidTankEnd)
-    {
-        SafeRemoveControlCallback(MidTankReadControlId, MidTankCallBackFunction);
+    if (init) {
+        doorState = midTankCart_door();
+        prevDoorState = doorState;
+        init = 0;
+    } else {
+#ifndef IFS_DEBUG
+        doorState = midTankCart_door();
+#endif
     }
-    else if (MidTankCounter >= MidTankCloseAir)
-    {
-        //close air valve for midtank (MidTankCounter-MidTankCloseAir)
-        portId = (MidTankCounter - MidTankCloseAir) ; //0-7
-        if (IDS_IsHomingActive(portId) == false)
-        {
-            Disable_MidTank_Pressure_Reading(portId);
-            Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
+    if (!RdInkCartridgeSensor()) { // cartridge not presence
+        Pannel_Leds( CART_1,  MODE_OFF);
+        cart1.color = colorOFF;
+        if (IFS_MidTankFilling()) {
+            MidTankValvesAction(Cartridge_MidTank_OFF);
+            Report("Cartridge extracted during filling!!!", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+            Report("Mid-tank # filling fail", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+            midTankState = MidTankStateIdle;
         }
-    }
-    else if (MidTankCounter >= MidTankReadPressure)
-    {
-        //read pressure for midtank (MidTankCounter-MidTankCloseAir)
-        for (portId = 0;portId < MAX_SYSTEM_DISPENSERS;portId++)
-        {
-            Read_MidTank_Pressure_Sensor(portId);
+    } else { // cartridge presence
+        if (cart1.color == colorOFF) {
+            Pannel_Leds( CART_1,  MODE_ON);
+            cart1.color = colorON;
         }
     }
-    else if (MidTankCounter >= MidTankOpenAir)
+    switch (midTankState)
     {
-        //open air valve for midtank (MidTankCounter-MidTankOpenAir)
-        portId = (MidTankCounter - MidTankOpenAir) ; //0-7
-        Enable_MidTank_Pressure_Reading(portId);
-        Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_ON );   //Atm_MidTank_OFF/ON
-    }
-    MidTankCounter++;
-    return OK;
-}
+        case MidTankStateIdle:
+            if ((doorState == CLOSE) && (prevDoorState == OPEN)) {
+                //check if power down in process
+                if (PowerOffInProcessGetState()) {
+                    Report("Power-down in process", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                    break;
+                }
 
-uint32_t MidTankReading(void)
-{
-    MidTankCounter = 0;
-    if (IFS_info.Ink.time_out > 0)
-        return ERROR;
-    MidTankReadControlId = AddControlCallback("MidTankReading", MidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,0,0, 0 );
+                //check RFID? (TBD)
 
-    return OK;
-}
+                // send request for validation to ppc
+                inkAuthenticationPass = 2; //not defined
+                authenticationTimeOutCounter = 0;
+                ReadInkCartridgeData();
 
-bool  INKCartridgeAuthentication(cartridge_name cart_name)
-{
-    bool ret = notOK;
-    // TBD;
-    switch (cart_name)
-    {
-        case INK_cartridge:
-            IFS_info.Ink.autheticate = PASSED;  //todo
-            //Pannel_Leds(CART_1, MODE_ON);
+                //move to next state
+                midTankState = MidTankStateWait;
+                Report("move to midTankStateWait", __FILE__, prevDoorState, doorState, RpMessage, midTankState, 0);
+            }
+            if (doorState != prevDoorState) {
+                prevDoorState = doorState;
+            }
             break;
+        case MidTankStateWait:
+            if (inkAuthenticationPass == 2) {
+                authenticationTimeOutCounter++;
+                Report("MidTank Cartridge authentication wait...", __FILE__, __LINE__, authenticationTimeOutCounter, RpMessage, 0, 0);
+                if (authenticationTimeOutCounter > 60) {
+                    midTankState = MidTankStateIdle;
+                }
+                break;
+            }
+            if (inkAuthenticationPass == false) {
+                Report("MidTank Cartridge authentication fail", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+                break;
+            }
+
+            midTankCapacity = Get_MidTank_Pressure_Sensor(midTankCartColor);
+            if (midTankCapacity > MidTankEmptyLimit)
+            {
+                Pannel_Leds(CART_1, MODE_ON);
+                cart1.color = fastBILNK;
+                Report("Mid-tank not empty", __FILE__, __LINE__, midTankCapacity, RpMessage, MidTankEmptyLimit, 0);
+                midTankState = MidTankStateIdle;
+                break;
+            }
+            Report("Mid-tank filling in process", __FILE__,__LINE__,(int)(MidTankEmptyLimit*1000), RpMessage, (int)(midTankCapacity*1000), 0);
+            Pannel_Leds(CART_1, MODE_ON);
+            cart1.color  = BLINK;
+
+            if (midTankCartColor == MIDTANK_8)
+            {
+                CartridgeInkTimeout = CARTRIDGE_INK_TIMEOUT*4;
+                CartridgeCheckTime = FIVE_SECONDS*3;
+            }
+            // set valve selection
+            MidTankValvesAction(Cartridge_MidTank_ON);
+            Report("midTankStateWait set valve", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+
+            // RFID change status - TBD
 
+            // set count down
+            midTankTimeoutCounter = 1;
+
+            //start  filling
+            midTankState = MidTankStateFilling;
+            break;
+        case MidTankStateFilling:
+            // if time out or emptying done - idle
+            if (midTankTimeoutCounter++ > CartridgeInkTimeout) {
+                midTankTimeoutCounter = 0;
+                Pannel_Leds(CART_1, MODE_ON);
+                cart1.color = fastBILNK;
+                MidTankValvesAction(Cartridge_MidTank_OFF);
+                Report("Mid-tank # filling fail", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                Report("Call customer support", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+            } else if (CartridgeInkFull()) {
+                midTankTimeoutCounter = 0;
+                Pannel_Leds(CART_1, MODE_OFF);
+                cart1.color = colorOFF;
+                MidTankValvesAction(Cartridge_MidTank_OFF);
+                Report("Mid-tank # filling done", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+            }
+            break;
         default:
-            //WHS_info.cartridge_1.autheticate = FAILED; //todo
-           // WHS_info.cartridge_2.autheticate = FAILED; //todo
-            //Pannel_Leds(CART_2, MODE_OFF);
-            //Pannel_Leds(CART_3, MODE_OFF);
+            Report("wrong state", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
             break;
     }
-    ret = OK;
-    return ret;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
index 6225dc1d4..bf42c0e9a 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
@@ -11,55 +11,17 @@
 #ifndef MODULES_IFS_IFS_H_
 #define MODULES_IFS_IFS_H_
 
-#include "Modules/Waste/Waste.h"
+#define CARTRIDGE_INK_TIMEOUT           420
+#define MidTank_Pressure_EMPTY          0.5
+#define CARTRIDGE_CAPATICY              1500
 
-//void IFS_Init(void)
-#define SECONDS_5                       5               /**<  5seconds  */
-#define SECONDS_300                     300             /**<  300 seconds = 5 minutes  */
-#define SECONDS_420                     (7*60)          /**<  420 seconds = 7 minutes  */
-#define SECONDS_600                     600             /**<  600 seconds = 10 minutes  */
-#define SECONDS_2_DLY                   0               /**< dly 0 seconds  */
-#define CARTRIDGE_INK_TIMEOUT           (SECONDS_420 / SECONDS_5)             /**< TimeOut (7 minutes) for finishing transfer the Ink from Ink cartridge to MidTank  */
-#define CARTRIDGE_INK_TIMEOUTx2         (CARTRIDGE_INK_TIMEOUT * 2)           /**< TimeOut (7 minutes) for second try finishing transfer the Ink from Ink cartridge to MidTank  */
-#define CartridgeInkTimeOutCallBackTime (eOneSecond * SECONDS_5)
-#define CartridgeInkCallBackTime        eOneSecond
-#define MidTank_Pressure_EMPTY          0.3  //todo get from Moti the correct number  (Oleg -> 300cc)
-#define MidTank_PP_Pressure_EMPTY       0.5  //todo get from Moti the correct number  (Oleg -> 300cc)
-#define CARTRIDGE_CAPATICY 1500 //todo get from Moti the correct number
-#define FULL 1000   //todo get from Moti the correct number
-
-
-
-/**
- *  A structure to represent IFS_information
- */
-struct IFS_information
-{
-    struct cartridge_params Ink;                                /**< cartridge_params */
-    CarteidgPrecenseSensorStatus IFS_Ink_precense_sensor;       /**< INK cartridge presence(1) or not_precense(0) */
-    uint32_t Cartridge_Ink_device_Id;                           /**< INK device_Id number for callback */
-    uint32_t Cartridge_Ink_TimeOut_device_Id;                   /**< INK device_Id number for TimeOut callback */
-    uint32_t INK_isOK_flag ;                                    /**< INK cartridge pass authentication (from PPC) */
-    double MidTank_capacity;                                    /**< MidTank current capacity  */
-};
-
-struct IFS_information IFS_info;
-
-WHS_sensor CartridgeInkCallBackFunction();
-WHS_sensor CartridgeInkTimeOutCallBackFunction();
-uint32_t ReadCartridgeData(cartridge_name cart_name);
-bool INKCartridgeAuthentication(cartridge_name cart_name);
-bool MidTankValvesAction(bool action); //Cartridge_MidTank_ON of Cartridge_MidTank_OFF
-bool ColorMatch();
-bool IFS_TimeOutAlarm(bool status);
-void ResponseDemo(int MidtankId);
-bool IFS_MidTankFilling(void);
-MidTank_t IFS_MidTankIsActive(void);
-
-bool IFS_CartridgeLowerPresent();
-
-bool CartridgeValidationResponseFunc(MessageContainer* requestContainer);
+typedef enum{
+    MidTankStateIdle,
+    MidTankStateWait,
+    MidTankStateFilling
+}MidTankStateEnum;
 
+void midTankStateMachine(void);
 uint32_t MidTankReading(void);
-
+void ResponseDemo(int MidtankId);
 #endif /* MODULES_IFS_IFS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 4a7008a7a..bfea7b13b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -63,6 +63,7 @@ extern uint8_t Test_Read_Buf[4];
 
 extern uint8_t Head_Fan_PWM_Command[2];
 extern uint16_t Head_Fan_Tach[2];
+extern bool doorState;
 
 bool RestartNeeded = false;
 
@@ -525,6 +526,14 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
+    if(request->amount == 0xAD9) //halt
+    {
+        LOG_ERROR(request->delay,"halt");
+        memset (0,0,100000);
+        response.progress = IgnoreConeMissing;
+        response.has_progress = true;
+    }
+    else
     if((request->amount == 0x01) && ((request->delay &0x010000) == 0x010000)) //change mode powerset01
     {
         response.progress = Power_Step_01_Mode(((request->delay &0x00FF00)>>8), request->delay &0x0000FF);
@@ -1051,6 +1060,16 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
             response.has_progress = true;
         }
     else
+    if(request->amount == 0xB9)  //fast refresh for pressure
+    {
+        if ((request->delay == 0) || (request->delay == 1)) {
+            doorState = request->delay;
+            LOG_ERROR(request->delay,"door");
+        }
+        response.progress = request->delay;
+        response.has_progress = true;
+    }
+    else
     if(request->amount == 0xC3)  //suspend I2C task
     {
         if (request->delay == 0)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 42a56ae8a..1c52a9b68 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -352,9 +352,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0))
     {
-        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
         SendJobProgress(0.0,0,false, Lenstr);
-        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0);
         // segment/intersegment/distance to spool finished
         if (ProcessedLengthFuncPtr)
             ProcessedLengthFuncPtr();
@@ -696,18 +696,19 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             }*`/
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
-        //if (JobCounter % eHundredMillisecond == 50)
-        if (FirstCalcInJob == true)
+        if (0)//(JobCounter % 1000 == 0)
+        //if (JobCounter < 100)//(FirstCalcInJob == true)
         {
-            if (index == POOLER_MOTOR)
+            if (index == FEEDER_MOTOR)
             {
-                FirstCalcInJob = false;
-                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
-                                ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
+           //     FirstCalcInJob = false;
+                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",
+                                TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
-                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
-                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));
+                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);
 
             }
         }
@@ -720,6 +721,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             if (calculated_speed>5.0)
             {
+                if (calculated_speed>(CurrentControlledSpeed[index]+100))
+                {
+                        ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,CurrentControlledSpeed[index]+100,0);
+                        calculated_speed=CurrentControlledSpeed[index]+100;
+                }
                 CurrentControlledSpeed[index] = calculated_speed;
                 MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
             }
@@ -845,6 +851,7 @@ uint32_t Release_Right_TFU_Tension()
     return status;
 }
 int SecondFeederCorrection = 4;
+int PrepareWaitCount = 0;
 uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue)
 {
     MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands
@@ -854,6 +861,18 @@ uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadVa
         Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0);
         Release_Right_TFU_Tension();
     }
+    if (PrepareWaitCount)
+    {
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareWaitCount--;
+    }
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
+    {
+        PrepareState = false;
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareReady(Module_Thread,ModuleDone);
+    }
+
     return OK;
 }
 uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
@@ -869,16 +888,16 @@ uint32_t Adjust_Right_TFU_Tension(double tension)
     uint32_t status = OK;
     if (tension > 0.5) //0 = lower position, 1 = high position
     {
+        //PrepareWaitCount++;
         if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
         {
-            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 40,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
+            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
             Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,GPI_LS_RDANCER_UP,0);
         }
     }
 
     return status;
 }
-int PrepareWaitCount = 0;
 uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
 {
     if (PrepareWaitCount)
@@ -886,7 +905,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
         ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
         PrepareWaitCount--;
     }
-    if (PrepareWaitCount == 0)
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
     {
         PrepareState = false;
         ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
@@ -1046,71 +1065,73 @@ uint32_t ThreadPrepareState(void *JobDetails)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
-            MotorControlConfig[Motor_i].m_params.MAX = 1;
-            MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
-            MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
-            MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
-            MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
-            MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
-            MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
-            MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
-            MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
-            MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
-            MotorControlConfig[Motor_i].m_calculatedError = 0;
-            MotorControlConfig[Motor_i].m_integral = 0;
-            MotorControlConfig[Motor_i].m_isEnabled = true;
-            MotorControlConfig[Motor_i].m_isReady = true;
-            MotorControlConfig[Motor_i].m_mesuredParam = 0;
-            MotorControlConfig[Motor_i].m_preError = 0;
-            MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorControlConfig[Motor_i].m_params.MAX = 1;
+        MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
+        MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
+        MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
+        MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
+        MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
+        MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
+        MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
+        MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
+        MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[Motor_i].m_calculatedError = 0;
+        MotorControlConfig[Motor_i].m_integral = 0;
+        MotorControlConfig[Motor_i].m_isEnabled = true;
+        MotorControlConfig[Motor_i].m_isReady = true;
+        MotorControlConfig[Motor_i].m_mesuredParam = 0;
+        MotorControlConfig[Motor_i].m_preError = 0;
+        MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
-		    HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
+        HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
-            temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
-            MotorTiming[Motor_i] = (int)temp_dt;
-            if (MotorTiming[Motor_i])
-            {
-                MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
-                ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
-            }
+        temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+        MotorTiming[Motor_i] = (int)temp_dt;
+        if (MotorTiming[Motor_i])
+        {
+            MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+            ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
+        }
 //////////////////////////////////////////////////
-            for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
-            {
-                //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                    MotorSamples[Motor_i][i] = 0;
-               // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
-                //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
-                //MotorSpeedSamples[Motor_i][i] = 0;
-            }
-            MotorSamplePointer[Motor_i] = 0;
+        for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+        {
+            //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                MotorSamples[Motor_i][i] = 0;
+                if (Motor_i == FEEDER_MOTOR)
+                    MotorSamples[Motor_i][i] = -500;
+           // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
+            //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
+            //MotorSpeedSamples[Motor_i][i] = 0;
+        }
+        MotorSamplePointer[Motor_i] = 0;
 /////////////////////////////////////////////////////
-            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
+        MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
-            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        {
+            ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (SpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (SpeedControlId != 0xFF)
-                {
-                    RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
-                    SpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                SpeedControlId = 0xFF;
             }
-            if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
+        if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        {
+            ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (PoolerSpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (PoolerSpeedControlId != 0xFF)
-                {
-                    if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
-                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
-                     PoolerSpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
+                    ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
+                 PoolerSpeedControlId = 0xFF;
             }
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
@@ -1152,10 +1173,6 @@ uint32_t ThreadPrepareState(void *JobDetails)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
-//            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
-            if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                continue;
     }
 
 #ifdef TEST_PID_THREAD
@@ -1382,7 +1399,7 @@ char Endstr[150];
      int Motor_i;
      ThreadControlActive = false;
      uint32_t status = OK;
-     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
      SendJobProgress(0.0,0,false, Endstr);
      ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
index 2fdd3a479..7d6837018 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
@@ -1,5 +1,5 @@
-#ifndef WASTE_MAINT_H
-#define WASTE_MAINT_H
+#ifndef WASTE_EX_H
+#define WASTE_EX_H
 
 typedef enum {
     CartridgeStateOUT,
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
index 03ee73daf..870463321 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
@@ -1,4 +1,5 @@
 /*   ----  WHS      */
+#if 0
 #include <Container.h>
 #include <DataDef.h>
 #include "include.h"
@@ -1446,7 +1447,7 @@ U8 WHS_HW_test()
     return ret;
 }
 
-
+#endif
 /*------------------------------------------------------------------------------------------------*/
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index 099fc0c2e..dc76a3390 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -32,6 +32,10 @@ uint32_t Cartridge_Cover_Control;
 button *CartLed[MaxWasteCartridges] = {0, &cart2, &cart3};
 WasteTankStateEnum WasteTankState = WasteTankStateIdle;
 
+int init = 1;
+
+bool setWasteValve (WasteCartridgeEnum WasteCartridge);
+
 #define WASTE_CARTRIDGE_SIZE 1500
 #define WASTE_LEVEL_OVERFLOW 2700
 #define WASTE_LEVEL_FULL 2300
@@ -65,6 +69,8 @@ void cartCART_EXTRACTED(WasteCartridgeEnum CartId) //{called from polling functi
     Report("Cartridges Cart extracted", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
     CartState[CartId] = CartridgeStateOUT;
     CartLed[CartId]->color = colorOFF;
+    if (init==0)
+        WHS_Set_IFS_Clearing_Suction(NUM_OF_MIDTANKS);
 
 }
 WasteCartridgeEnum cartSELECT_CART(void) //{ if there is no SELECTED cartridge select the first cartridge in IN state. If there is none, return -1}
@@ -113,6 +119,8 @@ void cartFILLING_END(void)
     } else {
         Report("Cartridges end with no active cartridge", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0);
     }
+    setWasteValve(WasteCartridge_middle);
+
 }
 
 CartridgeStateEnum cartGetState(WasteCartridgeEnum CartId)
@@ -163,6 +171,119 @@ WasteCartridgeEnum cartNotAllFull()
     return false;
 }
 
+bool SetWastePump( bool power)
+{
+    if ( power == OPEN ) {
+        if (WHS_Type == WHS_TYPE_UNKNOWN) {
+            Pumps_Control(WHS_WTANKPUMP2, OPEN); //waste_pump_power_on();
+        } else { //new WHS
+            Trigger_SetWHSPump(OPEN);
+        }
+        return OPEN;
+    } else {
+        if (WHS_Type == WHS_TYPE_UNKNOWN) {
+            Pumps_Control(WHS_WTANKPUMP2, CLOSE); //waste_pump_power_on();
+        } else { //new WHS
+            Trigger_SetWHSPump(CLOSE);
+        }
+        return CLOSE;
+    }
+}
+
+bool setWasteValve (WasteCartridgeEnum WasteCartridge)
+{
+    bool direction;
+
+    if (WasteCartridge == WasteCartridge_lower) {
+        direction = true;
+    } else {
+        direction = false;
+    }
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        Valve_Set(VALVE_WASTE_TANK,direction);
+    } else { //new WHS
+        Trigger_SetWHSValveWatseCartridge(direction);
+    }
+    return direction;
+}
+
+bool RdWasteTankEmptySensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WCONTAINER_WARN();
+    } else { //new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel < wasteLevelEmpty)
+            return 1; //Empty
+    }
+    return 0;
+}
+
+bool RdWasteTankFullSensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WCONTAINER_FULL();
+    } else { //new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel >= wasteLevelFull)
+            return 1; //full
+    }
+    return 0;
+}
+
+bool RdWasteTankOverFlowSensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WASTE_OVERFULL();
+    } else {//new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel > wasteLevelOverflow)
+            return 1; //overflow
+    }
+    return 0;
+}
+
+bool WHS_IsContainerFull()
+{
+    return RdWasteTankFullSensor();
+}
+
+bool WHS_IsContainerEmpty()
+{
+    return RdWasteTankEmptySensor();
+}
+
+bool WHS_IsContainerOverflow()
+{
+    return RdWasteTankOverFlowSensor();
+}
+
+bool WHS_WasteCartridgeLowerPresent()
+{
+    return Is_Cartridge_Present(CART_3);
+}
+
+bool WHS_WasteCartridgeMiddlePresent()
+{
+    return Is_Cartridge_Present(CART_2);
+}
+
+bool WHS_IsEmptying()
+{
+    if (WasteTankState == WasteTankStateEmptying) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void Waste_Init()
 {
     CartState[WasteCartridge_middle] = CartridgeStateOUT;
@@ -186,7 +307,6 @@ bool cartCart_door()//Polled by polling function. Notify waste. Poll each 1 seco
     return DoorState;
 }
 
-//Waste_DoorOpenDuringEmptying
 uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
 {
     cartCart_door();
@@ -212,7 +332,6 @@ uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
 
 void Waste_StateMachine_OneSecond_Call(void)
 {
-    static int init = 1;
     static WasteTankStateEnum prev_state = 0;
     static int prev_wasteLevel = 0;
 
@@ -263,7 +382,7 @@ void Waste_StateMachine(void)
     switch (WasteTankState)
     {
         case WasteTankStateIdle:
-            if (wasteLevel > wasteLevelOverFlow) {
+            if(WHS_IsContainerOverflow()) {
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  true);
             } else {
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  false);
@@ -271,7 +390,7 @@ void Waste_StateMachine(void)
             if (cartNotAllFull() == true) {
                 AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  false);
             }
-            if (wasteLevel > wasteLevelFull) {
+            if(WHS_IsContainerFull()) {
                 //check if power down in process
                 if ( PowerOffInProcessGetState() ) {
                     Report("Power-down in process", __FILE__, __LINE__, 0, RpMessage, 0, 0);
@@ -307,9 +426,8 @@ void Waste_StateMachine(void)
 
             // set valve selection
             activeCart = cartGetActiveCart();
-            Valve_Set(VALVE_WASTE_TANK, activeCart);
-            Report("WasteTankStateEmptying set valve", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
-
+            //Report("WasteTankStateEmptying set valve", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
+            setWasteValve(activeCart);
             // RFID change status - TBD
 
             // set count down
@@ -317,6 +435,7 @@ void Waste_StateMachine(void)
 
             // set emptying limit
             wasteLevelEmpty = wasteLevel - WASTE_CARTRIDGE_SIZE;
+            waste_seq_step1_cont();
 
             // start a timer to observe door opening
             Cartridge_Cover_Control = AddControlCallback("Cartridge Cover Door", Waste_DoorOpenDuringEmptying, 100,  cartCart_door, 0, 0, 0);
@@ -338,15 +457,18 @@ void Waste_StateMachine(void)
                 Report("WasteTankStateEmptying TimeOut", __FILE__, __LINE__, 0, RpMessage, 0, 0);
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_EMPTYING_TIMEOUT,  true);
                 RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
-            } else if (wasteLevel < wasteLevelEmpty) {
+                timeout_counter = 0;
+            } else if (WHS_IsContainerEmpty()) {
                 SetWastePump(CLOSE);
                 cartFILLING_END();
+                waste_seq_cont_stop();
                 // RFID change status - TBD
                 WasteTankState = WasteTankStateIdle;
                 RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
                 if (cartNotAllFull() == false)
                     AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
                 Report("WasteTankStateEmptying done", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                timeout_counter = 0;
             }
 #ifdef WHS_DEBUG
             wasteLevel -= 100;
@@ -355,6 +477,21 @@ void Waste_StateMachine(void)
             break;
         case WasteTankStatePaused:
             Report("WasteTankStatePaused...", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+            activeCart = cartGetActiveCart();
+            if (activeCart == MaxWasteCartridges)
+            {
+                SetWastePump(CLOSE);
+                cartFILLING_END();
+                waste_seq_cont_stop();
+                // RFID change status - TBD
+                WasteTankState = WasteTankStateIdle;
+                RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
+                if (cartNotAllFull() == false)
+                    AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
+                Report("WasteTankStatePaused done", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                timeout_counter = 0;
+
+            }
             break;
         default:
             Report("wrong state", __FILE__, WasteTankState, 0, RpMessage, SelectedCard, 0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h
deleted file mode 100644
index e69de29bb..000000000
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h
+++ /dev/null
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_print.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_print.c
deleted file mode 100644
index e69de29bb..000000000
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_print.c
+++ /dev/null