fixes for thread loading, VOC and waste handling

5 files changed, 54 insertions, 31 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index dfd01cef5..67a532e78 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -773,10 +773,6 @@ uint32_t MillisecLowLoop(uint32_t tick)
         midtankDisplay = 1-midtankDisplay;
         /*if (WHS_Type == WHS_TYPE_UNKNOWN)
             Gas_PPM_Info = Calculate_Gas_Power_Consumption();*/
-        if (WHS_Type == WHS_TYPE_NEW)
-        {
-            waste_seq_step1();// include 1Sec delay           <- to open !!!!
-        }
 //            ReportWithPackageFilter(ThreadFilter,"waste tank calculate level",__FILE__,__LINE__,(int)(GetWHSWasteTankLevelMiliLiter()*1000),RpWarning,(int) msec_millisecondCounter,0);
         //Trigger_WHS_MAX11614_Read_allADC();
 #ifdef CONTROL_DEBUG
@@ -793,6 +789,10 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpCounter = 0;
         }
         MidTankReading();
+        if (WHS_Type == WHS_TYPE_NEW)
+        {
+            waste_seq_step1();// include 1Sec delay           <- to open !!!!
+        }
     }
     //ROM_IntMasterEnable();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 6782a8ecb..49058c359 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -150,7 +150,7 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
                         direction = DRIER_LID_OPEN;
                     }
                     Report("MotorHomingRequestFunc Dryer lid open",__FILE__,MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType,DRIER_LID_OPEN,RpMessage,GPI_LS_DRYER_LID_OPEN,0);
-                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_OPEN, MotorHomingRequestCallback,4000);
+                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_OPEN, MotorHomingRequestCallback,10000);
                     status = OK;
                 }
                 else
@@ -198,7 +198,7 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
                         direction = DRIER_LID_CLOSE;
                     }
                     Report("MotorHomingRequestFunc Dryer lid close",__FILE__,MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType,DRIER_LID_CLOSE,RpMessage,GPI_LS_DRYER_LID_CLOSED,0);
-                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_CLOSED, MotorHomingRequestCallback,4000);
+                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_CLOSED, MotorHomingRequestCallback,10000);
                     status = OK;
                 }
                 else
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
index 6c7bb087f..77c177fce 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
@@ -60,7 +60,7 @@ void IFS_Init(void)
  * @return bool ret OK = 0 , CartridgeFinished = 1
  */
 #define CARTRIDGE_EMPTY_QUANTITY_LIMIT 10
-#define CARTRIDGE_EMPTY_CALCULATION_TIME 6
+#define CARTRIDGE_EMPTY_CALCULATION_TIME 12
 bool DetectIfCartridgeFinished(int int_MidTank_Pressure_1000)
 {
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 19392c754..2ad58ea0a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -108,7 +108,7 @@
     float numberOfSteps = 0;
     float numberOfCycles = 0;
     double DrierPrevLocation = 0;
-    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT};
+    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT};
     int Screw_Dir = false;
     bool InitCalled = false;
     uint32_t LoadArmRounds;
@@ -326,10 +326,19 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
         //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
+        int direction;
+        if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep
+        {
+            direction = DRIER_LID_CLOSE;
+        }
+        else
+        {
+            direction = DRIER_LID_OPEN;
+        }
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
         Report("Thread_Load_Lift_actuators",__FILE__,__LINE__,LOW,RpMessage,false,0);
-        Trigger_Head_Actuators_Control(ACTOT, LOW,false);
-        Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+        Trigger_Head_Actuators_Control(ACTOT, LOW,true);
+        Trigger_Head_Actuators_Control(ACTIN, LOW,true);
         return OK;
     }
     uint32_t Thread_Load_Lift_Dancers(void)
@@ -450,10 +459,19 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
+        int direction;
+        if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep
+        {
+            direction = DRIER_LID_OPEN;
+        }
+        else
+        {
+            direction = DRIER_LID_CLOSE;
+        }
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
         Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
-        Trigger_Head_Actuators_Control(ACTOT, LOW,true);
-        Trigger_Head_Actuators_Control(ACTIN, LOW,true);
+        Trigger_Head_Actuators_Control(ACTOT, LOW,false);
+        Trigger_Head_Actuators_Control(ACTIN, LOW,false);
         return OK;
     }
     uint32_t Thread_Load_Resume_Heating(void)
@@ -575,7 +593,7 @@
             }
             else
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -652,7 +670,8 @@
         MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
         MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize);
 
-        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
+        //6 seconds per round
 
         LoadingControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0);
@@ -670,7 +689,7 @@
         CallbackCounter++;
         Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                               numberOfSteps-400,    Thread_Load_Dryer_Loading_Callback, 10000);
+                               numberOfSteps-800,    Thread_Load_Dryer_Loading_Callback, 10000);
         return OK;
     }
     uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
@@ -767,11 +786,11 @@
             if (numberOfCycles<LoadArmRounds)
             {
                 MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                                       numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 30000);
+                                       numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 10000);
             }
             else //done enough cycles, go to the center point
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,10000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -890,15 +909,19 @@
         else
         {
             MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);
+            if (LoadArmRounds == 0) //prev trial stopped
+            {
+                LoadArmRounds = (int)dryerbufferlength;
+            }
         }
         MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
         DrierPrevLocation = temp;
         Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0);
-        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
 //        status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
 //                                        MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,0,1000);
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
-                               numberOfSteps-400,    Thread_Load_Dryer_UnLoading_Callback, 30000);
+                               numberOfSteps-800,    Thread_Load_Dryer_UnLoading_Callback, 10000);
 
         Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
 
@@ -1204,15 +1227,15 @@ bool ThreadLoaded(void)
 
     ReadValue = Control_Read_Dancer_Position(FEEDER_DANCER,0);
     FeederValue = ReadValue - DancersCfg[FEEDER_DANCER].zeropoint;
-    Report("Feeder out",__FILE__,FeederValue,ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0);
+    Report("Feeder out",__FILE__,abs(FeederValue),ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0);
     ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0);
     WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint;
-    Report("Winder out",__FILE__,WinderValue,ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0);
+    Report("Winder out",__FILE__,abs(WinderValue),ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0);
     ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0);
     PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint;
         //pooler dancer is right sided: data is opposite
     PoolerValue = (-1*PoolerValue);
-    Report("Puller out",__FILE__,PoolerValue,ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0);
+    Report("Puller out",__FILE__,abs(PoolerValue),ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0);
 
     if (((PoolerValue)<(-1200))&&((WinderValue)<(-1200))&&((FeederValue)<(-1200)))
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
index f011e8a1f..906a0d458 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
@@ -458,10 +458,10 @@ U8 CartridgeWasteFilling(bool status)
 }
 
 /* ------read waste tank sensors -----------  */
-#define WASTE_CARTRIDGE_SIZE 1.5
-#define WASTE_LEVEL_OVERFLOW 4.0
-#define WASTE_LEVEL_FULL 3.5
-#define WASTE_LEVEL_EMPTY 2.10
+#define WASTE_CARTRIDGE_SIZE 1500
+#define WASTE_LEVEL_OVERFLOW 4000
+#define WASTE_LEVEL_FULL 3500
+#define WASTE_LEVEL_EMPTY 2100
 double WasteLevelOverflow = WASTE_LEVEL_OVERFLOW;
 double WasteLevelFull = WASTE_LEVEL_FULL;
 double WasteLevelEmpty = WASTE_LEVEL_EMPTY;
@@ -493,7 +493,7 @@ bool RdWasteTankEmptySensor()
 bool RdWasteTankFullSensor()
 {
     double WasteLevel = 0.0;
-    bool ret = notOK;
+    bool ret = SENSORnotFULL;
     //WHS_Read_GPI_Registers();
     //WHS_info.WHS_sensors.waste_tank_full_sensor =  WHS_GPI_WCONTAINER_FULL();
     //ret = WHS_info.WHS_sensors.waste_tank_full_sensor;
@@ -504,8 +504,8 @@ bool RdWasteTankFullSensor()
     else //new WHS
     {
         WasteLevel = GetWHSWasteTankLevelMiliLiter();
-        if (WasteLevel < WasteLevelFull)
-            ret = OK;
+        if (WasteLevel >= WasteLevelFull)
+            ret = SENSORFULL;
     }
     return ret;
 }