diff --git a/MatterControlLib/PrinterCommunication/Io/PauseHandlingStream.cs b/MatterControlLib/PrinterCommunication/Io/PauseHandlingStream.cs index e5931dfe3..eec7a0c64 100644 --- a/MatterControlLib/PrinterCommunication/Io/PauseHandlingStream.cs +++ b/MatterControlLib/PrinterCommunication/Io/PauseHandlingStream.cs @@ -42,6 +42,15 @@ namespace MatterHackers.MatterControl.PrinterCommunication.Io { public class PauseHandlingStream : GCodeStreamProxy { + internal class PositionSensorData + { + public bool RecievingData { get; internal set; } + + public double LastSensorDistance { get; internal set; } + public double LastStepperDistance { get; internal set; } + public int ExtrusionDiscrepency { get; internal set; } + } + protected PrinterMove lastDestination = new PrinterMove(); private List commandQueue = new List(); private object locker = new object(); @@ -49,30 +58,74 @@ namespace MatterHackers.MatterControl.PrinterCommunication.Io private Stopwatch timeSinceLastEndstopRead = new Stopwatch(); bool readOutOfFilament = false; - private EventHandler unregisterEvents; + PositionSensorData positionSensorData = new PositionSensorData(); public PauseHandlingStream(PrinterConfig printer, GCodeStream internalStream) : base(printer, internalStream) { - printer.Connection.LineReceived += (s, line) => + // if we have a runout sensor, register to listen for lines to check it + if (printer.Settings.GetValue(SettingsKey.filament_runout_sensor)) { - if (line != null) + printer.Connection.LineReceived += (s, line) => { - if (line.Contains("ros_")) + if (line != null) { - if(line.Contains("TRIGGERED")) + if (line.Contains("ros_")) { - readOutOfFilament = true; + if (line.Contains("TRIGGERED")) + { + readOutOfFilament = true; + } + } + + if (line.Contains("pos_")) + { + double sensorDistance = 0; + double stepperDistance = 0; + if (GCodeFile.GetFirstNumberAfter("SENSOR:", line, ref sensorDistance)) + { + if (sensorDistance < -1 || sensorDistance > 1) + { + positionSensorData.RecievingData = true; + } + + if (positionSensorData.RecievingData) + { + GCodeFile.GetFirstNumberAfter("STEPPER:", line, ref stepperDistance); + + var stepperDelta = Math.Abs(stepperDistance - positionSensorData.LastStepperDistance); + + // if we think we should have move the filament by more than 1mm + if (stepperDelta > 1) + { + var sensorDelta = sensorDistance - positionSensorData.LastSensorDistance; + // check if the sensor data is within a tolerance of the stepper data + + var deltaRatio = sensorDelta / stepperDelta; + if (deltaRatio < .9 || deltaRatio > 1.1) + { + // we have a repartable discrepency set a runout state + positionSensorData.ExtrusionDiscrepency++; + if (positionSensorData.ExtrusionDiscrepency > 4) + { + readOutOfFilament = true; + } + } + else + { + positionSensorData.ExtrusionDiscrepency = 0; + } + + // and recard this position + positionSensorData.LastSensorDistance = sensorDistance; + positionSensorData.LastStepperDistance = stepperDistance; + } + } + } } } - } - }; - } - - public override void Dispose() - { - unregisterEvents?.Invoke(this, null); - base.Dispose(); + }; + } } public enum PauseReason { UserRequested, PauseLayerReached, GCodeRequest, FilamentRunout }