Source code

001package jmri.jmrix.bachrus.speedmatcher.speedStepScale;
002
003import java.util.Locale;
004
005import jmri.DccThrottle;
006import jmri.jmrix.bachrus.Speed;
007
008/**
009 * This is a speed step scale speed matcher which will speed match a locomotive
010 * such that its speed in mph/kph will be equal to its speed step in 128 speed
011 * step mode. This uses the complex speed table, and the locomotive's speed will
012 * plateau at either its actual top speed or the set max speed, whichever is
013 * lower.
014 *
015 * @author Todd Wegter Copyright (C) 2024
016 */
017public class SpeedStepScaleSpeedTableSpeedMatcher extends SpeedStepScaleSpeedMatcher {
018
019    //<editor-fold defaultstate="collapsed" desc="Constants">
020    private final int INITIAL_STEP1 = 1;
021    private final int INITIAL_STEP28 = 255;
022    private final int INITIAL_TRIM = 128;
023
024    private final int TOP_SPEED_STEP_MAX = 255;
025    //</editor-fold>
026
027    //<editor-fold defaultstate="collapsed" desc="Enums">
028    protected enum SpeedMatcherState {
029        IDLE,
030        WAIT_FOR_THROTTLE,
031        INIT_THROTTLE,
032        INIT_ACCEL,
033        INIT_DECEL,
034        INIT_SPEED_TABLE,
035        INIT_FORWARD_TRIM,
036        INIT_REVERSE_TRIM,
037        POST_INIT,
038        FORWARD_WARM_UP,
039        READ_MAX_SPEED,
040        FORWARD_SPEED_MATCH_STEP28,
041        SET_UPPER_SPEED_STEPS,
042        FORWARD_SPEED_MATCH,
043        POST_SPEED_MATCH,
044        REVERSE_WARM_UP,
045        REVERSE_SPEED_MATCH_TRIM,
046        COMPLETE,
047        USER_STOPPED,
048        CLEAN_UP,
049    }
050    //</editor-fold>
051
052    //<editor-fold defaultstate="collapsed" desc="Instance Variables">
053    private SpeedTableStep initSpeedTableStep;
054    private int initSpeedTableStepValue;
055    private SpeedTableStep speedMatchSpeedTableStep;
056    private int speedMatchMaxSpeedStep;
057
058    private int step28CVValue = INITIAL_STEP28;
059    private float speedStepTargetSpeedKPH;
060
061    private int speedMatchCVValue = INITIAL_STEP28;
062    private int lastSpeedMatchCVValue = INITIAL_STEP28;
063    private int lastSpeedTableStepCVValue = INITIAL_STEP28;
064
065    private int reverseTrimValue = INITIAL_TRIM;
066    private int lastReverseTrimValue = INITIAL_TRIM;
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("___");
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(Locale.getDefault(),
265                        "Measured maximum speed = %.1f KPH (%.1f MPH)",
266                            measuredMaxSpeedKPH, Speed.kphToMph(measuredMaxSpeedKPH));
267                    logger.info(statusMessage);
268                    
269                    float speedMatchMaxSpeed;
270
271                    if (measuredMaxSpeedKPH > targetMaxSpeedKPH) {
272                        speedMatchMaxSpeedStep = targetMaxSpeedStep.getSpeedTableStep().getSpeedStep();
273                        speedMatchMaxSpeed = targetMaxSpeedStep.getSpeed();
274                        speedMatchMaxSpeedKPH = targetMaxSpeedKPH;
275                    } else {
276                        float measuredMaxSpeed = speedUnit == Speed.Unit.MPH ? Speed.kphToMph(measuredMaxSpeedKPH) : measuredMaxSpeedKPH;
277                        speedMatchMaxSpeedStep = getNextLowestSpeedTableStepForSpeed(measuredMaxSpeed);
278                        speedMatchMaxSpeed = getSpeedForSpeedTableStep(speedMatchMaxSpeedStep);
279                        speedMatchMaxSpeedKPH = speedUnit == Speed.Unit.MPH ? Speed.mphToKph(speedMatchMaxSpeed): speedMatchMaxSpeed;
280                    }
281                    
282                    actualMaxSpeedField.setText(String.format(Locale.getDefault(), "%.1f", speedMatchMaxSpeed));
283                    
284                    initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_STEP28, 30);
285                }
286                break;
287
288            case FORWARD_SPEED_MATCH_STEP28:
289                //Use PID Controller to adjust speed table step 28 to max speed
290                if (programmerState == ProgrammerState.IDLE) {
291                    if (stepDuration == 0) {
292                        speedMatchSpeedTableStep = SpeedTableStep.STEP28;
293                    }
294                    speedMatchSpeedStepInner(TOP_SPEED_STEP_MAX, speedMatchMaxSpeedStep, SpeedMatcherState.SET_UPPER_SPEED_STEPS, true);
295                    step28CVValue = speedMatchCVValue;
296                }
297                break;
298
299            case SET_UPPER_SPEED_STEPS:
300                //Set Speed table steps 27 through lowestMaxSpeedStep to step28CVValue
301                if (programmerState == ProgrammerState.IDLE) {
302                    if (stepDuration == 0) {
303                        speedMatchSpeedTableStep = SpeedTableStep.STEP27;
304                        stepDuration = 1;
305                    }
306
307                    writeSpeedTableStep(speedMatchSpeedTableStep, step28CVValue);
308
309                    speedMatchSpeedTableStep = speedMatchSpeedTableStep.getPrevious();
310
311                    if (speedMatchSpeedTableStep.getSpeedStep() < speedMatchMaxSpeedStep) {
312                        initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH);
313                    }
314                }
315                break;
316
317            case FORWARD_SPEED_MATCH:
318                //Use PID Controller to adjust table speed steps lowestMaxSpeedStep through 1 to the appropriate speed
319                if (programmerState == ProgrammerState.IDLE) {
320                    speedMatchSpeedStepInner(lastSpeedTableStepCVValue, speedMatchSpeedTableStep.getSpeedStep(), SpeedMatcherState.POST_SPEED_MATCH);
321                }
322                break;
323
324            case POST_SPEED_MATCH: {
325                statusLabel.setText(Bundle.getMessage("StatRestoreThrottle"));
326
327                //un-brick Digitrax decoders
328                setThrottle(false, 0);
329                setThrottle(true, 0);
330
331                SpeedMatcherState nextState;
332                if (trimReverseSpeed) {
333                    if (warmUpReverseSeconds > 0) {
334                        nextState = SpeedMatcherState.REVERSE_WARM_UP;
335                    } else {
336                        nextState = SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM;
337                    }
338                } else {
339                    nextState = SpeedMatcherState.COMPLETE;
340                }
341                initNextSpeedMatcherState(nextState);
342                break;
343            }
344
345            case REVERSE_WARM_UP:
346                //Run specified reverse warm up time at high speed in reverse
347                statusLabel.setText(Bundle.getMessage("StatReverseWarmUp", warmUpReverseSeconds - stepDuration));
348
349                if (stepDuration >= warmUpReverseSeconds) {
350                    initNextSpeedMatcherState(SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM);
351                } else {
352                    if (stepDuration == 0) {
353                        setSpeedMatchStateTimerDuration(5000);
354                        setThrottle(false, 28);
355                    }
356                    stepDuration += 5;
357                }
358
359                break;
360
361            case REVERSE_SPEED_MATCH_TRIM:
362                //Use PID controller logic to adjust reverse trim until high speed reverse speed matches forward
363                if (programmerState == ProgrammerState.IDLE) {
364                    if (stepDuration == 0) {
365                        statusLabel.setText(Bundle.getMessage("StatSettingReverseTrim"));
366                        setThrottle(false, speedMatchMaxSpeedStep);
367                        setSpeedMatchStateTimerDuration(8000);
368                        stepDuration = 1;
369                    } else {
370                        setSpeedMatchError(speedMatchMaxSpeedKPH);
371
372                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
373                            initNextSpeedMatcherState(SpeedMatcherState.COMPLETE);
374                        } else {
375                            reverseTrimValue = getNextSpeedMatchValue(lastReverseTrimValue, REVERSE_TRIM_MAX, REVERSE_TRIM_MIN);
376
377                            if (((lastReverseTrimValue == REVERSE_TRIM_MAX) || (lastReverseTrimValue == REVERSE_TRIM_MIN)) && (reverseTrimValue == lastReverseTrimValue)) {
378                                statusLabel.setText(Bundle.getMessage("StatSetReverseTrimFail"));
379                                logger.info("Unable to trim reverse to match forward");
380                                abort();
381                                break;
382                            }
383
384                            lastReverseTrimValue = reverseTrimValue;
385                            writeReverseTrim(reverseTrimValue);
386                        }
387                    }
388                }
389                break;
390
391            case COMPLETE:
392                if (programmerState == ProgrammerState.IDLE) {
393                    statusLabel.setText(Bundle.getMessage("StatSpeedMatchComplete"));
394                    setThrottle(true, 0);
395                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
396                }
397                break;
398
399            case USER_STOPPED:
400                if (programmerState == ProgrammerState.IDLE) {
401                    statusLabel.setText(Bundle.getMessage("StatUserStoppedSpeedMatch"));
402                    setThrottle(true, 0);
403                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
404                }
405                break;
406
407            case CLEAN_UP:
408                //wrap it up
409                if (programmerState == ProgrammerState.IDLE) {
410                    cleanUpSpeedMatcher();
411                }
412                break;
413
414            default:
415                cleanUpSpeedMatcher();
416                logger.error("Unexpected speed match timeout");
417                break;
418        }
419
420        if (speedMatcherState != SpeedMatcherState.IDLE) {
421            startSpeedMatchStateTimer();
422        }
423    }
424    //</editor-fold>
425
426    //<editor-fold defaultstate="collapsed" desc="ThrottleListener Overrides">
427    /**
428     * Called when a throttle is found
429     *
430     * @param t the requested DccThrottle
431     */
432    @Override
433    public void notifyThrottleFound(DccThrottle t) {
434        super.notifyThrottleFound(t);
435
436        if (speedMatcherState == SpeedMatcherState.WAIT_FOR_THROTTLE) {
437            logger.info("Starting speed matching");
438            // using speed matching timer to trigger each phase of speed matching            
439            initNextSpeedMatcherState(SpeedMatcherState.INIT_THROTTLE);
440            startSpeedMatchStateTimer();
441        } else {
442            cleanUpSpeedMatcher();
443        }
444    }
445    //</editor-fold>
446
447    //<editor-fold defaultstate="collapsed" desc="Helper Functions">
448    /**
449     * Helper function for speed matching the current speedMatchSpeedTableStep
450     *
451     * @param maxCVValue the maximum allowable value for the CV
452     * @param minCVValue the minimum allowable value for the CV
453     * @param nextState  the SpeedMatcherState to advance to if speed matching
454     *                   is complete
455     */
456    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState) {
457        speedMatchSpeedStepInner(maxCVValue, minCVValue, nextState, false);
458    }
459
460    /**
461     * Helper function for speed matching the current speedMatchSpeedTableStep
462     *
463     * @param maxCVValue     the maximum allowable value for the CV
464     * @param minCVValue     the minimum allowable value for the CV
465     * @param nextState      the SpeedMatcherState to advance to if speed
466     *                       matching is complete
467     * @param forceNextState set to true to force speedMatcherState to the next
468     *                       state when speed matching the current
469     *                       speedMatchSpeedTableStep is complete
470     */
471    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState, boolean forceNextState) {
472        if (stepDuration == 0) {
473            speedStepTargetSpeedKPH = getSpeedStepScaleSpeedInKPH(speedMatchSpeedTableStep.getSpeedStep());
474
475            statusLabel.setText(Bundle.getMessage("StatSettingSpeed", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
476            logger.info("Setting CV {} (speed step {}) to {} KPH ({} MPH)", speedMatchSpeedTableStep.getCV(), speedMatchSpeedTableStep.getSpeedStep(), String.valueOf(speedStepTargetSpeedKPH), String.valueOf(Speed.kphToMph(speedStepTargetSpeedKPH)));
477
478            setThrottle(true, speedMatchSpeedTableStep.getSpeedStep());
479
480            writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
481
482            setSpeedMatchStateTimerDuration(speedMatchSpeedTableStep == SpeedTableStep.STEP1 ? 15000: 8000);
483            stepDuration = 1;
484        } else {
485            setSpeedMatchError(speedStepTargetSpeedKPH);
486
487            if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
488                lastSpeedTableStepCVValue = speedMatchCVValue;
489
490                if (forceNextState) {
491                    initNextSpeedMatcherState(nextState);
492                    return;
493                }
494
495                speedMatchSpeedTableStep = speedMatchSpeedTableStep.getPrevious();
496
497                if (speedMatchSpeedTableStep != null) {
498                    if (speedMatchSpeedTableStep == SpeedTableStep.STEP1) {
499                        initNextSpeedMatcherState(speedMatcherState, 3);
500                    }
501                    else {
502                        initNextSpeedMatcherState(speedMatcherState);
503                    }
504                } else {
505                    initNextSpeedMatcherState(nextState);
506                }
507            } else {
508                speedMatchCVValue = getNextSpeedMatchValue(lastSpeedMatchCVValue, maxCVValue, minCVValue);
509
510                if (((speedMatchCVValue == maxCVValue) || (speedMatchCVValue == minCVValue)) && (speedMatchCVValue == lastSpeedMatchCVValue)) {
511                    statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
512                    logger.info("Unable to achieve desired speed for CV {} (Speed Step {})", speedMatchSpeedTableStep.getCV(), String.valueOf(speedMatchSpeedTableStep.getSpeedStep()));
513                    abort();
514                    return;
515                }
516
517                lastSpeedMatchCVValue = speedMatchCVValue;
518                writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
519            }
520        }
521    }
522
523    /**
524     * Aborts the speed matching process programmatically
525     */
526    private void abort() {
527        initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
528    }
529
530    /**
531     * Stops the speed matching process due to user input
532     */
533    private void userStop() {
534        initNextSpeedMatcherState(SpeedMatcherState.USER_STOPPED);
535    }
536
537    /**
538     * Sets up the speed match state by resetting the speed matcher with a value delta of 10,
539     * clearing the step duration, setting the timer duration, and setting the next state
540     *
541     * @param nextState next SpeedMatcherState to set
542     */
543    protected void initNextSpeedMatcherState(SpeedMatcherState nextState) {
544        initNextSpeedMatcherState(nextState, 10); 
545    }
546    
547    /**
548     * Sets up the speed match state by resetting the speed matcher with the given value delta,
549     * clearing the step duration, setting the timer duration, and setting the next state
550     *
551     * @param nextState next SpeedMatcherState to set
552     * @param speedMatchValueDelta the value delta to use when resetting the speed matcher
553     */
554    protected void initNextSpeedMatcherState(SpeedMatcherState nextState, int speedMatchValueDelta) {
555        resetSpeedMatcher(speedMatchValueDelta);
556        stepDuration = 0;
557        speedMatcherState = nextState;
558        setSpeedMatchStateTimerDuration(1800);
559    }
560    //</editor-fold>
561
562    //debugging logger
563    private final static org.slf4j.Logger logger = org.slf4j.LoggerFactory.getLogger(SpeedStepScaleSpeedTableSpeedMatcher.class);
564}