diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs index a421474c4bfcd9ab09c729875092f7f25f2cbef4..df5b2a1b00277ce737e04540b2c644011eeb9c81 100644 --- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs +++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs @@ -450,14 +450,26 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl } protected BrakingPhase Phase; + protected bool RetryDistanceExceeded = false; protected override IResponse DoHandleRequest(Second absTime, Meter ds, MeterPerSecond targetVelocity, Radian gradient) { IResponse response = null; if (DataBus.VehicleSpeed <= DriverStrategy.BrakeTrigger.NextTargetSpeed) { - response = DataBus.ClutchClosed(absTime) - ? Driver.DrivingActionAccelerate(absTime, ds, DriverStrategy.BrakeTrigger.NextTargetSpeed, gradient) - : Driver.DrivingActionRoll(absTime, ds, DriverStrategy.BrakeTrigger.NextTargetSpeed, gradient); + if (DataBus.ClutchClosed(absTime)) { + if (DataBus.VehicleSpeed.IsGreater(0.SI<MeterPerSecond>())) { + response = Driver.DrivingActionAccelerate(absTime, ds, DriverStrategy.BrakeTrigger.NextTargetSpeed, gradient); + } else { + if (RetryDistanceExceeded) { + response = Driver.DrivingActionAccelerate(absTime, ds, targetVelocity, gradient); + } else { + RetryDistanceExceeded = true; + return new ResponseDrivingCycleDistanceExceeded() { MaxDistance = ds / 2 }; + } + } + } else { + response = Driver.DrivingActionRoll(absTime, ds, DriverStrategy.BrakeTrigger.NextTargetSpeed, gradient); + } response.Switch(). Case<ResponseGearShift>(() => { response = Driver.DrivingActionRoll(absTime, ds, targetVelocity, gradient); @@ -609,6 +621,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl public override void ResetMode() { + RetryDistanceExceeded = false; Phase = BrakingPhase.Coast; } }