Exemple #1
0
        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]
Exemple #2
0
        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;
            }
        }
Exemple #3
0
        // 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();
            }
        }