Source code

001package jmri.jmrix.bachrus.speedmatcher.speedStepScale;
002
003import jmri.DccThrottle;
004import jmri.jmrix.bachrus.Speed;
005
006/**
007 * This is a speed step scale speed matcher which will speed match a locomotive
008 * such that its speed in mph/kph will be equal to its speed step in 128 speed
009 * step mode. This uses the complex speed table, and the locomotive's speed will
010 * plateau at either its actual top speed or the set max speed, whichever is
011 * lower.
012 *
013 * @author Todd Wegter Copyright (C) 2024
014 */
015public class SpeedStepScaleSpeedTableSpeedMatcher extends SpeedStepScaleSpeedMatcher {
016
017    //<editor-fold defaultstate="collapsed" desc="Constants">
018    private final int INITIAL_STEP1 = 1;
019    private final int INITIAL_STEP28 = 255;
020    private final int INITIAL_TRIM = 128;
021
022    private final int TOP_SPEED_STEP_MAX = 255;
023    //</editor-fold>
024
025    //<editor-fold defaultstate="collapsed" desc="Enums">
026    protected enum SpeedMatcherState {
027        IDLE,
028        WAIT_FOR_THROTTLE,
029        INIT_THROTTLE,
030        INIT_ACCEL,
031        INIT_DECEL,
032        INIT_SPEED_TABLE,
033        INIT_FORWARD_TRIM,
034        INIT_REVERSE_TRIM,
035        POST_INIT,
036        FORWARD_WARM_UP,
037        READ_MAX_SPEED,
038        FORWARD_SPEED_MATCH_STEP28,
039        SET_UPPER_SPEED_STEPS,
040        FORWARD_SPEED_MATCH,
041        POST_SPEED_MATCH,
042        REVERSE_WARM_UP,
043        REVERSE_SPEED_MATCH_TRIM,
044        COMPLETE,
045        USER_STOPPED,
046        CLEAN_UP,
047    }
048    //</editor-fold>
049
050    //<editor-fold defaultstate="collapsed" desc="Instance Variables">
051    private SpeedTableStep initSpeedTableStep;
052    private int initSpeedTableStepValue;
053    private SpeedTableStep speedMatchSpeedTableStep;
054    private int lowestMaxSpeedStep;
055
056    private int step28CVValue = INITIAL_STEP28;
057    private float speedStepTargetSpeedKPH;
058
059    private int speedMatchCVValue = INITIAL_STEP28;
060    private int lastSpeedMatchCVValue = INITIAL_STEP28;
061    private int lastSpeedTableStepCVValue = INITIAL_STEP28;
062
063    private int reverseTrimValue = INITIAL_TRIM;
064    private int lastReverseTrimValue = INITIAL_TRIM;
065    private int reverseTrimSpeedStep;
066    private float reverseTrimSpeedKPH;
067
068    private SpeedMatcherState speedMatcherState = SpeedMatcherState.IDLE;
069    //</editor-fold>
070
071    /**
072     * Constructs the SpeedStepScaleSpeedTableSpeedMatcher from a
073     * SpeedStepScaleSpeedMatcherConfig
074     *
075     * @param config SpeedStepScaleSpeedMatcherConfig
076     */
077    public SpeedStepScaleSpeedTableSpeedMatcher(SpeedStepScaleSpeedMatcherConfig config) {
078        super(config);
079    }
080
081    //<editor-fold defaultstate="collapsed" desc="SpeedMatcherOverrides">
082    /**
083     * Starts the speed matching process
084     *
085     * @return true if speed matching started successfully, false otherwise
086     */
087    @Override
088    public boolean startSpeedMatcher() {
089        if (!validate()) {
090            return false;
091        }
092
093        //reset instance variables
094        speedMatchCVValue = INITIAL_STEP28;
095        lastSpeedMatchCVValue = INITIAL_STEP28;
096        lastSpeedTableStepCVValue = INITIAL_STEP28;
097        reverseTrimValue = INITIAL_TRIM;
098        lastReverseTrimValue = INITIAL_TRIM;
099        measuredMaxSpeedKPH = 0;
100        speedMatchMaxSpeedKPH = 0;
101
102        speedMatcherState = SpeedMatcherState.WAIT_FOR_THROTTLE;
103
104        actualMaxSpeedField.setText(String.format("___"));
105
106        if (!initializeAndStartSpeedMatcher(e -> speedMatchTimeout())) {
107            cleanUpSpeedMatcher();
108            return false;
109        }
110
111        startStopButton.setText(Bundle.getMessage("SpeedMatchStopBtn"));
112
113        return true;
114    }
115
116    /**
117     * Stops the speed matching process
118     */
119    @Override
120    public void stopSpeedMatcher() {
121        if (!isSpeedMatcherIdle()) {
122            logger.info("Speed matching manually stopped");
123            userStop();
124        } else {
125            cleanUpSpeedMatcher();
126        }
127    }
128
129    /**
130     * Indicates if the speed matcher is idle (not currently speed matching)
131     *
132     * @return true if idle, false otherwise
133     */
134    @Override
135    public boolean isSpeedMatcherIdle() {
136        return speedMatcherState == SpeedMatcherState.IDLE;
137    }
138
139    /**
140     * Cleans up the speed matcher when speed matching is stopped or is finished
141     */
142    @Override
143    protected void cleanUpSpeedMatcher() {
144        speedMatcherState = SpeedMatcherState.IDLE;
145        super.cleanUpSpeedMatcher();
146    }
147    //</editor-fold>
148
149    //<editor-fold defaultstate="collapsed" desc="Speed Matcher State">
150    /**
151     * Main speed matching timeout handler. This is the state machine that
152     * effectively does the speed matching process.
153     */
154    private synchronized void speedMatchTimeout() {
155        switch (speedMatcherState) {
156            case WAIT_FOR_THROTTLE:
157                cleanUpSpeedMatcher();
158                logger.error("Timeout waiting for throttle");
159                statusLabel.setText(Bundle.getMessage("StatusTimeout"));
160                break;
161
162            case INIT_THROTTLE:
163                //set throttle to 0 for init
164                setThrottle(true, 0);
165                initNextSpeedMatcherState(SpeedMatcherState.INIT_ACCEL);
166                break;
167
168            case INIT_ACCEL:
169                //set acceleration momentum to 0 (CV 3)
170                if (programmerState == ProgrammerState.IDLE) {
171                    writeMomentumAccel(INITIAL_MOMENTUM);
172                    initNextSpeedMatcherState(SpeedMatcherState.INIT_DECEL);
173                }
174                break;
175
176            case INIT_DECEL:
177                //set deceleration mementum to 0 (CV 4)
178                if (programmerState == ProgrammerState.IDLE) {
179                    writeMomentumDecel(INITIAL_MOMENTUM);
180                    initNextSpeedMatcherState(SpeedMatcherState.INIT_SPEED_TABLE);
181                }
182                break;
183
184            case INIT_SPEED_TABLE:
185                //initialize every speed table step to 1 except speed table step 28 = 255
186                if (programmerState == ProgrammerState.IDLE) {
187                    if (stepDuration == 0) {
188                        initSpeedTableStepValue = INITIAL_STEP1;
189                        initSpeedTableStep = SpeedTableStep.STEP1;
190                        stepDuration = 1;
191                    }
192
193                    if (initSpeedTableStep == SpeedTableStep.STEP28) {
194                        initSpeedTableStepValue = INITIAL_STEP28;
195                    }
196
197                    writeSpeedTableStep(initSpeedTableStep, initSpeedTableStepValue);
198
199                    initSpeedTableStep = initSpeedTableStep.getNext();
200                    if (initSpeedTableStep == null) {
201                        initNextSpeedMatcherState(SpeedMatcherState.INIT_FORWARD_TRIM);
202                    }
203                }
204                break;
205
206            case INIT_FORWARD_TRIM:
207                //set forward trim to 128 (CV 66)
208                if (programmerState == ProgrammerState.IDLE) {
209                    writeForwardTrim(INITIAL_TRIM);
210                    initNextSpeedMatcherState(SpeedMatcherState.INIT_REVERSE_TRIM);
211                }
212                break;
213
214            case INIT_REVERSE_TRIM:
215                //set reverse trim to 128 (CV 95)
216                if (programmerState == ProgrammerState.IDLE) {
217                    writeReverseTrim(INITIAL_TRIM);
218                    initNextSpeedMatcherState(SpeedMatcherState.POST_INIT);
219                }
220                break;
221
222            case POST_INIT: {
223                statusLabel.setText(Bundle.getMessage("StatRestoreThrottle"));
224
225                //un-brick Digitrax decoders
226                setThrottle(false, 0);
227                setThrottle(true, 0);
228
229                SpeedMatcherState nextState;
230                if (warmUpForwardSeconds > 0) {
231                    nextState = SpeedMatcherState.FORWARD_WARM_UP;
232                } else {
233                    nextState = SpeedMatcherState.READ_MAX_SPEED;
234                }
235                initNextSpeedMatcherState(nextState);
236                break;
237            }
238
239            case FORWARD_WARM_UP:
240                //Run 4 minutes at high speed forward
241                statusLabel.setText(Bundle.getMessage("StatForwardWarmUp", warmUpForwardSeconds - stepDuration));
242
243                if (stepDuration >= warmUpForwardSeconds) {
244                    initNextSpeedMatcherState(SpeedMatcherState.READ_MAX_SPEED);
245                } else {
246                    if (stepDuration == 0) {
247                        setSpeedMatchStateTimerDuration(5000);
248                        setThrottle(true, 28);
249                    }
250                    stepDuration += 5;
251                }
252                break;
253
254            case READ_MAX_SPEED:
255                //Run 10 second at high speed forward and record the speed
256                if (stepDuration == 0) {
257                    statusLabel.setText("Recording locomotive's maximum speed");
258                    setSpeedMatchStateTimerDuration(10000);
259                    setThrottle(true, 28);
260                    stepDuration = 1;
261                } else {
262                    measuredMaxSpeedKPH = currentSpeedKPH;
263
264                    String statusMessage = String.format("Measured maximum speed = %.1f KPH (%.1f MPH)", measuredMaxSpeedKPH, Speed.kphToMph(measuredMaxSpeedKPH));
265                    logger.info(statusMessage);
266
267                    if (measuredMaxSpeedKPH > targetMaxSpeedKPH) {
268                        speedMatchMaxSpeedKPH = targetMaxSpeedKPH;
269                        initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_STEP28);
270                    } else {
271                        //skip speed matching step 28 if max speed is less than target
272                        speedMatchMaxSpeedKPH = measuredMaxSpeedKPH;
273                        initNextSpeedMatcherState(SpeedMatcherState.SET_UPPER_SPEED_STEPS);
274                    }
275
276                    //set TOP_SPEED_STEP_MIN to the lowest speed step that will be set to the max speed
277                    float speedMatchMaxSpeed = speedUnit == Speed.Unit.MPH ? Speed.kphToMph(speedMatchMaxSpeedKPH) : speedMatchMaxSpeedKPH;
278                    lowestMaxSpeedStep = getLowestMaxSpeedStep(speedMatchMaxSpeed);
279
280                    actualMaxSpeedField.setText(String.format("%.1f", speedMatchMaxSpeed));
281                }
282                break;
283
284            case FORWARD_SPEED_MATCH_STEP28:
285                //Use PID Controller to adjust speed table step 28 to max speed
286                if (programmerState == ProgrammerState.IDLE) {
287                    if (stepDuration == 0) {
288                        speedMatchSpeedTableStep = SpeedTableStep.STEP28;
289                    }
290                    speedMatchSpeedStepInner(TOP_SPEED_STEP_MAX, lowestMaxSpeedStep, SpeedMatcherState.SET_UPPER_SPEED_STEPS, true);
291                    step28CVValue = speedMatchCVValue;
292                }
293                break;
294
295            case SET_UPPER_SPEED_STEPS:
296                //Set Speed table steps 27 through lowestMaxSpeedStep to step28CVValue
297                if (programmerState == ProgrammerState.IDLE) {
298                    if (stepDuration == 0) {
299                        speedMatchSpeedTableStep = SpeedTableStep.STEP27;
300                        stepDuration = 1;
301                    }
302
303                    writeSpeedTableStep(speedMatchSpeedTableStep, step28CVValue);
304
305                    speedMatchSpeedTableStep = speedMatchSpeedTableStep.getPrevious();
306
307                    if (speedMatchSpeedTableStep.getSpeedStep() < lowestMaxSpeedStep) {
308                        initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH);
309                    }
310                }
311                break;
312
313            case FORWARD_SPEED_MATCH:
314                //Use PID Controller to adjust table speed steps lowestMaxSpeedStep through 1 to the appropriate speed
315                if (programmerState == ProgrammerState.IDLE) {
316                    speedMatchSpeedStepInner(lastSpeedTableStepCVValue, speedMatchSpeedTableStep.getSpeedStep(), SpeedMatcherState.POST_SPEED_MATCH);
317                }
318                break;
319
320            case POST_SPEED_MATCH: {
321                statusLabel.setText(Bundle.getMessage("StatRestoreThrottle"));
322
323                //un-brick Digitrax decoders
324                setThrottle(false, 0);
325                setThrottle(true, 0);
326
327                SpeedMatcherState nextState;
328                if (trimReverseSpeed) {
329                    if (warmUpReverseSeconds > 0) {
330                        nextState = SpeedMatcherState.REVERSE_WARM_UP;
331                    } else {
332                        nextState = SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM;
333                    }
334                } else {
335                    nextState = SpeedMatcherState.COMPLETE;
336                }
337                initNextSpeedMatcherState(nextState);
338                break;
339            }
340
341            case REVERSE_WARM_UP:
342                //Run specified reverse warm up time at high speed in reverse
343                statusLabel.setText(Bundle.getMessage("StatReverseWarmUp", warmUpReverseSeconds - stepDuration));
344
345                if (stepDuration >= warmUpReverseSeconds) {
346                    initNextSpeedMatcherState(SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM);
347                } else {
348                    if (stepDuration == 0) {
349                        setSpeedMatchStateTimerDuration(5000);
350                        setThrottle(false, 28);
351                    }
352                    stepDuration += 5;
353                }
354
355                break;
356
357            case REVERSE_SPEED_MATCH_TRIM:
358                //Use PID controller logic to adjust reverse trim until high speed reverse speed matches forward
359                if (programmerState == ProgrammerState.IDLE) {
360                    if (stepDuration == 0) {
361                        statusLabel.setText(Bundle.getMessage("StatSettingReverseTrim"));
362                        reverseTrimSpeedStep = Math.max(lowestMaxSpeedStep - 2, 1);
363                        reverseTrimSpeedKPH = getSpeedStepScaleSpeedInKPH(reverseTrimSpeedStep);
364                        setThrottle(false, reverseTrimSpeedStep);
365                        setSpeedMatchStateTimerDuration(8000);
366                        stepDuration = 1;
367                    } else {
368                        setSpeedMatchError(reverseTrimSpeedKPH);
369
370                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
371                            initNextSpeedMatcherState(SpeedMatcherState.COMPLETE);
372                        } else {
373                            reverseTrimValue = getNextSpeedMatchValue(lastReverseTrimValue, REVERSE_TRIM_MAX, REVERSE_TRIM_MIN);
374
375                            if (((lastReverseTrimValue == REVERSE_TRIM_MAX) || (lastReverseTrimValue == REVERSE_TRIM_MIN)) && (reverseTrimValue == lastReverseTrimValue)) {
376                                statusLabel.setText(Bundle.getMessage("StatSetReverseTrimFail"));
377                                logger.info("Unable to trim reverse to match forward");
378                                abort();
379                                break;
380                            }
381
382                            lastReverseTrimValue = reverseTrimValue;
383                            writeReverseTrim(reverseTrimValue);
384                        }
385                    }
386                }
387                break;
388
389            case COMPLETE:
390                if (programmerState == ProgrammerState.IDLE) {
391                    statusLabel.setText(Bundle.getMessage("StatSpeedMatchComplete"));
392                    setThrottle(true, 0);
393                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
394                }
395                break;
396
397            case USER_STOPPED:
398                if (programmerState == ProgrammerState.IDLE) {
399                    statusLabel.setText(Bundle.getMessage("StatUserStoppedSpeedMatch"));
400                    setThrottle(true, 0);
401                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
402                }
403                break;
404
405            case CLEAN_UP:
406                //wrap it up
407                if (programmerState == ProgrammerState.IDLE) {
408                    cleanUpSpeedMatcher();
409                }
410                break;
411
412            default:
413                cleanUpSpeedMatcher();
414                logger.error("Unexpected speed match timeout");
415                break;
416        }
417
418        if (speedMatcherState != SpeedMatcherState.IDLE) {
419            startSpeedMatchStateTimer();
420        }
421    }
422    //</editor-fold>
423
424    //<editor-fold defaultstate="collapsed" desc="ThrottleListener Overrides">
425    /**
426     * Called when a throttle is found
427     *
428     * @param t the requested DccThrottle
429     */
430    @Override
431    public void notifyThrottleFound(DccThrottle t) {
432        super.notifyThrottleFound(t);
433
434        if (speedMatcherState == SpeedMatcherState.WAIT_FOR_THROTTLE) {
435            logger.info("Starting speed matching");
436            // using speed matching timer to trigger each phase of speed matching            
437            initNextSpeedMatcherState(SpeedMatcherState.INIT_THROTTLE);
438            startSpeedMatchStateTimer();
439        } else {
440            cleanUpSpeedMatcher();
441        }
442    }
443    //</editor-fold>
444
445    //<editor-fold defaultstate="collapsed" desc="Helper Functions">
446    /**
447     * Helper function for speed matching the current speedMatchSpeedTableStep
448     *
449     * @param maxCVValue the maximum allowable value for the CV
450     * @param minCVValue the minimum allowable value for the CV
451     * @param nextState  the SpeedMatcherState to advance to if speed matching
452     *                   is complete
453     */
454    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState) {
455        speedMatchSpeedStepInner(maxCVValue, minCVValue, nextState, false);
456    }
457
458    /**
459     * Helper function for speed matching the current speedMatchSpeedTableStep
460     *
461     * @param maxCVValue     the maximum allowable value for the CV
462     * @param minCVValue     the minimum allowable value for the CV
463     * @param nextState      the SpeedMatcherState to advance to if speed
464     *                       matching is complete
465     * @param forceNextState set to true to force speedMatcherState to the next
466     *                       state when speed matching the current
467     *                       speedMatchSpeedTableStep is complete
468     */
469    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState, boolean forceNextState) {
470        if (stepDuration == 0) {
471            speedStepTargetSpeedKPH = getSpeedStepScaleSpeedInKPH(speedMatchSpeedTableStep.getSpeedStep());
472
473            statusLabel.setText(Bundle.getMessage("StatSettingSpeed", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
474            logger.info("Setting CV {} (speed step {}) to {} KPH ({} MPH)", speedMatchSpeedTableStep.getCV(), speedMatchSpeedTableStep.getSpeedStep(), String.valueOf(speedStepTargetSpeedKPH), String.valueOf(Speed.kphToMph(speedStepTargetSpeedKPH)));
475
476            setThrottle(true, speedMatchSpeedTableStep.getSpeedStep());
477
478            writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
479
480            setSpeedMatchStateTimerDuration(8000);
481            stepDuration = 1;
482        } else {
483            setSpeedMatchError(speedStepTargetSpeedKPH);
484
485            if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
486                lastSpeedTableStepCVValue = speedMatchCVValue;
487
488                if (forceNextState) {
489                    initNextSpeedMatcherState(nextState);
490                    return;
491                }
492
493                speedMatchSpeedTableStep = speedMatchSpeedTableStep.getPrevious();
494
495                if (speedMatchSpeedTableStep != null) {
496                    initNextSpeedMatcherState(speedMatcherState);
497                } else {
498                    initNextSpeedMatcherState(nextState);
499                }
500            } else {
501                speedMatchCVValue = getNextSpeedMatchValue(lastSpeedMatchCVValue, maxCVValue, minCVValue);
502
503                if (((speedMatchCVValue == maxCVValue) || (speedMatchCVValue == minCVValue)) && (speedMatchCVValue == lastSpeedMatchCVValue)) {
504                    statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
505                    logger.info("Unable to achieve desired speed for CV {} (Speed Step {})", speedMatchSpeedTableStep.getCV(), String.valueOf(speedMatchSpeedTableStep.getSpeedStep()));
506                    abort();
507                    return;
508                }
509
510                lastSpeedMatchCVValue = speedMatchCVValue;
511                writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
512            }
513        }
514    }
515
516    /**
517     * Aborts the speed matching process programmatically
518     */
519    private void abort() {
520        initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
521    }
522
523    /**
524     * Stops the speed matching process due to user input
525     */
526    private void userStop() {
527        initNextSpeedMatcherState(SpeedMatcherState.USER_STOPPED);
528    }
529
530    /**
531     * Sets up the speed match state by clearing the speed match error, clearing
532     * the step duration, setting the timer duration, and setting the next state
533     *
534     * @param nextState next SpeedMatcherState to set
535     */
536    protected void initNextSpeedMatcherState(SpeedMatcherState nextState) {
537        resetSpeedMatchError();
538        stepDuration = 0;
539        speedMatcherState = nextState;
540        setSpeedMatchStateTimerDuration(1800);
541    }
542    //</editor-fold>
543
544    //debugging logger
545    private final static org.slf4j.Logger logger = org.slf4j.LoggerFactory.getLogger(SpeedStepScaleSpeedTableSpeedMatcher.class);
546}