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

19 files changed, 654 insertions, 149 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 5f9c3130b..4ad5336d9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -43,6 +43,7 @@
 #include "modules/thread/thread_ex.h"
 #include "modules/ifs/ifs.h"
 #include "modules/ids/ids_ex.h"
+#include "modules/waste/waste_ex.h"
 #include "modules/Diagnostics/Diagnostics.h"
 #include "Modules/General/MachineStatus.h"
 
@@ -658,7 +659,6 @@ uint32_t MillisecLowLoop(uint32_t tick)
 #ifndef EVALUATION_BOARD
         Read_Buttons_Reg();
 #endif
-        //Ink_Cart_Led();
         ///////////////////////////////////////////////////////////////////
 
 
@@ -766,6 +766,8 @@ uint32_t MillisecLowLoop(uint32_t tick)
             }
             */
         }
+        //call waste state machine
+        Waste_StateMachine_OneSecond_Call();
     }
     if (Tensecond_Tick)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 0f9848ba1..26fba0194 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -170,6 +170,7 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+int FPGA_ReInit_Count = 0; ///avoid too many reinitializations of motors as happens when FPGA is corrupted
 uint32_t ControlActivityLed( uint32_t Parameter1)
 {
     static bool flag = false;
@@ -192,11 +193,14 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
                     SendJobProgress(0.0,0,false, "Hardware Failure Error");
                     AbortJob("FPGA Watchdog Error");
                 }
-                ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+                if (FPGA_ReInit_Count++<20)
+                {
+                    ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
 
-                ACTIVITY_GREEN_LED_ON;
-                FPGA_SetMotorsInit();
-                Motor_ReconfigAllMotors();
+                    ACTIVITY_GREEN_LED_ON;
+                    FPGA_SetMotorsInit();
+                    Motor_ReconfigAllMotors();
+                }
             }
         }
 		else
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 6d4d1c668..27ed8d0e9 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -165,8 +165,12 @@ uint32_t HWConfigurationInit(void)
 #ifndef DISPESER_TEST
     if (WHS_Type == WHS_TYPE_NEW)
         newWHS_init();
-    WHS_init();
+    //WHS_init(); // remove call to old WHS
 #endif
+
+    // Waste Init (WHS)
+    Waste_Init();
+
     ADC_MUX_Init();
     GeneralHwReady = true;
 
@@ -554,7 +558,6 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     }
     ControlStart();
     AlarmHandlingStart();
-    //ThreadInitialTestStub(request);
     GeneralHwReady = true;
     STATUS_RED_LED_OFF;
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/General/Safety.c b/Software/Embedded_SW/Embedded/Modules/General/Safety.c
index 400b083e4..89ac8d020 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,16 +91,6 @@ 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__WASTE_CONTAINER_OVERFLOW, true);
-                       mWasteOverflowAlarmState = true;
-                       WasteOverflowAlarmState = true;
-                   }
-               }
            }
        }
 
@@ -234,12 +224,6 @@ 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.c b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
index f81cfb88f..328ff3203 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/buttons.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
@@ -691,34 +691,6 @@ return OK;
     return OK;
 }*/
 
-void Ink_Cart_Led()//temporary for ITMA
-{
-    if(Is_Cartridge_Present(CART_1) == true)
-    {
-        Pannel_Leds( CART_1,  MODE_ON);
-    }
-    else
-    {
-        Pannel_Leds( CART_1,  MODE_OFF);
-    }
-    if(Is_Cartridge_Present(CART_2) == true)
-    {
-        Pannel_Leds( CART_2,  MODE_ON);
-    }
-    else
-    {
-        Pannel_Leds( CART_2,  MODE_OFF);
-    }
-    if(Is_Cartridge_Present(CART_3) == true)
-    {
-        Pannel_Leds( CART_3,  MODE_ON);
-    }
-    else
-    {
-        Pannel_Leds( CART_3,  MODE_OFF);
-    }
-}
-
 void test_avi()
 {
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
index 41e729b9b..647797b69 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
@@ -28,6 +28,9 @@ extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS];
 extern bool HomingActive[MAX_SYSTEM_DISPENSERS];
 extern bool PrimingActive[MAX_SYSTEM_DISPENSERS];
 
+extern uint32_t LeftRockerSpeed;
+extern uint32_t RightRockerSpeed;
+
 uint32_t IDS_Dispenser_EmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue);
 
 /*typedef struct
@@ -45,6 +48,7 @@ void IDS_Dispenser_RefillEnded (char DispenserId,char  MicroSteps);
 
 //uint32_t IDS_Dispenser_Build_Pressure(int DispenserId, callback_fptr callback);
 
+uint32_t IDS_Cleaning_Move_Actuators(void);
 uint32_t IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed);
 uint32_t IDS_Cleaning_Center_And_Stop_Rockers (int timeout,callback_fptr callback);
 uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr callback);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
index bdde05b43..8f10a0e5d 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
@@ -13,11 +13,73 @@
 #include "PMR/Hardware/Hardwaremotor.pb-c.h"
 #include "PMR/Hardware/HardwareDispenser.pb-c.h"
 #include "StateMachines/Printing/printingSTM.h"
+#include <Drivers/I2C_Communication/I2C_Task.h>
+
 #include "drivers/motors/motor.h"
 #include "drivers/valves/valve.h"
+#include "Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h"
 
 int SaveLeftRockerSpeed = 50, SaveRightRockerSpeed = 50;
+typedef enum
+{
+    CleaningStageIdle,
+    CleaningStageActuatorUp = 1,
+    CleaningStageDelay1,
+    CleaningStageActuatorDown,
+    CleaningStageDelay2,
+}CleaningStageEnum;
+
+/*
+ *Cleaning sequence:
+
+
+t up – time actuator goes up
+t delay1 – time actuator stays in position (up position – not limit switch)
+Down to limit -  actuator goes to lower limit switch
+t delay2 – time actuator stays in lower limit switch position
+
+Values for start:
+t up – 300 msec
+t delay1 – 500 msec
+t delay2 – 2 sec
+
+ */
+
+CleaningStageEnum CleaningStage = CleaningStageIdle;
+int CleaningStageCounter = 0;
+int Tup = 4,Tdelay1 = 9,Tdelay2 = 29;
+
+uint32_t IDS_Cleaning_Move_Actuators()
+{
+   if(Head_Type != HEAD_TYPE_SYLKO)
+       return OK;
+   if (CleaningStageCounter == 1)
+   {
+       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();
+       Report("IDS_Cleaning_Stop_Actuator", __FILE__, __LINE__, Tup, RpWarning,   CleaningStageCounter, 0);
+   }
+   else if (CleaningStageCounter == Tdelay1)
+   {
+       Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+       Report("IDS_Cleaning_MoveDown", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
+   }
+   else if (CleaningStageCounter == Tdelay2)
+   {
+       CleaningStageCounter = 0;
+       Report("IDS_Cleaning_Stop_delay", __FILE__, __LINE__, Tdelay2, RpWarning,   CleaningStageCounter, 0);
+   }
 
+
+
+   CleaningStageCounter++;
+
+   return OK;
+}
 uint32_t IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed)
 {
     uint32_t status = OK;
@@ -34,17 +96,25 @@ uint32_t IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed)
 uint32_t IDS_Cleaning_Center_And_Stop_Rockers (int timeout,callback_fptr callback)
 {
     uint32_t status = OK;
-    status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, SaveRightRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], callback,timeout);
-    status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, SaveLeftRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], callback,timeout);
-    Report("IDS_Cleaning_Center_And_Stop_Rockers", __FILE__, __LINE__, timeout, RpWarning,   123456, 0);
+    //status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, SaveRightRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], callback,timeout);
+    //status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, SaveLeftRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], callback,timeout);
+    Trigger_Head_Actuators_Disable();
+    Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+    CleaningStageCounter = 0;
+    CleaningStage = CleaningStageIdle;
+    Report("IDS_Cleaning_Center_And_Stop_Rockers actuator down", __FILE__, __LINE__, timeout, RpWarning,   123456, 0);
 
     return status;
 }
 uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr callback)
 {
     uint32_t status = OK;
+    Tup = LeftRockerSpeed/100;
+    Tdelay1 = LeftRockerSpeed%100;
+    Tdelay2 = RightRockerSpeed;
     status = IDS_Dispenser_Start_Motor_and_Open_Valve(CLEANER_DISPENSER, dispenserSpeed, callback);
     Report("IDS_Cleaning_Spray_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   dispenserSpeed, 0);
+    Report("IDS_Cleaning parameters", __FILE__, Tup*100, Tdelay1*100, RpWarning,   Tdelay2*100, 0);
 
     return status;
 
@@ -54,6 +124,11 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback)
     uint32_t status = OK;
     status = IDS_Dispenser_Close_Valve_And_Stop_Motor(CLEANER_DISPENSER,callback);
     Report("IDS_Cleaning_Stop_Cleaning_Solution", __FILE__, __LINE__, CLEANER_DISPENSER, RpWarning,   status, 0);
+    if(Head_Type == HEAD_TYPE_SYLKO)
+    {
+        Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+        Report("IDS_Cleaning_MoveDown cleaning stop", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
+    }
 
     return status;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index b3f21e187..831fa18cd 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -45,6 +45,8 @@ extern DispenserRunningData IDS_Dispenser_Data[MAX_SYSTEM_DISPENSERS];
 //extern DispenserData IDSDispenserData;
 extern DispenserRunningData **dispenserdata;
 
+extern bool ActuatorsCleaningSequence;
+
 extern bool         DispenserUsedInJob[MAX_SYSTEM_DISPENSERS];
 extern int32_t CurrentDispenserSpeed[MAX_SYSTEM_DISPENSERS];
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index a76453f95..b4442c996 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -51,6 +51,7 @@ HardwarePidControlType ThreadDispenserIdToControlId[MAX_SYSTEM_DISPENSERS] = {
 JobUploadStrategy uploadstrategy = JOB_UPLOAD_STRATEGY__Default;
 bool    DispenserReady[MAX_SYSTEM_DISPENSERS] = {true};
 bool IDS_Active = false;
+bool ActuatorsCleaningSequence = true;
 
 bool Special_Dispensers = false;
 /******************** STRUCTURES AND ENUMs  ********************************************/
@@ -78,7 +79,7 @@ int lInterSegmentLength = 0;
  uint32_t InterSegmentStartRocking = 0;
  uint32_t InterSegmentCenterRockers = 0;
  uint32_t LeftRockerSpeed = 20;
- uint32_t RighttRockerSpeed = 20;
+ uint32_t RightRockerSpeed = 20;
  uint32_t CleaningDispenserSpeed = 40;
  uint32_t InterSegmentStartWFCFDispensers;
  uint32_t WFCF = 80;
@@ -115,7 +116,7 @@ void IDS_Dispenser_SetPreSegmentWFCFValues(double dispenserpresegmentwfcf, doubl
      if ( ids_leftcleaningmotorspeed)
          LeftRockerSpeed = ids_leftcleaningmotorspeed;
      if ( ids_rightcleaningmotorspeed)
-         RighttRockerSpeed = ids_rightcleaningmotorspeed;
+         RightRockerSpeed = ids_rightcleaningmotorspeed;
      Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,CleaningDispenserSpeed,RpWarning,(int)LeftRockerSpeed,0);
      Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,InterSegmentStartSprayCleaner,RpWarning,(int)InterSegmentCenterRockers,0);
 
@@ -1089,6 +1090,12 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
              Report("Start cleaning rockers",__FILE__,__LINE__,InterSegmentStepsCount,RpWarning,(int)lInterSegmentLength,0);
              //IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed);
          }*/
+         if (( InterSegmentStartSprayCleaner )&&
+            ((InterSegmentStepsCount>InterSegmentStartSprayCleaner)&&(InterSegmentStepsCount<(lInterSegmentLength-InterSegmentCenterRockers) )))
+         {
+             if (ActuatorsCleaningSequence)
+                 IDS_Cleaning_Move_Actuators();
+         }
          if (( InterSegmentCenterRockers)&&(InterSegmentCenterRockers == (lInterSegmentLength-InterSegmentStepsCount)))
          {
              Report("Stop spray and center rockers",__FILE__,__LINE__,InterSegmentStepsCount,RpWarning,(int)lInterSegmentLength,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index cbdab01d1..4a7008a7a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -31,6 +31,7 @@
 #include "Modules/Control/MillisecTask.h"
 #include "modules/thread/thread_ex.h"
 #include "modules/heaters/heaters_ex.h"
+#include "modules/waste/waste_ex.h"
 #include <Drivers/I2C_Communication/I2C.h>
 #include <Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.h>
 #include "Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h"
@@ -500,6 +501,30 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
+    if(request->amount == 0xAD6) //Set skip open lids
+    {
+        LOG_ERROR(request->delay,"Set SkipOpenLids");
+        SkipOpenLids = request->delay;
+        response.progress = SkipOpenLids;
+        response.has_progress = true;
+    }
+    else
+    if(request->amount == 0xAD7) //Set actuators cleaning sequence
+    {
+        LOG_ERROR(request->delay,"Set actuators cleaning sequence");
+        ActuatorsCleaningSequence = request->delay;
+        response.progress = ActuatorsCleaningSequence;
+        response.has_progress = true;
+    }
+    else
+    if(request->amount == 0xAD8) //Set ignore cone missing
+    {
+        LOG_ERROR(request->delay,"Set ignore cone missing");
+        IgnoreConeMissing = request->delay;
+        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);
@@ -1016,6 +1041,16 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
 
     }
     else
+    if(request->amount == 0xB8)  //fast refresh for pressure
+        {
+            if (request->delay == 1) {//init waste
+                Waste_Init();
+                LOG_ERROR(request->delay,"Waste_Init");
+            }
+            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.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index d1d23598f..3a7045a6e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -76,7 +76,6 @@ extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
 uint32_t MotorsConfigMessage( HardwareConfiguration* HW_request);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
-uint32_t ThreadInitialTestStub(HardwareMotor * request);
 uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 void     SetKeepWindingCone(bool value);
 void SetWinderBackToBaseTime(uint32_t value);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 2b65ca806..f5bc45d85 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -204,6 +204,7 @@
             strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed");
             LoadStatus = ERROR;
             TryAgain = true;
+            //ThreadLoadButton(LoadStages);
         }
         else
         {
@@ -300,6 +301,7 @@
                     LoadStatus = ERROR;
                     ThreadLoadingReport();
                     TryAgain = true;
+                    //ThreadLoadButton(LoadStages);
                 }
                 else
                 {
@@ -544,6 +546,7 @@
                 LoadStatus = ERROR;
                 ThreadLoadingReport();
                 TryAgain = true;
+                //ThreadLoadButton(LoadStages);
             }
         }
 
@@ -647,6 +650,7 @@
             Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);
 
             ThreadLoadingReport();
+            //ThreadLoadButton(LoadStages);
         }
         return OK;
     }
@@ -857,6 +861,7 @@
             LoadStatus = ERROR;
             TryAgain = true;
             ThreadLoadingReport();
+            //ThreadLoadButton(LoadStages);
         }
         return OK;
     }
@@ -971,7 +976,7 @@
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
     {
         REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
-        ThreadLoaded();
+        //ThreadLoaded();
         ThreadLoadingReport();
 
         if (LoadStages!=ReadValue)
@@ -1089,7 +1094,7 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
             Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
             ThreadLoadStateMachine(LoadStages);
         }
-        else
+        else if (SecondTry == true)
         {
             SecondTry = false;
             Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 80ca05cb7..92b953db5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -54,7 +54,7 @@ double   ScrewRunningTime = 0;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 bool     SCREW_TimerActivated = false;
 uint32_t ScrewControlId = 0xFF;
-
+bool IgnoreConeMissing = false;
 
 
 static bool KeepWindingCone = false;
@@ -156,10 +156,13 @@ uint32_t Winder_Prepare(void *JobDetails)
         REPORT_MSG(LIMIT, "No cone in winder");
         if (Is_PP_Machine())
         {
-            JobEndReason = JOB_THREAD_BREAK;
-            PrepareReady(Module_Winder,ModuleFail);
-            AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
-            return ERROR;
+            if (IgnoreConeMissing == false)
+            {
+                JobEndReason = JOB_THREAD_BREAK;
+                PrepareReady(Module_Winder,ModuleFail);
+                AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
+                return ERROR;
+            }
         }
     }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 4081b53f2..4339c5312 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -24,6 +24,7 @@ typedef enum threadMotorsEnum
 extern double TotalProcessedLength;
 extern double PoolerTotalProcessedLength;
 extern int SecondFeederCorrection;
+extern bool SkipOpenLids,IgnoreConeMissing;
 
 void ThreadSetBreakSensorLimit(int limit);
 
@@ -37,7 +38,6 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId);
 uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId);
 uint32_t ThreadDistanceToSpoolState(void);
 uint32_t ThreadEndState();
-uint32_t ThreadInitialTestStub();
 
 bool Set_Thread_Rockers_Bypass (int value);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 760a0726b..e1f887303 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -112,8 +112,6 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
             {
                 IDS_Dispenser_Init(Motor_i-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
             }
-//        if (Motor_i == MOTOR_RDRIVING)
-//            ThreadInitialTestStub(request);
     }
         return status;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index d762e507f..42a56ae8a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -423,7 +423,67 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
+uint16_t BreakSensorCounter = 0;
+uint16_t BreakSensorLatchCounter = 0;
+char TMessage[150];
 
+uint32_t checkBreakSensor(uint32_t index)
+{
+    if (BreakSensorenabled == true)
+    {
+        if (JobCounter > eOneSecond)
+        {
+            if (ReadBreakSensor()==ERROR)
+            {
+                BreakSensorCounter++;
+                BreakSensorLatchCounter++;
+                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
+                {
+                    //consider applying the debouce parameters later
+                    usnprintf(TMessage, 60, "ReadBreakSensor Error");
+                    //BreakSensordebouncetimemilli
+                    JobEndReason = JOB_THREAD_BREAK;
+                    ThreadControlActive = false;
+                    SendJobProgress(0.0,0,false, TMessage);
+                    SendSegmentFail();
+                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
+                    //EndState(CurrentJob,"ReadBreakSensor Error" );
+                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                    return ERROR;
+                } //passed limit
+            }//ReadBreakSensor()==ERROR
+            else //reset counter - we are looking for consequent calls
+            {
+                if (BreakSensorCounter)
+                {
+                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                }
+                BreakSensorCounter = 0;
+            }
+            if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
+            {
+                BreakSensorCounter++;
+                BreakSensorLatchCounter++;
+                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
+                {
+                    //consider applying the debouce parameters later
+                    usnprintf(TMessage, 60, "thread speed too low");
+                    JobEndReason = JOB_THREAD_BREAK;
+                    ThreadControlActive = false;
+                    SendJobProgress(0.0,0,false, TMessage);
+                    SendSegmentFail();
+                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
+                    //EndState(CurrentJob,"ReadBreakSensor Error" );
+                    ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
+                    return ERROR;
+                }
+            }
+
+        }
+    }
+    return OK;
+
+}
 int controlIndex = 0;
 bool keepdata = true;
 int32_t KeepReadValue = 0;
@@ -477,10 +537,7 @@ void testDancersControl()
 }
 #endif
 int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
-char TMessage[150];
 //char time[150];
-uint16_t BreakSensorCounter = 0;
-uint16_t BreakSensorLatchCounter = 0;
 bool FirstCalcInJob = true;
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
@@ -510,6 +567,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     index = IfIndex&0xFF;
 
+    if (index == POOLER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec
+    {
+        if (checkBreakSensor(index) == ERROR)
+            return OK;
+    }
     if (MotorTiming[index]>1)
     {
         MotorTimer[index]++;
@@ -571,61 +633,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;
 
-        if (BreakSensorenabled == true)
-        {
-            if (index == POOLER_MOTOR)
-            {
-                if (JobCounter > eOneSecond)
-                {
-                    if (ReadBreakSensor()==ERROR)
-                    {
-                        BreakSensorCounter++;
-                        BreakSensorLatchCounter++;
-                        if (BreakSensorCounter>=BreakSensordebouncetimemilli)
-                        {
-                            //consider applying the debouce parameters later
-                            usnprintf(TMessage, 60, "ReadBreakSensor Error");
-                            //BreakSensordebouncetimemilli
-                            JobEndReason = JOB_THREAD_BREAK;
-                            ThreadControlActive = false;
-                            SendJobProgress(0.0,0,false, TMessage);
-                            SendSegmentFail();
-                            //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
-                            //EndState(CurrentJob,"ReadBreakSensor Error" );
-                            ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-                            return OK;
-                        } //passed limit
-                    }//ReadBreakSensor()==ERROR
-                    else //reset counter - we are looking for consequent calls
-                    {
-                        if (BreakSensorCounter)
-                        {
-                            ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-                        }
-                        BreakSensorCounter = 0;
-                    }
-                    if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
-                    {
-                        BreakSensorCounter++;
-                        BreakSensorLatchCounter++;
-                        if (BreakSensorCounter>=BreakSensordebouncetimemilli)
-                        {
-	                        //consider applying the debouce parameters later
-	                        usnprintf(TMessage, 60, "thread speed too low");
-	                        JobEndReason = JOB_THREAD_BREAK;
-	                        ThreadControlActive = false;
-	                        SendJobProgress(0.0,0,false, TMessage);
-	                        SendSegmentFail();
-	                        //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
-	                        //EndState(CurrentJob,"ReadBreakSensor Error" );
-                            ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
-	                        return OK;
-						}
-                    }
-
-                }
-            }
-        }
 
         //Stop Execution if the dancer moves too much
         if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
@@ -783,15 +790,6 @@ double ThreadGetMotorCalculatedError(int DancerId)
 }
 
 //********************************************************************************************************************
-uint32_t ThreadInitialTestStub(HardwareMotor * request)
-{
-
-
-    //MotorsConfigMessage(request);
-     ThreadPrepareState(request);
-     ThreadPreSegmentState(request,0);
-    return OK;
-}
 uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
 {
     if (ThreadParams == NULL)
@@ -799,21 +797,21 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
         return OK;
     }
     if(ThreadParams->feederp)
-        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederp;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp;
     if(ThreadParams->feederi)
         MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi;
     if(ThreadParams->feederd)
         MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd;
 
     if(ThreadParams->pullerp)
-        MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerp;
+        MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp;
     if(ThreadParams->pulleri)
         MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri;
     if(ThreadParams->pullerd)
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd;
 
     if(ThreadParams->winderp)
-        MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderp;
+        MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp;
     if(ThreadParams->winderi)
         MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi;
     if(ThreadParams->winderd)
@@ -951,7 +949,8 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     return status;
 }
 //********************************************************************************************************************
- uint32_t ThreadPrepareState(void *JobDetails)
+bool SkipOpenLids = true;
+uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i,i, HW_Motor_Id, Pid_Id;
     JobTicket* JobTicket = JobDetails;
@@ -1016,22 +1015,30 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
         //return ERROR;
     }*/
 
-    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false))
+//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false))
+    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false))
     {
         if(Head_Type != HEAD_TYPE_STAPLE_SPUN)
         {
             ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0);
-            JobEndReason = JOB_LIDS_OPEN;
-            PrepareReady(Module_Thread,ModuleFail);
-            return ERROR;
+            if (SkipOpenLids == false)
+            {
+                JobEndReason = JOB_LIDS_OPEN;
+                PrepareReady(Module_Thread,ModuleFail);
+                return ERROR;
+            }
         }
     }
-    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false))
+//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false))
+    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false))
     {
         ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0);
-        JobEndReason = JOB_LIDS_OPEN;
-        PrepareReady(Module_Thread,ModuleFail);
-        return ERROR;
+        if (SkipOpenLids == false)
+        {
+            JobEndReason = JOB_LIDS_OPEN;
+            PrepareReady(Module_Thread,ModuleFail);
+            return ERROR;
+        }
     }
 
     //start thread control for all motors
@@ -1057,6 +1064,8 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             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!!!
+
             temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
             MotorTiming[Motor_i] = (int)temp_dt;
             if (MotorTiming[Motor_i])
@@ -1068,7 +1077,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             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] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0);  //reset the samples value for control beginning
+                    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;
@@ -1148,7 +1157,6 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             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;
     }
-    HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
 #ifdef TEST_PID_THREAD
     testDancersControl();
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
index 8b1378917..2fdd3a479 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
@@ -1 +1,42 @@
+#ifndef WASTE_MAINT_H
+#define WASTE_MAINT_H
+
+typedef enum {
+    CartridgeStateOUT,
+    CartridgeStateIN,
+    CartridgeStateSELECTED,
+    CartridgeStateACTIVE,
+    CartridgeStateFULL
+}CartridgeStateEnum;
+
+typedef enum {
+    WasteCartridge_upper,
+    WasteCartridge_middle,
+    WasteCartridge_lower,
+    MaxWasteCartridges
+}WasteCartridgeEnum;
+
+typedef enum{
+    WasteTankStateIdle,
+    WasteTankStateFull,
+    WasteTankStateEmptying,
+    WasteTankStatePaused
+}WasteTankStateEnum;
+
+extern bool DoorState;
+/////////////////////////////// Cartridges  //////////////////////////////////////
+void cartCART_INSERTED (WasteCartridgeEnum); //{called from polling function. validate, set state to IN)
+void cartCART_EXTRACTED(WasteCartridgeEnum); //{called from polling function. set state to out, if was ACTIVE – notify waste module)
+WasteCartridgeEnum cartSELECT_CART(void); //{ if there is no SELECTED cartridge select the first cartridge in IN state. If there is none, return -1}
+void cartFILLING_START(void); //{set state of SELECTED to ACTIVE}
+void cartFILLING_END(void); //{set state of ACTIVE to FULL}
+CartridgeStateEnum cartGetState (WasteCartridgeEnum);
+bool cartGetPresence (WasteCartridgeEnum CartId);
+bool cartCart_door();//Polled by polling function. Notify waste. Poll each 1 second. During active filling poll each 100msec
+
+void Waste_Init();
+void Waste_StateMachine(void);
+void Waste_StateMachine_OneSecond_Call(void);
+
+#endif
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index e69de29bb..099fc0c2e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -0,0 +1,363 @@
+#include <Container.h>
+#include <DataDef.h>
+#include "include.h"
+#include "Modules/Control/control.h" //  use for FPGA IO
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"//#include "FPGA_GPIO.h"  //  use for FPGA IO
+#include "drivers/Valves/Valve.h"
+#include "Common/report/report.h"
+#include "Modules/Waste/Waste_ex.h"
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+#include "Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+#include "Drivers/I2C_Communication/RFID_NFC/NFC.h"
+//#include <Utilities/RfidTagHandling.h>
+
+#include  <PMR/Diagnostics/EventType.pb-c.h>
+#include "Modules/AlarmHandling/AlarmHandling.h"
+#include "StateMachines/Initialization/PowerOffSequence.h"
+#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
+#include "CartridgeValidationRequest.pb-c.h"
+#include "CartridgeValidationResponse.pb-c.h"
+#include "Modules/General/buttons.h"
+#include "Modules/IFS/ifs.h"
+#include <Drivers/I2C_Communication/I2C_Task.h>
+#include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.h>
+#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
+
+bool DoorState = OPEN;
+uint32_t timeout_counter = 0;
+int wasteLevel = 0;
+WasteCartridgeEnum SelectedCard = MaxWasteCartridges;
+CartridgeStateEnum CartState[MaxWasteCartridges];
+uint32_t Cartridge_Cover_Control;
+button *CartLed[MaxWasteCartridges] = {0, &cart2, &cart3};
+WasteTankStateEnum WasteTankState = WasteTankStateIdle;
+
+#define WASTE_CARTRIDGE_SIZE 1500
+#define WASTE_LEVEL_OVERFLOW 2700
+#define WASTE_LEVEL_FULL 2300
+#define WASTE_LEVEL_EMPTY 900
+#define WASTE_EMPTING_TIMEOUT 1200
+double wasteLevelOverflow = WASTE_LEVEL_OVERFLOW;
+double wasteLevelFull = WASTE_LEVEL_FULL;
+double wasteLevelEmpty = WASTE_LEVEL_EMPTY;
+double wasteLevelOverFlow = WASTE_LEVEL_OVERFLOW;
+
+//#define WHS_DEBUG
+
+/////////////////////////////// Cartridges  //////////////////////////////////////
+void Waste_StateMachine(void);
+void cartCART_INSERTED (WasteCartridgeEnum CartId)
+{
+    assert (CartId<MaxWasteCartridges);
+    if (CartState[CartId] != CartridgeStateOUT)
+        Report("Cartridges Cart inserted wrong state", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
+    CartState[CartId] = CartridgeStateIN;
+    CartLed[CartId]->color = colorON;
+
+    Report("Cartridges Cart inserted", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
+}
+void cartCART_EXTRACTED(WasteCartridgeEnum CartId) //{called from polling function. set state to out, if was ACTIVE – notify waste module)
+{
+    assert (CartId<MaxWasteCartridges);
+    if (CartState[CartId] == CartridgeStateOUT)
+        Report("Cartridges Cart extracted wrong state", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
+
+    Report("Cartridges Cart extracted", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
+    CartState[CartId] = CartridgeStateOUT;
+    CartLed[CartId]->color = colorOFF;
+
+}
+WasteCartridgeEnum cartSELECT_CART(void) //{ if there is no SELECTED cartridge select the first cartridge in IN state. If there is none, return -1}
+{
+    Report("Cartridges select cart", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0);
+    if ((CartState[WasteCartridge_middle] == CartridgeStateSELECTED)||(CartState[WasteCartridge_middle] == CartridgeStateACTIVE))
+        return WasteCartridge_middle;
+    if ((CartState[WasteCartridge_lower]  == CartridgeStateSELECTED)||(CartState[WasteCartridge_lower] == CartridgeStateACTIVE))
+        return WasteCartridge_lower;
+    //no selected/active cartridge yet
+    if (CartState[WasteCartridge_middle] == CartridgeStateIN) {
+        CartState[WasteCartridge_middle] = CartridgeStateSELECTED;
+        return WasteCartridge_middle;
+    }
+    if (CartState[WasteCartridge_lower] == CartridgeStateIN) {
+        CartState[WasteCartridge_lower] = CartridgeStateSELECTED;
+        return WasteCartridge_lower;
+    }
+    return MaxWasteCartridges;
+}
+
+void cartFILLING_START(void) //{set state of SELECTED to ACTIVE}
+{
+    if (CartState[WasteCartridge_middle] == CartridgeStateSELECTED) {
+        Report("Cartridges middle Cart active", __FILE__, __LINE__, WasteCartridge_middle, RpMessage, CartState[WasteCartridge_middle], 0);
+        CartState[WasteCartridge_middle] = CartridgeStateACTIVE;
+        CartLed[WasteCartridge_middle]->color = BLINK;
+    } else if (CartState[WasteCartridge_lower] == CartridgeStateSELECTED) {
+        Report("Cartridges lower Cart active", __FILE__, __LINE__, WasteCartridge_lower, RpMessage, CartState[WasteCartridge_lower], 0);
+        CartState[WasteCartridge_lower] = CartridgeStateACTIVE;
+        CartLed[WasteCartridge_lower]->color = BLINK;
+    } else {
+        Report("Cartridges start with no selected cartridge", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0);
+    }
+}
+
+void cartFILLING_END(void)
+{
+    //{set state of ACTIVE to FULL}
+    if (CartState[WasteCartridge_middle] == CartridgeStateACTIVE) {
+        CartState[WasteCartridge_middle] = CartridgeStateFULL;
+        CartLed[WasteCartridge_middle]->color = colorOFF;
+    } else if (CartState[WasteCartridge_lower] == CartridgeStateACTIVE) {
+        CartState[WasteCartridge_lower] = CartridgeStateFULL;
+        CartLed[WasteCartridge_lower]->color = colorOFF;
+    } else {
+        Report("Cartridges end with no active cartridge", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0);
+    }
+}
+
+CartridgeStateEnum cartGetState(WasteCartridgeEnum CartId)
+{
+    assert (CartId<MaxWasteCartridges);
+    return CartState[CartId];
+}
+
+bool cartGetPresence (WasteCartridgeEnum CartId)
+{
+    assert (CartId<MaxWasteCartridges);
+    if (CartState[CartId] == CartridgeStateOUT)
+        return false;
+    else
+        return true;
+}
+
+bool NoCartAvailable()
+{
+    uint32_t CartId;
+
+    for(CartId = CART_2; CartId <= CART_3; CartId++) {
+        if (Is_Cartridge_Present(CartId))
+            return false;
+    }
+    return true;
+}
+
+WasteCartridgeEnum cartGetActiveCart()
+{
+    uint32_t CartId;
+
+    for (CartId = 0; CartId < MaxWasteCartridges; CartId++) {
+        if (CartState[CartId] == CartridgeStateACTIVE)
+            return CartId;
+    }
+    return MaxWasteCartridges;
+}
+
+WasteCartridgeEnum cartNotAllFull()
+{
+    uint32_t CartId;
+
+    for (CartId = WasteCartridge_middle; CartId < MaxWasteCartridges; CartId++) {
+        if (CartState[CartId] != CartridgeStateFULL)
+            return true;
+    }
+    return false;
+}
+
+void Waste_Init()
+{
+    CartState[WasteCartridge_middle] = CartridgeStateOUT;
+    CartState[WasteCartridge_lower] = CartridgeStateOUT;
+    DoorState = OPEN;
+}
+
+bool cartCart_door()//Polled by polling function. Notify waste. Poll each 1 second. During active filling poll each 100msec
+{
+    if(Get_COVER_1_State(CartridgesDoor))//to check the polarity
+    {
+        DoorState = OPEN;
+    }
+    else
+    {
+        DoorState = CLOSE;
+    }
+#ifdef WHS_DEBUG
+    DoorState = CLOSE;
+#endif
+    return DoorState;
+}
+
+//Waste_DoorOpenDuringEmptying
+uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
+{
+    cartCart_door();
+    if ((DoorState == OPEN) && (WasteTankState == WasteTankStateEmptying)) {
+        SetWastePump(CLOSE);
+        WasteTankState = WasteTankStatePaused;
+        Report("move to WasteTankStatePaused", __FILE__, __LINE__, DoorState, RpMessage, WasteTankState, 0);
+    }
+    if ((DoorState == CLOSE) && (WasteTankState == WasteTankStatePaused)) {
+        if (cartGetActiveCart() != MaxWasteCartridges) {
+            SetWastePump(OPEN);
+            WasteTankState = WasteTankStateEmptying;
+            Report("return to WasteTankStateEmptying", __FILE__, __LINE__, DoorState, RpMessage, WasteTankState, 0);
+        } else {
+            Report("WasteTankStateEmptying no active cart", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+            WasteTankState = WasteTankStateIdle;
+            Report("move to WasteTankStateIdle", __FILE__, __LINE__, DoorState, RpMessage, WasteTankState, 0);
+        }
+    }
+    return OK;
+#warning add alarm on door open
+}
+
+void Waste_StateMachine_OneSecond_Call(void)
+{
+    static int init = 1;
+    static WasteTankStateEnum prev_state = 0;
+    static int prev_wasteLevel = 0;
+
+    //call cartridge state
+    if ((Is_Cartridge_Present(CART_2) != cartGetPresence(WasteCartridge_middle)) || (init)) {
+        if (Is_Cartridge_Present(CART_2)) {
+            cartCART_INSERTED(WasteCartridge_middle);
+        } else {
+            cartCART_EXTRACTED(WasteCartridge_middle);
+        }
+    }
+    if ((Is_Cartridge_Present(CART_3) != cartGetPresence(WasteCartridge_lower)) || (init)) {
+        if (Is_Cartridge_Present(CART_3)) {
+            cartCART_INSERTED(WasteCartridge_lower);
+        } else {
+            cartCART_EXTRACTED(WasteCartridge_lower);
+        }
+    }
+
+    //call door state
+    cartCart_door();
+
+    //call waste tank level
+#ifndef WHS_DEBUG
+    wasteLevel = GetWHSWasteTankLevelMiliLiter();
+#endif
+
+    if ((WasteTankState != prev_state) || (abs(wasteLevel - prev_wasteLevel) > 50)) {
+        Report("Waste_StateMachine_OneSecond_Call", __FILE__, WasteTankState, SelectedCard, RpMessage, wasteLevel, 0);
+        prev_state = WasteTankState;
+        prev_wasteLevel = wasteLevel;
+    }
+
+    //call state machine
+    Waste_StateMachine();
+
+    if(init)
+        init = 0;
+
+#ifdef WHS_DEBUG
+            wasteLevel += 50;
+#endif
+}
+
+void Waste_StateMachine(void)
+{
+    uint32_t activeCart;
+    switch (WasteTankState)
+    {
+        case WasteTankStateIdle:
+            if (wasteLevel > wasteLevelOverFlow) {
+                AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  true);
+            } else {
+                AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  false);
+            }
+            if (cartNotAllFull() == true) {
+                AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  false);
+            }
+            if (wasteLevel > wasteLevelFull) {
+                //check if power down in process
+                if ( PowerOffInProcessGetState() ) {
+                    Report("Power-down in process", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                    break;
+                }
+
+                //check door
+                if (DoorState == OPEN) {
+                    Report("Close cartridge cover", __FILE__, __LINE__, DoorState, RpMessage, 0, 0);
+                    break;
+                }
+
+                // select cartridge
+                SelectedCard = cartSELECT_CART();
+                if (SelectedCard == MaxWasteCartridges) {
+                    Report("No Available cart", __FILE__, __LINE__, SelectedCard, RpMessage, 0, 0);
+                    AlarmHandlingSetAlarm( EVENT_TYPE__NO_WASTE_CARTRIDGE_AVAILABLE,  true);
+                    break;
+                } else {
+                    AlarmHandlingSetAlarm( EVENT_TYPE__NO_WASTE_CARTRIDGE_AVAILABLE,  false);
+                }
+
+                //check RFID? (TBD)
+
+                //move to next state
+                WasteTankState = WasteTankStateFull;
+                Report("move to WasteTankStateFull", __FILE__, __LINE__, DoorState, RpMessage, WasteTankState, 0);
+            }
+            break;
+        case WasteTankStateFull:
+            //set cart Active
+            cartFILLING_START();
+
+            // set valve selection
+            activeCart = cartGetActiveCart();
+            Valve_Set(VALVE_WASTE_TANK, activeCart);
+            Report("WasteTankStateEmptying set valve", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
+
+            // RFID change status - TBD
+
+            // set count down
+            timeout_counter = 1;
+
+            // set emptying limit
+            wasteLevelEmpty = wasteLevel - WASTE_CARTRIDGE_SIZE;
+
+            // start a timer to observe door opening
+            Cartridge_Cover_Control = AddControlCallback("Cartridge Cover Door", Waste_DoorOpenDuringEmptying, 100,  cartCart_door, 0, 0, 0);
+            Report("add control Cartridge Cover Door", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+
+            SetWastePump(OPEN);
+            Report("WasteTankStateEmptying set Pump Open", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
+
+            //start  emptying
+            WasteTankState = WasteTankStateEmptying;
+            break;
+        case WasteTankStateEmptying:
+            // if time out or emptying done - idle
+            if (timeout_counter++ > WASTE_EMPTING_TIMEOUT) {
+                SetWastePump(CLOSE);
+                cartFILLING_END();
+                // RFID change status - TBD
+                WasteTankState = WasteTankStateIdle;
+                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) {
+                SetWastePump(CLOSE);
+                cartFILLING_END();
+                // 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);
+            }
+#ifdef WHS_DEBUG
+            wasteLevel -= 100;
+            timeout_counter += 10;
+#endif
+            break;
+        case WasteTankStatePaused:
+            Report("WasteTankStatePaused...", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+            break;
+        default:
+            Report("wrong state", __FILE__, WasteTankState, 0, RpMessage, SelectedCard, 0);
+            break;
+    }
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.h