some small changes, mostly in drier loading

9 files changed, 94 insertions, 28 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 26fba0194..14275533e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -597,6 +597,9 @@ uint32_t ControlLowLoop(uint32_t tick)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
+            if (ControlArray[ControlLowDevice_i].StartTick == tick)
+                continue;
+
             if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
             {
                 ControlBacklog[backlogindex]=ControlLowDevice_i;
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
index 4d3344ce7..7bda97734 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
@@ -24,6 +24,7 @@
 #include  <PMR/Diagnostics/SetBlowerStateResponse.pb-c.h>
 #include  <PMR/Hardware/HardwareBlowerType.pb-c.h>
 
+#include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "drivers/I2C_Communication/DAC/blower.h"
 #include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
@@ -214,6 +215,11 @@ uint32_t SetDigitalOutRequestRequestFunc(MessageContainer* requestContainer)
                 Trigger_SetWHSValveWatseCartridge(request->value);
             break;
         case INTERFACE_IOS__GPO_WHS_WTANKPUMP2:
+            if ((IFS_Availability[1] == IFS_RECOGNIZED)&&(IFS_Availability[2] == IFS_RECOGNIZED)) //ifs installed -check cartridges
+            {
+                if (NoCartAvailable())
+                    return ERROR;
+            }
             SetWastePump( request->value);
             break;
         case INTERFACE_IOS__GPO_SPARE_SSR12_CTRL:
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index a439a8715..93bf916b2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -480,18 +480,20 @@ uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadVal
     uint32_t temp = Read_Dryer_ENC_Position();
     Report("Diagnostics_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0);
     //Report("Diagnostics_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0);
-    if (ReadValue == NOTBUSY) // OK - take another round
+    if ((abs (temp -D_DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY)) // OK - take another round
+//    if (ReadValue == NOTBUSY) // OK - take another round
     {
         D_DrierPrevLocation = temp;
         Report("Diagnostics_Dryer_UnLoading cycles",__FILE__,D_numberOfCycles,LoadArmRounds,RpMessage,0,0);
         if (D_numberOfCycles<LoadArmRounds)
         {
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles);
             MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                    D_numberOfSteps,    Diagnostics_Dryer_UnLoading_Callback, 10000);
         }
         else //done enough cycles, go to the center point
         {
-            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,15000);
+            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,15000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
             MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles);
             Report("Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
         }
@@ -556,6 +558,7 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue
 
     Report("Diagnostics_Dryer_Loading_Callback",__FILE__,(int)D_numberOfCycles,(int)D_DrierPrevLocation,RpMessage,ReadValue,0);
 
+    MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles);
     if (ReadValue == NOTBUSY)
     {
         //Report("Diagnostics_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
@@ -567,15 +570,13 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue
         }
         else
         {
-            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000);
-            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles);
+            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
             Report("Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
         }
     }
     else
     {
         Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
-        MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles);
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
     }
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 73da7924b..a9899afd7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -684,8 +684,7 @@ void PrepareACHeater(int HeaterId,uint32_t Frequency, uint32_t SetTemperatue)
             }
             DeActivateHeater(HeaterId);
             HeaterCmd[HeaterId].targettemperatue = 0;
-#warning PT100 error is misidentified as overheat in alarms
-            AlarmHandlingSetAlarm(EVENT_TYPE__POWER_UP_BIT_FAILURE, true);
+            AlarmHandlingSetAlarm(EVENT_TYPE__TEMPERATURE_MEASUREMENT_ERROR, true);
 
             return;
         }
@@ -767,7 +766,7 @@ int PrepareDCHeater(int HeaterId, uint32_t Frequency, uint32_t SetTemperatue)
             ControlIdtoHeaterId [HeaterId] = 0xFF;
         }
         DeActivateHeater(HeaterId);
-        AlarmHandlingSetAlarm(EVENT_TYPE__POWER_UP_BIT_FAILURE, true);
+        AlarmHandlingSetAlarm(EVENT_TYPE__TEMPERATURE_MEASUREMENT_ERROR, true);
         HeaterCmd[HeaterId].targettemperatue = 0;
 
         return ERROR;
@@ -1154,6 +1153,17 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
             DeActivateHeater(HEATER_TYPE__DryerMainHeater);
             HeaterRecalculateSharedHeatersParams(HEATER_TYPE__DryerMainHeater,0);
             HeaterRecalculateSharedHeatersParams(HEATER_TYPE__DryerSecondaryHeater,0);
+            if (HeaterSpikeRead[index] == 0)
+            {
+                HeaterSpikeRead[index] = readValue;
+            }
+            else
+            {
+                ReportWithPackageFilter(HeatersFilter,"restoring read Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,HeaterSpikeRead[index], index);
+                HeaterPreviousRead[index] = HeaterSpikeRead[index];
+                HeaterSpikeRead[index] = 0;
+            }
+
         }
         return ERROR;
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 8682d648a..5f1dfe138 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -593,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)&&(NumOfActiveDispensers))
+     else if (endOfPrepareWCF == false)
      {
          if ((DispenserBuildTimeCounter == PRESSURE_READ_TIME_GAP)&&(FirstBrushStop))
          {
@@ -686,13 +686,6 @@ 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)
      {
@@ -1612,7 +1605,7 @@ uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId)
 //handle ending of presegment
     if (DispenserPreSegmentControlId != 0xFF)
     {
-        Report("IDS presegment not yet ended!!",__FILE__,__LINE__,(int)InterSegmentStepsCount,RpWarning,(int)lInterSegmentLength,0);
+        Report("IDS presegment not yet ended!!",__FILE__,DispenserPreSegmentControlId,(int)InterSegmentStepsCount,RpWarning,(int)lInterSegmentLength,0);
         //IDS_Cleaning_Stop_Cleaning_Solution (NULL);
         RemoveControlCallback(DispenserPreSegmentControlId, IDSPreSegmentStateCallbackRunner );
         DispenserPreSegmentControlId = 0xFF;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 6b6b85f2f..e14053ae3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -1125,7 +1125,9 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
     uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     free(responseContainer.data.data);
-    progress_request__free_unpacked(request,NULL);
+    if (request)
+        progress_request__free_unpacked(request,NULL);
+    //writeLine("Progress Completed!");
     SendChars((char*)container_buffer, container_size);
     //free(container_buffer);
     if (RestartNeeded == true)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index f5bc45d85..2c8db8b6c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -226,6 +226,19 @@
         }
         return OK;
     }
+    uint32_t Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+
+        Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
+
+        Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+        NumberOfDrierLoaderCycles=0;
+        //storeLoadArmParameters();
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+
+        return OK;
+    }
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
 
@@ -600,7 +613,7 @@
     {
         numberOfCycles++;
 
-        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0);
+        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,numberOfCycles,0);
 
         if ((fabs (TotalLoadedLen  -DrierPrevLocation)>50  )&&(ReadValue == NOTBUSY))
         {
@@ -618,7 +631,7 @@
             }
             else
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -803,6 +816,7 @@
     }
     uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
+        bool direction;
         numberOfCycles++;
         uint32_t temp = Read_Dryer_ENC_Position();
         Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
@@ -823,7 +837,7 @@
             }
             else //done enough cycles, go to the center point
             {
-                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000);
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
                 MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
@@ -831,7 +845,23 @@
         else  //timeout or no movement
         {
             Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
-            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
+            if (SecondTry == true)
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
+            else
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
+            if (abs(temp -DrierPrevLocation)<200)
+            {
+                if (temp<DrierPrevLocation)
+                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                else
+                    direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction);
+
+            }
+            else
+                MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
+
+
             TimeoutsCounter = 0;
             CallbackCounter = 0;
             /*if(PullerControlId != 0xFF)
@@ -853,7 +883,6 @@
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
 
             MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
-            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
             load.color = fastBILNK;
             usnprintf(LoadErrorMsg, 100, "Stage %s - Drier load arm timeout",LoadStagesStr[LoadStages]);
             Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
@@ -1210,15 +1239,15 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
 
     NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-    MotorControlConfig[index].m_mesuredParam = NormalizedError;
-    MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
-                                                                          &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
     if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
     {
-        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        NormalizedError = (-1*NormalizedError);
     }
+    MotorControlConfig[index].m_mesuredParam = NormalizedError;
+    MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
+                                                                          &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    //if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
index 308de0bd0..af4bd5e08 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
@@ -33,6 +33,7 @@ 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
+bool NoCartAvailable();
 
 void Waste_Init();
 void Waste_StateMachine(void);
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index 7d358cfc2..40e23e0b3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -499,3 +499,24 @@ void Waste_StateMachine(void)
             break;
     }
 }
+void Waste_Prepare(void)
+{
+    //check if the waste is ready to run - waste level not above overflow level and no cartridge in the slots
+    if (wasteLevel > wasteLevelOverflow)
+    {
+        Report("cannot start a job with waste tank overflow", __FILE__, __LINE__, wasteLevel, RpMessage, wasteLevelOverflow, 0);
+        PrepareReady(Module_Waste,ModuleFail);
+        return;
+    }
+    if ((WHS_IsEmptying())||(NoCartAvailable()))
+    {
+        PrepareReady(Module_Waste,ModuleDone);
+    }
+    else
+    {
+        Report("cannot start a job with cartridges in the IFS", __FILE__, __LINE__, wasteLevel, RpMessage, wasteLevelOverflow, 0);
+        PrepareReady(Module_Waste,ModuleFail);
+    }
+    return;
+
+}