From c9f1ae4e986ebcf63257b1fb715a031bcc251907 Mon Sep 17 00:00:00 2001 From: Markus Quaritsch <markus.quaritsch@tugraz.at> Date: Wed, 18 Jan 2017 14:24:22 +0100 Subject: [PATCH] refactor method 'getStopTimeInterval': use explicit states, split stop time interval with pto cycle into 3 parts --- .../Impl/DistanceBasedDrivingCycle.cs | 121 +++++++++++------- 1 file changed, 74 insertions(+), 47 deletions(-) diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs index 81cbc004fa..1b23510ed3 100644 --- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs +++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs @@ -121,8 +121,12 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl Left.StoppingTime, Left.VehicleTargetSpeed); throw new VectoSimulationException("Stopping Time only allowed when target speed is zero!"); } - - CurrentState.Response = DriveTimeInterval(absTime, GetStopTimeInterval()); + var dt = GetStopTimeInterval(); + if (dt == null) { + CurrentState.WaitPhase++; + dt = GetStopTimeInterval(); + } + CurrentState.Response = DriveTimeInterval(absTime, dt); return CurrentState.Response; } } @@ -165,54 +169,73 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl private Second GetStopTimeInterval() { - if (Left.PTOActive && IdleController != null) { - if (Left.StoppingTime.IsGreater(0)) { - // we have a pto cycle with stopping time: split into 3 parts: 1/2 stoptime, pto duration, 1/2 stoptime - if (PreviousState.WaitTime.IsEqual(0)) { - // first step: set dt to 1/2 stopping time - return Left.StoppingTime / 2; - } else { - if (PreviousState.WaitTime.IsEqual(Left.StoppingTime / 2)) { - // begining the second step: activate pto - IdleController.ActivatePTO(); - } - // second step: drive pto cycle intervals - var dt = IdleController.GetNextCycleTime(); - if (dt == null) { - // third step: pto has finished. activate normal idle controller and drive 1/2 stopping time again. - IdleController.ActivateIdle(); - return Left.StoppingTime / 2; - } - return dt; - } - } else { - // we have a pto cycle without stopping time. - if (PreviousState.WaitTime.IsEqual(0)) { - // begininng: activate pto - IdleController.ActivatePTO(); - } + if (!Left.PTOActive || IdleController == null) { + return Left.StoppingTime.IsGreater(3 * Constants.SimulationSettings.TargetTimeInterval) + ? GetStopTimeIntervalThreePhases() + : Left.StoppingTime; + } + if (Left.StoppingTime.IsGreater(6 * Constants.SimulationSettings.TargetTimeInterval)) { + // 7 pahses + return GetStopTimeIntervalSevenPhasesPTO(); + } + if (Left.StoppingTime.IsGreater(0)) { + // 3 phases + return GetStopTimeIntervalThreePhasesPTO(); + } + // we have a pto cycle without stopping time. + IdleController.ActivatePTO(); + return IdleController.GetNextCycleTime(); + } + + private Second GetStopTimeIntervalThreePhases() + { + switch (CurrentState.WaitPhase) { + case 1: + case 3: + CurrentState.WaitPhase++; + return Constants.SimulationSettings.TargetTimeInterval; + case 2: + return Left.StoppingTime - 2 * Constants.SimulationSettings.TargetTimeInterval; + } + return null; + } + private Second GetStopTimeIntervalThreePhasesPTO() + { + switch (CurrentState.WaitPhase) { + case 1: + case 3: + CurrentState.WaitPhase++; + IdleController.ActivateIdle(); + return Left.StoppingTime / 2; + case 2: + IdleController.ActivatePTO(); return IdleController.GetNextCycleTime(); - } - } else { - if (Left.StoppingTime.IsGreater(3 * Constants.SimulationSettings.TargetTimeInterval)) { - // split into 3 parts: targettime, stoptime-2*targettime, targettime - if (PreviousState.WaitTime.IsEqual(0)) { - // first step: just started waiting: set dt to targettime - return Constants.SimulationSettings.TargetTimeInterval; - } else { - // continue waiting with rest time - var dt = Left.StoppingTime - PreviousState.WaitTime; - // in second step dt is stoptime - targettime, therefore 1 targettime still has to be subtracted. - // in third step dt is exactly targettime. - if (dt.IsGreater(Constants.SimulationSettings.TargetTimeInterval)) { - dt -= Constants.SimulationSettings.TargetTimeInterval; - } - return dt; - } - } - return Left.StoppingTime; } + return null; + } + + private Second GetStopTimeIntervalSevenPhasesPTO() + { + switch (CurrentState.WaitPhase) { + case 1: + case 5: + CurrentState.WaitPhase++; + IdleController.ActivateIdle(); + return Constants.SimulationSettings.TargetTimeInterval; + case 3: + case 7: + CurrentState.WaitPhase++; + return Constants.SimulationSettings.TargetTimeInterval; + case 2: + case 6: + CurrentState.WaitPhase++; + return Left.StoppingTime / 2 - 2 * Constants.SimulationSettings.TargetTimeInterval; + case 4: + IdleController.ActivatePTO(); + return IdleController.GetNextCycleTime(); + } + return null; } private IResponse DriveTimeInterval(Second absTime, Second dt) @@ -238,6 +261,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl }; } + CurrentState.WaitPhase = 0; CurrentState.Distance = PreviousState.Distance + ds; CurrentState.SimulationDistance = ds; CurrentState.VehicleTargetSpeed = Left.VehicleTargetSpeed; @@ -494,6 +518,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl Distance = Distance, VehicleTargetSpeed = VehicleTargetSpeed, Altitude = Altitude, + WaitPhase = WaitPhase, // WaitTime is not cloned on purpose! WaitTime = 0.SI<Second>(), Response = null @@ -506,6 +531,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl public Second WaitTime; + public uint WaitPhase; + public MeterPerSecond VehicleTargetSpeed; public Meter Altitude; -- GitLab