private void GoToStation(int stationId, TrainStatus trainStatus) { // find destination station var station = _network.GetStationById(stationId); _log.Feed(_infoPin, $"Planning route to station {station.Name}"); // FIXME: wrong! correct to set this _only_ after arrival confirmation! _lastStationGoneTo = stationId; // FIXME: at the moment, this will only try the first stop of the station foreach (var stop in station.Stops) { int destinationSegmentId = stop.SegmentId; float destinationT = stop.T; var result = _routePlanner.PlanRoute(trainStatus.SegmentId, trainStatus.T, trainStatus.Dir, destinationSegmentId, destinationT); if (result != null) { var currentSegmentLength = _network.GetSegmentById(trainStatus.SegmentId).GetLength(); _currentPlanForStationId = stationId; _currentPlanCommand = new TractionControllerCommand { segmentsToFollow = new (int, SegmentEndpoint, float, float)[1 + result.route.Length]
private void GoToNearestStation(TrainStatus trainStatus, int?excludedStationId) { var(segmentId, t, dir) = (trainStatus.SegmentId, trainStatus.T, trainStatus.Dir); var nearest = _network.FindNearestStationAlongTrack(segmentId, t, dir, excludedStationId, false); if (nearest != null) { var(station, stop, distance, plan) = nearest.Value; // Very rough ETA -- 30 seconds + distance over max speed // var estimatedSecondsToGoal = 30 + distance / maxVelocity; // _eta = DateTime.Now + TimeSpan.FromSeconds(estimatedSecondsToGoal); // FIXME: sim time instead of real time // _planStation = station; // _plan = plan; _log.Feed(_infoPin, $"Set goal: station {station.Name}, {distance:F0} m away"); // , ETA {_eta:HH:mm:ss} GoToStation(station.Id, trainStatus); } else { _log.Feed(_infoPin, $"GOTO_NEAREST_STATION: Cannot find any station; segmentId={segmentId} t={t} dir={dir}"); _state = State.NO_PATH; } }
// WaypointControllerCommand is NOT idempotent, because we want to for example recognize two separate GOTO_NEAREST_STATION commands // Tried before: idempotent commands, nested switches... it was a mess public TractionControllerCommand?Update(DateTime simTime, WaypointControllerCommand?maybeCommand, TrainStatus trainStatus, TractionController.State tcState) { // Process any commands if (maybeCommand.HasValue) { var command = maybeCommand.Value; switch (command.mode) { case Mode.STOP: Stop(); break; case Mode.GOTO_STATION: GoToStation(command.gotoStationId.Value, trainStatus); break; case Mode.GOTO_NEAREST_STATION: GoToNearestStation(trainStatus, _lastStationGoneTo); break; } } // State update switch (_state) { case State.STOPPING: // TODO: check if finished stopping -> go to state STOPPED break; case State.EN_ROUTE: if (tcState == TractionController.State.IN_DESTINATION) { // (This assumes that the current plan goes all the way to destination) _state = State.ARRIVED; _lastStationArrivedAt = _currentPlanForStationId; } break; } _log.Feed(_statePin, _state.ToString()); // Output switch (_state) { case State.NO_PATH: case State.STOPPED: case State.STOPPING: case State.PLANNING: case State.ARRIVED: return(TractionControllerCommand.STOP); case State.EN_ROUTE: return(_currentPlanCommand.Value); default: throw new InvalidOperationException(); } }