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 speedMatchMaxSpeedStep;
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
066    private SpeedMatcherState speedMatcherState = SpeedMatcherState.IDLE;
067    //</editor-fold>
068
069    /**
070     * Constructs the SpeedStepScaleSpeedTableSpeedMatcher from a
071     * SpeedStepScaleSpeedMatcherConfig
072     *
073     * @param config SpeedStepScaleSpeedMatcherConfig
074     */
075    public SpeedStepScaleSpeedTableSpeedMatcher(SpeedStepScaleSpeedMatcherConfig config) {
076        super(config);
077    }
078
079    //<editor-fold defaultstate="collapsed" desc="SpeedMatcherOverrides">
080    /**
081     * Starts the speed matching process
082     *
083     * @return true if speed matching started successfully, false otherwise
084     */
085    @Override
086    public boolean startSpeedMatcher() {
087        if (!validate()) {
088            return false;
089        }
090
091        //reset instance variables
092        speedMatchCVValue = INITIAL_STEP28;
093        lastSpeedMatchCVValue = INITIAL_STEP28;
094        lastSpeedTableStepCVValue = INITIAL_STEP28;
095        reverseTrimValue = INITIAL_TRIM;
096        lastReverseTrimValue = INITIAL_TRIM;
097        measuredMaxSpeedKPH = 0;
098        speedMatchMaxSpeedKPH = 0;
099
100        speedMatcherState = SpeedMatcherState.WAIT_FOR_THROTTLE;
101
102        actualMaxSpeedField.setText(String.format("___"));
103
104        if (!initializeAndStartSpeedMatcher(e -> speedMatchTimeout())) {
105            cleanUpSpeedMatcher();
106            return false;
107        }
108
109        startStopButton.setText(Bundle.getMessage("SpeedMatchStopBtn"));
110
111        return true;
112    }
113
114    /**
115     * Stops the speed matching process
116     */
117    @Override
118    public void stopSpeedMatcher() {
119        if (!isSpeedMatcherIdle()) {
120            logger.info("Speed matching manually stopped");
121            userStop();
122        } else {
123            cleanUpSpeedMatcher();
124        }
125    }
126
127    /**
128     * Indicates if the speed matcher is idle (not currently speed matching)
129     *
130     * @return true if idle, false otherwise
131     */
132    @Override
133    public boolean isSpeedMatcherIdle() {
134        return speedMatcherState == SpeedMatcherState.IDLE;
135    }
136
137    /**
138     * Cleans up the speed matcher when speed matching is stopped or is finished
139     */
140    @Override
141    protected void cleanUpSpeedMatcher() {
142        speedMatcherState = SpeedMatcherState.IDLE;
143        super.cleanUpSpeedMatcher();
144    }
145    //</editor-fold>
146
147    //<editor-fold defaultstate="collapsed" desc="Speed Matcher State">
148    /**
149     * Main speed matching timeout handler. This is the state machine that
150     * effectively does the speed matching process.
151     */
152    private synchronized void speedMatchTimeout() {
153        switch (speedMatcherState) {
154            case WAIT_FOR_THROTTLE:
155                cleanUpSpeedMatcher();
156                logger.error("Timeout waiting for throttle");
157                statusLabel.setText(Bundle.getMessage("StatusTimeout"));
158                break;
159
160            case INIT_THROTTLE:
161                //set throttle to 0 for init
162                setThrottle(true, 0);
163                initNextSpeedMatcherState(SpeedMatcherState.INIT_ACCEL);
164                break;
165
166            case INIT_ACCEL:
167                //set acceleration momentum to 0 (CV 3)
168                if (programmerState == ProgrammerState.IDLE) {
169                    writeMomentumAccel(INITIAL_MOMENTUM);
170                    initNextSpeedMatcherState(SpeedMatcherState.INIT_DECEL);
171                }
172                break;
173
174            case INIT_DECEL:
175                //set deceleration mementum to 0 (CV 4)
176                if (programmerState == ProgrammerState.IDLE) {
177                    writeMomentumDecel(INITIAL_MOMENTUM);
178                    initNextSpeedMatcherState(SpeedMatcherState.INIT_SPEED_TABLE);
179                }
180                break;
181
182            case INIT_SPEED_TABLE:
183                //initialize every speed table step to 1 except speed table step 28 = 255
184                if (programmerState == ProgrammerState.IDLE) {
185                    if (stepDuration == 0) {
186                        initSpeedTableStepValue = INITIAL_STEP1;
187                        initSpeedTableStep = SpeedTableStep.STEP1;
188                        stepDuration = 1;
189                    }
190
191                    if (initSpeedTableStep == SpeedTableStep.STEP28) {
192                        initSpeedTableStepValue = INITIAL_STEP28;
193                    }
194
195                    writeSpeedTableStep(initSpeedTableStep, initSpeedTableStepValue);
196
197                    initSpeedTableStep = initSpeedTableStep.getNext();
198                    if (initSpeedTableStep == null) {
199                        initNextSpeedMatcherState(SpeedMatcherState.INIT_FORWARD_TRIM);
200                    }
201                }
202                break;
203
204            case INIT_FORWARD_TRIM:
205                //set forward trim to 128 (CV 66)
206                if (programmerState == ProgrammerState.IDLE) {
207                    writeForwardTrim(INITIAL_TRIM);
208                    initNextSpeedMatcherState(SpeedMatcherState.INIT_REVERSE_TRIM);
209                }
210                break;
211
212            case INIT_REVERSE_TRIM:
213                //set reverse trim to 128 (CV 95)
214                if (programmerState == ProgrammerState.IDLE) {
215                    writeReverseTrim(INITIAL_TRIM);
216                    initNextSpeedMatcherState(SpeedMatcherState.POST_INIT);
217                }
218                break;
219
220            case POST_INIT: {
221                statusLabel.setText(Bundle.getMessage("StatRestoreThrottle"));
222
223                //un-brick Digitrax decoders
224                setThrottle(false, 0);
225                setThrottle(true, 0);
226
227                SpeedMatcherState nextState;
228                if (warmUpForwardSeconds > 0) {
229                    nextState = SpeedMatcherState.FORWARD_WARM_UP;
230                } else {
231                    nextState = SpeedMatcherState.READ_MAX_SPEED;
232                }
233                initNextSpeedMatcherState(nextState);
234                break;
235            }
236
237            case FORWARD_WARM_UP:
238                //Run 4 minutes at high speed forward
239                statusLabel.setText(Bundle.getMessage("StatForwardWarmUp", warmUpForwardSeconds - stepDuration));
240
241                if (stepDuration >= warmUpForwardSeconds) {
242                    initNextSpeedMatcherState(SpeedMatcherState.READ_MAX_SPEED);
243                } else {
244                    if (stepDuration == 0) {
245                        setSpeedMatchStateTimerDuration(5000);
246                        setThrottle(true, 28);
247                    }
248                    stepDuration += 5;
249                }
250                break;
251
252            case READ_MAX_SPEED:
253                //Run 10 second at high speed forward and record the speed
254                if (stepDuration == 0) {
255                    statusLabel.setText("Recording locomotive's maximum speed");
256                    setSpeedMatchStateTimerDuration(10000);
257                    setThrottle(true, 28);
258                    stepDuration = 1;
259                } else {
260                    measuredMaxSpeedKPH = currentSpeedKPH;
261
262                    String statusMessage = String.format("Measured maximum speed = %.1f KPH (%.1f MPH)", measuredMaxSpeedKPH, Speed.kphToMph(measuredMaxSpeedKPH));
263                    logger.info(statusMessage);
264                    
265                    float speedMatchMaxSpeed;
266
267                    if (measuredMaxSpeedKPH > targetMaxSpeedKPH) {
268                        speedMatchMaxSpeedStep = targetMaxSpeedStep.getSpeedTableStep().getSpeedStep();
269                        speedMatchMaxSpeed = targetMaxSpeedStep.getSpeed();
270                        speedMatchMaxSpeedKPH = targetMaxSpeedKPH;
271                    } else {
272                        float measuredMaxSpeed = speedUnit == Speed.Unit.MPH ? Speed.kphToMph(measuredMaxSpeedKPH) : measuredMaxSpeedKPH;
273                        speedMatchMaxSpeedStep = getNextLowestSpeedTableStepForSpeed(measuredMaxSpeed);
274                        speedMatchMaxSpeed = getSpeedForSpeedTableStep(speedMatchMaxSpeedStep);
275                        speedMatchMaxSpeedKPH = speedUnit == Speed.Unit.MPH ? Speed.mphToKph(speedMatchMaxSpeed): speedMatchMaxSpeed;
276                    }
277                    
278                    actualMaxSpeedField.setText(String.format("%.1f", speedMatchMaxSpeed));
279                    
280                    initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_STEP28);
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, speedMatchMaxSpeedStep, 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() < speedMatchMaxSpeedStep) {
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                        setThrottle(false, speedMatchMaxSpeedStep);
363                        setSpeedMatchStateTimerDuration(8000);
364                        stepDuration = 1;
365                    } else {
366                        setSpeedMatchError(speedMatchMaxSpeedKPH);
367
368                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
369                            initNextSpeedMatcherState(SpeedMatcherState.COMPLETE);
370                        } else {
371                            reverseTrimValue = getNextSpeedMatchValue(lastReverseTrimValue, REVERSE_TRIM_MAX, REVERSE_TRIM_MIN);
372
373                            if (((lastReverseTrimValue == REVERSE_TRIM_MAX) || (lastReverseTrimValue == REVERSE_TRIM_MIN)) && (reverseTrimValue == lastReverseTrimValue)) {
374                                statusLabel.setText(Bundle.getMessage("StatSetReverseTrimFail"));
375                                logger.info("Unable to trim reverse to match forward");
376                                abort();
377                                break;
378                            }
379
380                            lastReverseTrimValue = reverseTrimValue;
381                            writeReverseTrim(reverseTrimValue);
382                        }
383                    }
384                }
385                break;
386
387            case COMPLETE:
388                if (programmerState == ProgrammerState.IDLE) {
389                    statusLabel.setText(Bundle.getMessage("StatSpeedMatchComplete"));
390                    setThrottle(true, 0);
391                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
392                }
393                break;
394
395            case USER_STOPPED:
396                if (programmerState == ProgrammerState.IDLE) {
397                    statusLabel.setText(Bundle.getMessage("StatUserStoppedSpeedMatch"));
398                    setThrottle(true, 0);
399                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
400                }
401                break;
402
403            case CLEAN_UP:
404                //wrap it up
405                if (programmerState == ProgrammerState.IDLE) {
406                    cleanUpSpeedMatcher();
407                }
408                break;
409
410            default:
411                cleanUpSpeedMatcher();
412                logger.error("Unexpected speed match timeout");
413                break;
414        }
415
416        if (speedMatcherState != SpeedMatcherState.IDLE) {
417            startSpeedMatchStateTimer();
418        }
419    }
420    //</editor-fold>
421
422    //<editor-fold defaultstate="collapsed" desc="ThrottleListener Overrides">
423    /**
424     * Called when a throttle is found
425     *
426     * @param t the requested DccThrottle
427     */
428    @Override
429    public void notifyThrottleFound(DccThrottle t) {
430        super.notifyThrottleFound(t);
431
432        if (speedMatcherState == SpeedMatcherState.WAIT_FOR_THROTTLE) {
433            logger.info("Starting speed matching");
434            // using speed matching timer to trigger each phase of speed matching            
435            initNextSpeedMatcherState(SpeedMatcherState.INIT_THROTTLE);
436            startSpeedMatchStateTimer();
437        } else {
438            cleanUpSpeedMatcher();
439        }
440    }
441    //</editor-fold>
442
443    //<editor-fold defaultstate="collapsed" desc="Helper Functions">
444    /**
445     * Helper function for speed matching the current speedMatchSpeedTableStep
446     *
447     * @param maxCVValue the maximum allowable value for the CV
448     * @param minCVValue the minimum allowable value for the CV
449     * @param nextState  the SpeedMatcherState to advance to if speed matching
450     *                   is complete
451     */
452    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState) {
453        speedMatchSpeedStepInner(maxCVValue, minCVValue, nextState, false);
454    }
455
456    /**
457     * Helper function for speed matching the current speedMatchSpeedTableStep
458     *
459     * @param maxCVValue     the maximum allowable value for the CV
460     * @param minCVValue     the minimum allowable value for the CV
461     * @param nextState      the SpeedMatcherState to advance to if speed
462     *                       matching is complete
463     * @param forceNextState set to true to force speedMatcherState to the next
464     *                       state when speed matching the current
465     *                       speedMatchSpeedTableStep is complete
466     */
467    private void speedMatchSpeedStepInner(int maxCVValue, int minCVValue, SpeedMatcherState nextState, boolean forceNextState) {
468        if (stepDuration == 0) {
469            speedStepTargetSpeedKPH = getSpeedStepScaleSpeedInKPH(speedMatchSpeedTableStep.getSpeedStep());
470
471            statusLabel.setText(Bundle.getMessage("StatSettingSpeed", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
472            logger.info("Setting CV {} (speed step {}) to {} KPH ({} MPH)", speedMatchSpeedTableStep.getCV(), speedMatchSpeedTableStep.getSpeedStep(), String.valueOf(speedStepTargetSpeedKPH), String.valueOf(Speed.kphToMph(speedStepTargetSpeedKPH)));
473
474            setThrottle(true, speedMatchSpeedTableStep.getSpeedStep());
475
476            writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
477
478            setSpeedMatchStateTimerDuration(8000);
479            stepDuration = 1;
480        } else {
481            setSpeedMatchError(speedStepTargetSpeedKPH);
482
483            if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
484                lastSpeedTableStepCVValue = speedMatchCVValue;
485
486                if (forceNextState) {
487                    initNextSpeedMatcherState(nextState);
488                    return;
489                }
490
491                speedMatchSpeedTableStep = speedMatchSpeedTableStep.getPrevious();
492
493                if (speedMatchSpeedTableStep != null) {
494                    initNextSpeedMatcherState(speedMatcherState);
495                } else {
496                    initNextSpeedMatcherState(nextState);
497                }
498            } else {
499                speedMatchCVValue = getNextSpeedMatchValue(lastSpeedMatchCVValue, maxCVValue, minCVValue);
500
501                if (((speedMatchCVValue == maxCVValue) || (speedMatchCVValue == minCVValue)) && (speedMatchCVValue == lastSpeedMatchCVValue)) {
502                    statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", speedMatchSpeedTableStep.getCV() + " (Speed Step " + String.valueOf(speedMatchSpeedTableStep.getSpeedStep()) + ")"));
503                    logger.info("Unable to achieve desired speed for CV {} (Speed Step {})", speedMatchSpeedTableStep.getCV(), String.valueOf(speedMatchSpeedTableStep.getSpeedStep()));
504                    abort();
505                    return;
506                }
507
508                lastSpeedMatchCVValue = speedMatchCVValue;
509                writeSpeedTableStep(speedMatchSpeedTableStep, speedMatchCVValue);
510            }
511        }
512    }
513
514    /**
515     * Aborts the speed matching process programmatically
516     */
517    private void abort() {
518        initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
519    }
520
521    /**
522     * Stops the speed matching process due to user input
523     */
524    private void userStop() {
525        initNextSpeedMatcherState(SpeedMatcherState.USER_STOPPED);
526    }
527
528    /**
529     * Sets up the speed match state by clearing the speed match error, clearing
530     * the step duration, setting the timer duration, and setting the next state
531     *
532     * @param nextState next SpeedMatcherState to set
533     */
534    protected void initNextSpeedMatcherState(SpeedMatcherState nextState) {
535        resetSpeedMatchError();
536        stepDuration = 0;
537        speedMatcherState = nextState;
538        setSpeedMatchStateTimerDuration(1800);
539    }
540    //</editor-fold>
541
542    //debugging logger
543    private final static org.slf4j.Logger logger = org.slf4j.LoggerFactory.getLogger(SpeedStepScaleSpeedTableSpeedMatcher.class);
544}