ATC: speed restrictions and AI

src/Devices/AtcBombardier.cs

@@ -63,6 +63,40 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>A counter to keep track of YARD mode idle time, in seconds.</summary>
         private double YardIdleCounter;
 
+        /// <summary>The maximum speed in YARD mode.</summary>
+        private double YardMaximumSpeed = 20;
+
+        /// <summary>Represents an ATC movement permission order.</summary>
+        private class MovementPermission
+        {
+            internal Speed LimitBase { get; private set; }
+            internal Speed LimitNoTraction { get; private set; }
+            internal Speed LimitService { get; private set; }
+            internal Speed LimitEmergency { get; private set; }
+            internal Speed TargetLimit { get; private set; }
+            internal double TargetLocation { get; private set; }
+
+            internal MovementPermission(Speed limit, Speed target, double targetLocation)
+            {
+                LimitBase = limit;
+                LimitNoTraction = new Speed(limit.MetersPerSecond + 3 / 3.6);
+                LimitService = new Speed(limit.MetersPerSecond + 6 / 3.6);
+                LimitEmergency = new Speed(limit.MetersPerSecond + 9 / 3.6);
+                TargetLimit = target;
+                TargetLocation = targetLocation;
+            }
+        }
+
+        /// <summary>The current permission regarding speed and travel distance.</summary>
+        private MovementPermission CurrentPermission;
+
+        /// <summary>The ideal power notch for the AI.</summary>
+        private int AiPowerNotch;
+
+        /// <summary>The ideal brake notch for the AI.</summary>
+        private int AiBrakeNotch;
+
+
         /// <summary>Is called when the device state should be updated.</summary>
         /// <param name="train">The current train.</param>
         /// <param name="route">The current route.</param>
@@ -76,70 +110,63 @@ namespace OpenbveFcmbTrainPlugin
                 DeviceState = DeviceStates.Initializing;
             }
 
+            // Update ATC movement permission
+            UpdatePermission();
+
             double speed = train.State.Speed.KilometersPerHour;
             bool stopped = speed < 0.05;
 
-            // Speed limit calculation
-            // By default, use YARD mode speed limit
-            double limit = 25;
-            double powerLimit = limit - 2;
-            double releaseLimit = limit - 5;
-            double emergencyLimit = limit + 4;
-
             // Speed limit enforcement
             if (DeviceState == DeviceStates.YARD || DeviceState == DeviceStates.ATP || DeviceState == DeviceStates.ATO)
             {
-                if (AtcControlState == AtcControlStates.Released && speed >= powerLimit)
+                if (AtcControlState == AtcControlStates.Released && speed > CurrentPermission.LimitNoTraction.KilometersPerHour)
                 {
                     // Cut power on power limit
                     AtcControlState = AtcControlStates.NoPower;
                 }
-                if (AtcControlState == AtcControlStates.NoPower && speed < powerLimit)
+                if (AtcControlState == AtcControlStates.NoPower && speed < CurrentPermission.LimitNoTraction.KilometersPerHour)
                 {
                     // Stop cutting power below power limit
                     AtcControlState = AtcControlStates.Released;
                 }
-                if (speed > limit && speed < emergencyLimit)
+                if (speed > CurrentPermission.LimitService.KilometersPerHour && speed < CurrentPermission.LimitEmergency.KilometersPerHour)
                 {
                     // Apply service brake on limit
                     AtcControlState = AtcControlStates.BrakeServiceNoRelease;
                 }
-                if (speed > emergencyLimit)
+                if (speed > CurrentPermission.LimitEmergency.KilometersPerHour)
                 {
                     // Unselect driving mode to apply emergency brake if overspeeding
                     DeviceState = DeviceStates.Initialized;
                 }
 
                 // Allow brake release under defined threshold
-                if (AtcControlState == AtcControlStates.BrakeServiceNoRelease && speed < releaseLimit)
+                if (AtcControlState == AtcControlStates.BrakeServiceNoRelease && speed < CurrentPermission.LimitBase.KilometersPerHour)
                 {
                     AtcControlState = AtcControlStates.BrakeService;
                 }
             }
 
             // Brake application
-            if (DeviceState != DeviceStates.Override)
+            switch (AtcControlState)
             {
-                switch (AtcControlState)
+                case AtcControlStates.Released:
-                {
+                    RequestedBrakeNotch = -1;
-                    case AtcControlStates.Released:
+                    RequestedPowerNotch = -1;
-                        RequestedBrakeNotch = -1;
+                    break;
-                        RequestedPowerNotch = -1;
+                case AtcControlStates.NoPower:
-                        break;
+                    RequestedBrakeNotch = -1;
-                    case AtcControlStates.NoPower:
+                    RequestedPowerNotch = 0;
-                        RequestedBrakeNotch = -1;
+                    break;
-                        RequestedPowerNotch = 0;
+                case AtcControlStates.BrakeService:
-                        break;
+                case AtcControlStates.BrakeServiceNoRelease:
-                    case AtcControlStates.BrakeService:
+                    RequestedBrakeNotch = train.ServiceBrakeNotch;
-                    case AtcControlStates.BrakeServiceNoRelease:
+                    RequestedPowerNotch = 0;
-                        RequestedBrakeNotch = train.ServiceBrakeNotch;
+                    break;
-                        RequestedPowerNotch = 0;
+                case AtcControlStates.BrakeEmergency:
-                        break;
+                    RequestedBrakeNotch = train.Specs.BrakeNotches + 1;
-                    case AtcControlStates.BrakeEmergency:
+                    RequestedPowerNotch = 0;
-                        RequestedBrakeNotch = train.Specs.BrakeNotches + 1;
+                    break;
-                        RequestedPowerNotch = 0;
-                        break;
-                }
             }
 
             switch (DeviceState)
@@ -149,8 +176,7 @@ namespace OpenbveFcmbTrainPlugin
                     train.ContinuousProtection = false;
                     train.VigilanceOverride = false;
                     // Release control of the brakes and power
-                    RequestedBrakeNotch = -1;
+                    AtcControlState = AtcControlStates.Released;
-                    RequestedPowerNotch = -1;
                     break;
                 // ATC device is initializing
                 case DeviceStates.Initializing:
@@ -173,6 +199,8 @@ namespace OpenbveFcmbTrainPlugin
                     train.VigilanceOverride = false;
                     // Apply service/emergency brake while no driving mode is selected
                     AtcControlState = stopped ? AtcControlStates.BrakeServiceNoRelease : AtcControlStates.BrakeEmergency;
+                    // ATC movement permission
+                    CurrentPermission = new MovementPermission(new Speed(0), new Speed(0), 0);
                     break;
                 // ATC device is in YARD (M+25) driving mode
                 case DeviceStates.YARD:
@@ -182,36 +210,18 @@ namespace OpenbveFcmbTrainPlugin
                     if (stopped)
                     {
                         YardIdleCounter += elapsedTime.Seconds;
+                        // If above idle time, apply the service brake
+                        AtcControlState = YardIdleCounter >= YardIdleTime ? AtcControlStates.BrakeService : AtcControlStates.Released;
                     }
                     else
                     {
                         YardIdleCounter = 0;
                     }
-                    // If above idle time, apply the service brake
-                    if (YardIdleCounter >= YardIdleTime)
-                    {
-                        AtcControlState = AtcControlStates.BrakeService;
-                    }
-                    else
-                    {
-                        AtcControlState = AtcControlStates.Released;
-                    }
                     // Apply brake if any door opens
                     if (train.DoorState != DoorStates.None)
                     {
                         RequestedBrakeNotch = stopped ? train.Specs.BrakeNotches + 1 : train.ServiceBrakeNotch;
                     }
-                    // Speed limitations
-                    if (speed > 23)
-                    {
-                        // Cut power over 23 km/h
-                        RequestedPowerNotch = 0;
-                        if (speed > 25)
-                        {
-                            // Apply service brake over 25 km/h
-                            AtcControlState = AtcControlStates.BrakeServiceNoRelease;
-                        }
-                    }
                     break;
                 // ATC device is in ATP (M+ATP) driving mode
                 case DeviceStates.ATP:
@@ -239,17 +249,19 @@ namespace OpenbveFcmbTrainPlugin
             switch (AtcControlState)
             {
                 case AtcControlStates.Released:
-                case AtcControlStates.BrakeEmergency:
+                case AtcControlStates.NoPower:
                     train.Panel[32] = 0;
                     break;
                 case AtcControlStates.BrakeServiceNoRelease:
                 case AtcControlStates.BrakeFull:
-                    train.Panel[32] = 1;
+                case AtcControlStates.BrakeEmergency:
+                    train.Panel[32] = DeviceState != DeviceStates.Initializing ? 1 : 0;
                     break;
                 case AtcControlStates.BrakeService:
                     train.Panel[32] = blink ? 1 : 0;
                     break;
             }
+            train.Panel[34] = (int)CurrentPermission.LimitBase.KilometersPerHour * 1000;
         }
 
 
@@ -259,6 +271,9 @@ namespace OpenbveFcmbTrainPlugin
         /// <param name="train">The current train.</param>
         internal override void KeyChange(VirtualKeys key, bool pressed, Train train)
         {
+            double speed = train.State.Speed.KilometersPerHour;
+            bool stopped = speed < 0.05;
+
             if (pressed)
             {
                 switch (key)
@@ -267,7 +282,7 @@ namespace OpenbveFcmbTrainPlugin
                         // YARD (M+25) mode selection button
                         if (!OpenbveFcmbTrainPlugin.KeysPressed[(int)key])
                         {
-                            if (DeviceState == DeviceStates.Initialized && train.State.Speed.KilometersPerHour < 0.05)
+                            if (DeviceState == DeviceStates.Initialized && stopped)
                             {
                                 DeviceState = DeviceStates.YARD;
                             }
@@ -314,14 +329,14 @@ namespace OpenbveFcmbTrainPlugin
                                 case DeviceStates.Initialized:
                                 case DeviceStates.YARD:
                                     // Override device if possible
-                                    if (train.State.Speed.KilometersPerHour < 0.05 && train.PhysicalHandles.PowerNotch == 0)
+                                    if (stopped && train.PhysicalHandles.PowerNotch == 0)
                                     {
                                         DeviceState = DeviceStates.Override;
                                     }
                                     break; 
                                 case DeviceStates.Override:
                                     // Disable override if possible
-                                    if (train.State.Speed.KilometersPerHour < 0.05 && train.PhysicalHandles.PowerNotch == 0)
+                                    if (stopped && train.PhysicalHandles.PowerNotch == 0)
                                     {
                                         DeviceState = DeviceStates.Initializing;
                                     }
@@ -346,6 +361,13 @@ namespace OpenbveFcmbTrainPlugin
         /// <param name="route">The current route.</param>
         internal override void PerformAI(AIData data, Train train, Route route)
         {
+            // Set AI notches to what the AI is doing right now
+            AiBrakeNotch = data.Handles.BrakeNotch;
+            AiPowerNotch = data.Handles.PowerNotch;
+
+            double speed = train.State.Speed.KilometersPerHour;
+            bool stopped = speed < 0.05;
+
             switch (DeviceState)
             {
                 case DeviceStates.Initializing:
@@ -357,14 +379,92 @@ namespace OpenbveFcmbTrainPlugin
                     data.Handles.BrakeNotch = train.Specs.BrakeNotches;
                     data.Response = AIResponse.Short;
                     // If the train is stopped, select YARD (M+25) mode
-                    if (train.State.Speed.KilometersPerHour < 0.05)
+                    if (stopped)
                     {
-                        data.Response = AIResponse.Long;
                         KeyChange(VirtualKeys.I, true, train);
                         data.Response = AIResponse.Short;
                         KeyChange(VirtualKeys.I, false, train);
                     }
                     break;
+                case DeviceStates.YARD:
+                case DeviceStates.ATP:
+                    // If the ATC brakes can be released, press the ATC brake release button
+                    if (AtcControlState == AtcControlStates.BrakeService)
+                    {
+                        KeyChange(VirtualKeys.B1, true, train);
+                        data.Response = AIResponse.Medium;
+                        KeyChange(VirtualKeys.B1, false, train);
+                    }
+                    // Calculate ideal notch for AI when approaching the speed limit
+                    if (speed > CurrentPermission.LimitBase.KilometersPerHour - 2 && train.Acceleration > 0.1)
+                    {
+                        if (data.Handles.PowerNotch > 0)
+                        {
+                            AiPowerNotch -= 1;
+                        }
+                        else if (train.Specs.HasHoldBrake)
+                        {
+                            data.Handles.HoldBrake = true;
+                        }
+                        else
+                        {
+                            AiBrakeNotch += 1;
+                        }
+                    }
+                    else if (speed < CurrentPermission.LimitBase.KilometersPerHour - 4 && train.Acceleration < 0.25)
+                    {
+                        if (data.Handles.BrakeNotch > 0)
+                        {
+                            AiBrakeNotch -= 1;
+                        }
+                        else
+                        {
+                            AiPowerNotch += 1;
+                        }
+                    }
+                    // If still far from next station, take control of AI handles
+                    if (speed / (route.CurrentStation.StopPosition - train.State.Location) < 0.2)
+                    {
+                        if (speed > CurrentPermission.LimitBase.KilometersPerHour - 5)
+                        {
+                            data.Handles.PowerNotch = AiPowerNotch;
+                            data.Handles.BrakeNotch = AiBrakeNotch;
+                            data.Response = AIResponse.Medium;
+                        }
+                    }
+                    break;
+            }
+        }
+
+        /// <summary>Updates the data for the current and next station.</summary>
+        internal void UpdatePermission()
+        {
+            Speed zero = new Speed(0);
+            Speed yard = new Speed(YardMaximumSpeed / 3.6);
+
+            switch (DeviceState)
+            {
+                case DeviceStates.Override:
+                case DeviceStates.Initializing:
+                case DeviceStates.Initialized:
+                    if (CurrentPermission == null || CurrentPermission.LimitBase != zero || CurrentPermission.TargetLimit != zero)
+                    {
+                        CurrentPermission = new MovementPermission(zero, zero, 0);
+                    }
+                    break;
+                case DeviceStates.YARD:
+                    if (CurrentPermission == null || CurrentPermission.LimitBase != yard || CurrentPermission.TargetLimit != yard)
+                    {
+                        CurrentPermission = new MovementPermission(yard, yard, 0);
+                    }
+                    break;
+                case DeviceStates.ATP:
+                case DeviceStates.ATO:
+                    if (CurrentPermission == null || CurrentPermission.LimitBase != zero || CurrentPermission.TargetLimit != zero)
+                    {
+                        CurrentPermission = new MovementPermission(zero, zero, 0);
+                    }
+                    break;
             }
         }
 

src/Train/Train.cs

@@ -1,4 +1,5 @@
-using System.Collections.Generic;
+using System;
+using System.Collections.Generic;
 using OpenBveApi.Runtime;
 
 namespace OpenbveFcmbTrainPlugin
@@ -47,6 +48,15 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>The service brake notch for ATC.</summary>
         internal int ServiceBrakeNotch { get; private set; }
 
+        /// <summary>The current acceleration of the train.</summary>
+        internal double Acceleration { get; private set; }
+
+        /// <summary>The time when acceleration was calculated.</summary>
+        private Time AccelerationTime = new Time(0);
+
+        /// <summary>The speed when acceleration was calculated.</summary>
+        private Speed AccelerationSpeed = new Speed(0);
+
         /// <summary>Creates a new train with the device configuration provided.</summary>
         /// <param name="panel">The array of panel variables.</pa
         internal Train(int[] panel)
@@ -91,6 +101,18 @@ namespace OpenbveFcmbTrainPlugin
             data.Handles.PowerNotch = data.Handles.PowerNotch;
             data.Handles.BrakeNotch = data.Handles.BrakeNotch;
 
+            // Calculate acceleration
+            if (OpenbveFcmbTrainPlugin.Initializing)
+            {
+                AccelerationTime = data.TotalTime;
+            }
+            if (data.TotalTime.Milliseconds - AccelerationTime.Milliseconds > 200)
+            {
+                Acceleration = Math.Round((State.Speed.MetersPerSecond - AccelerationSpeed.MetersPerSecond) / (data.TotalTime.Seconds - AccelerationTime.Seconds), 4);
+                AccelerationTime = data.TotalTime;
+                AccelerationSpeed = State.Speed;
+            }
+
             // Retrieve data from all devices
             foreach (Device dev in Devices)
             {