diff --git a/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs b/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs index e16b637a9f81dbd5adedfe8603b0e29897d042ad..20e33b17a8fc6d4d2b0c9c45001d6079fdcd27df 100644 --- a/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs +++ b/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs @@ -78,6 +78,16 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl return retVal; } + public IResponse Request(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient) + { + DriverBehavior = DrivingBehavior.Halted; + return Driver.DrivingActionHalt(absTime, dt, targetVelocity, gradient); + } + + + public DrivingBehavior DriverBehavior { get; internal set; } + + private void UpdateDrivingAction(Meter currentDistance) { var nextAction = GetNextDrivingAction(currentDistance); @@ -126,15 +136,6 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl } - public IResponse Request(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient) - { - DriverBehavior = DrivingBehavior.Halted; - return Driver.DrivingActionHalt(absTime, dt, targetVelocity, gradient); - } - - - public DrivingBehavior DriverBehavior { get; internal set; } - protected DrivingBehaviorEntry GetNextDrivingAction(Meter minDistance) { var currentSpeed = Driver.DataBus.VehicleSpeed; @@ -152,7 +153,6 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl ? entry.VehicleTargetSpeed + Driver.DriverData.OverSpeedEcoRoll.OverSpeed : entry.VehicleTargetSpeed; if (nextTargetSpeed < currentSpeed) { - // TODO @@@quam currentSpeed ? targetSpeed? nextTargetSpeed? if (!Driver.DriverData.LookAheadCoasting.Enabled || currentSpeed < Driver.DriverData.LookAheadCoasting.MinSpeed) { var brakingDistance = Driver.ComputeDecelerationDistance(nextTargetSpeed); @@ -178,14 +178,6 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl }); } } - //if (nextTargetSpeed > currentSpeed) { - // nextActions.Add(new DrivingBehaviorEntry { - // Action = DefaultDriverStrategy.DrivingBehavior.Accelerating, - // NextTargetSpeed = entry.VehicleTargetSpeed, - // TriggerDistance = entry.Distance, - // ActionDistance = entry.Distance - // }); - //} } return nextActions.Count == 0 ? null : nextActions.OrderBy(x => x.ActionDistance).First(); diff --git a/VectoCore/Models/SimulationComponent/Impl/Vehicle.cs b/VectoCore/Models/SimulationComponent/Impl/Vehicle.cs index 1472ab894312eea87f5be8fc2b5eaae5ec5ce70f..a5ac4cdc5657739facb9dc8ed99b49f72b4fe710 100644 --- a/VectoCore/Models/SimulationComponent/Impl/Vehicle.cs +++ b/VectoCore/Models/SimulationComponent/Impl/Vehicle.cs @@ -125,6 +125,9 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl _currentState.dt = dt; _currentState.Acceleration = acceleration; _currentState.Velocity = _previousState.Velocity + acceleration * dt; + if (_currentState.Velocity.IsEqual(0, 1e-4)) { + _currentState.Velocity = 0.SI<MeterPerSecond>(); + } _currentState.Distance = _previousState.Distance + dt * (_previousState.Velocity + _currentState.Velocity) / 2; _currentState.DriverAcceleration = DriverAcceleration(acceleration);