IDS Distance to spool - stop dispensers

sensor break in comment

3 files changed, 44 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index c96078771..a3134c303 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -14,6 +14,7 @@
 uint32_t IDSPrepareState(void *JobDetails);
 uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId);
 uint32_t IDSSegmentState(void *JobDetails, int SegmentId);
+uint32_t IDSDistanceToSpoolState(void);
 uint32_t IDSEndState(void *JobDetails);
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index ed9e37fcc..bfdb7b5d7 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -47,6 +47,7 @@ uint32_t    ControlIdtoDispenserId [MAX_SYSTEM_DISPENSERS] = {0xFF,0xFF,0xFF,0xF
 int OriginalDispenserSpd_2PPS[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0};
 bool         DispenserPreSegmentReady[MAX_SYSTEM_DISPENSERS] = {true,true,true,true,true,true,true,true};
 bool         DispenserSegmentReady[MAX_SYSTEM_DISPENSERS] = {true,true,true,true,true,true,true,true};
+bool         DispenserDistanceToSpoolReady[MAX_SYSTEM_DISPENSERS] = {true,true,true,true,true,true,true,true};
 int JobBrushStopId = 0;
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
@@ -165,7 +166,8 @@ void DispenserPrepareReady(void)
      PreSegmentReady(Module_IDS,ModuleDone);
   return OK; // all configured heaters are ready
  }
- //********************************************************************************************************************
+
+//********************************************************************************************************************
 uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
 {
 //set the speed only before the first segment, speed is constant accros job
@@ -279,6 +281,41 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
 
     return OK;
 }
+ //********************************************************************************************************************
+  uint32_t IDS_Valve_DistanceToSpoolReady(uint32_t deviceID, uint32_t ReadValue)
+  {
+      int i;
+      DispenserDistanceToSpoolReady[deviceID] = true;
+      for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
+      {
+          if (DispenserDistanceToSpoolReady[i] == false)
+          {
+              return OK; //not all configured heaters are ready
+          }
+      }
+      DistanceToSpoolReady(Module_IDS,ModuleDone);
+   return OK; // all configured heaters are ready
+  }
+ //********************************************************************************************************************
+ uint32_t IDSDistanceToSpoolState(void)
+ {
+     int Dispenser_i,DispenserId;
+     TimerMotors_t HW_Motor_Id;
+
+         for (Dispenser_i = 0;Dispenser_i < MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+         {
+             HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
+             if (MotorsCfg[HW_Motor_Id].hardwaremotortype != DispenserIdToMotorId[Dispenser_i])//unconfigured dispenser
+                 continue;
+             DispenserDistanceToSpoolReady[Dispenser_i] = false;
+             Control3WayValvesWithCallback ((Valves_t)Dispenser_i, MidTank_Dispenser, IDS_Valve_DistanceToSpoolReady); //direction: MidTank_Dispenser or Dispenser_Mixer
+
+             MotorStop(HW_Motor_Id,Hard_Hiz);
+
+         }
+
+     return OK;
+ }
 
 //********************************************************************************************************************
  uint32_t IDSEndState(void *JobDetails)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6a0bac394..6822493cd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -352,7 +352,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
-        //Stop Execution if the dancer moves too much
+        /*
         if (index == POOLER_MOTOR)
         {
             if (JobCounter > eOneSecond)
@@ -365,10 +365,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 }
             }
         }
+        */
+        //Stop Execution if the dancer moves too much
         if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
         {
-             usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
-
+            usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
+            JobAbortedByUser = true;
             EndState(CurrentJob,Message );
         }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];