Source code

001package jmri.jmrix.bachrus.speedmatcher.basic;
002
003import jmri.DccThrottle;
004import jmri.jmrix.bachrus.Speed;
005
006/**
007 * This is a simple speed matcher which will speed match a locomotive to a given
008 * start and top speed using the VStart, VMid, and VHigh CVs.
009 *
010 * @author Todd Wegter Copyright (C) 2024
011 */
012public class BasicSimpleCVSpeedMatcher extends BasicSpeedMatcher {
013
014    //<editor-fold defaultstate="collapsed" desc="Constants">
015    private final int INITIAL_VSTART = 1;
016    private final int INITIAL_VMID = 2;
017    private final int INITIAL_VHIGH = 255;
018    private final int INITIAL_TRIM = 128;
019
020    private final int VHIGH_MAX = 255;
021    private final int VHIGH_MIN = INITIAL_VMID + 1;
022    private final int VMID_MIN = INITIAL_VSTART + 1;
023    private final int VSTART_MIN = 1;
024    //</editor-fold>
025
026    //<editor-fold defaultstate="collapsed" desc="Enums">
027    protected enum SpeedMatcherState {
028        IDLE,
029        WAIT_FOR_THROTTLE,
030        INIT_THROTTLE,
031        INIT_ACCEL,
032        INIT_DECEL,
033        INIT_VSTART,
034        INIT_VMID,
035        INIT_VHIGH,
036        INIT_FORWARD_TRIM,
037        INIT_REVERSE_TRIM,
038        POST_INIT,
039        FORWARD_WARM_UP,
040        FORWARD_SPEED_MATCH_VHIGH,
041        FORWARD_SPEED_MATCH_VMID,
042        FORWARD_SPEED_MATCH_VSTART,
043        REVERSE_WARM_UP,
044        REVERSE_SPEED_MATCH_TRIM,
045        COMPLETE,
046        USER_STOPPED,
047        CLEAN_UP,
048    }
049    //</editor-fold>
050
051    //<editor-fold defaultstate="collapsed" desc="Instance Variables">
052    private int vHigh = INITIAL_VHIGH;
053    private int lastVHigh = INITIAL_VHIGH;
054    private int vMid = INITIAL_VSTART;
055    private int lastVMid = INITIAL_VSTART;
056    private int vMidMax;
057    private int vStart = INITIAL_VSTART;
058    private int lastVStart = INITIAL_VSTART;
059    private int vStartMax;
060    private int reverseTrimValue = INITIAL_TRIM;
061    private int lastReverseTrimValue = INITIAL_TRIM;
062
063    private final float targetMidSpeedKPH;
064
065    private SpeedMatcherState speedMatcherState = SpeedMatcherState.IDLE;
066    //</editor-fold>
067
068    /**
069     * Constructs the BasicSimpleCVSpeedMatcher from a BasicSpeedMatcherConfig
070     *
071     * @param config BasicSpeedMatcherConfig
072     */
073    public BasicSimpleCVSpeedMatcher(BasicSpeedMatcherConfig config) {
074        super(config);
075
076        this.targetMidSpeedKPH = this.targetStartSpeedKPH + ((this.targetTopSpeedKPH - this.targetStartSpeedKPH) / 2);
077    }
078
079    //<editor-fold defaultstate="collapsed" desc="SpeedMatcher Overrides">
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        vStart = INITIAL_VSTART;
093        lastVStart = INITIAL_VSTART;
094        vMid = INITIAL_VSTART;
095        lastVMid = INITIAL_VSTART;
096        vHigh = INITIAL_VHIGH;
097        lastVHigh = INITIAL_VHIGH;
098        reverseTrimValue = INITIAL_TRIM;
099        lastReverseTrimValue = INITIAL_TRIM;
100
101        speedMatcherState = SpeedMatcherState.WAIT_FOR_THROTTLE;
102
103        if (!initializeAndStartSpeedMatcher(e -> speedMatchTimeout())) {
104            cleanUpSpeedMatcher();
105            return false;
106        }
107
108        startStopButton.setText(Bundle.getMessage("SpeedMatchStopBtn"));
109
110        return true;
111    }
112
113    /**
114     * Stops the speed matching process
115     */
116    @Override
117    public void stopSpeedMatcher() {
118        if (!isSpeedMatcherIdle()) {
119            logger.info("Speed matching manually stopped");
120            userStop();
121        } else {
122            cleanUpSpeedMatcher();
123        }
124    }
125
126    /**
127     * Indicates if the speed matcher is idle (not currently speed matching)
128     *
129     * @return true if idle, false otherwise
130     */
131    @Override
132    public boolean isSpeedMatcherIdle() {
133        return speedMatcherState == SpeedMatcherState.IDLE;
134    }
135
136    /**
137     * Cleans up the speed matcher when speed matching is stopped or is finished
138     */
139    @Override
140    protected void cleanUpSpeedMatcher() {
141        speedMatcherState = SpeedMatcherState.IDLE;
142
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_VSTART);
179                }
180                break;
181
182            case INIT_VSTART:
183                //set vStart to 0 (CV 2)
184                if (programmerState == ProgrammerState.IDLE) {
185                    writeVStart(INITIAL_VSTART);
186                    initNextSpeedMatcherState(SpeedMatcherState.INIT_VMID);
187                }
188                break;
189
190            case INIT_VMID:
191                //set vMid to 1 (CV 6)
192                if (programmerState == ProgrammerState.IDLE) {
193                    writeVMid(INITIAL_VMID);
194                    initNextSpeedMatcherState(SpeedMatcherState.INIT_VHIGH);
195                }
196                break;
197
198            case INIT_VHIGH:
199                //set vHigh to 255 (CV 5)
200                if (programmerState == ProgrammerState.IDLE) {
201                    writeVHigh(INITIAL_VHIGH);
202                    initNextSpeedMatcherState(SpeedMatcherState.INIT_FORWARD_TRIM);
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.FORWARD_SPEED_MATCH_VHIGH;
234                }
235                initNextSpeedMatcherState(nextState, 30);
236                break;
237            }
238
239            case FORWARD_WARM_UP:
240                //Run specified forward warm up time at high speed forward
241                statusLabel.setText(Bundle.getMessage("StatForwardWarmUp", warmUpForwardSeconds - stepDuration));
242
243                if (stepDuration >= warmUpForwardSeconds) {
244                    initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_VHIGH, 30);
245                } else {
246                    if (stepDuration == 0) {
247                        setSpeedMatchStateTimerDuration(5000);
248                        setThrottle(true, 28);
249                    }
250                    stepDuration += 5;
251                }
252                break;
253
254            case FORWARD_SPEED_MATCH_VHIGH:
255                //Use PID Controller logic to adjust vHigh to achieve desired speed
256                if (programmerState == ProgrammerState.IDLE) {
257                    if (stepDuration == 0) {
258                        statusLabel.setText(Bundle.getMessage("StatSettingSpeed", "5 (vHigh)"));
259                        logger.info("Setting CV 5 (vHigh) to {} KPH ({} MPH)", String.valueOf(targetTopSpeedKPH), String.valueOf(Speed.kphToMph(targetTopSpeedKPH)));
260                        setThrottle(true, 28);
261                        setSpeedMatchStateTimerDuration(8000);
262                        stepDuration = 1;
263                    } else {
264                        setSpeedMatchError(targetTopSpeedKPH);
265
266                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
267                            initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_VMID);
268                        } else {
269                            vHigh = getNextSpeedMatchValue(lastVHigh, VHIGH_MAX, VHIGH_MIN);
270
271                            if (((vHigh == VHIGH_MAX) || (vHigh == VHIGH_MIN)) && (vHigh == lastVHigh)) {
272                                statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", "5 (vHigh)"));
273                                logger.info("Unable to achieve desired speed for CV 5 (vHigh)");
274                                abort();
275                                break;
276                            }
277
278                            lastVHigh = vHigh;
279                            writeVHigh(vHigh);
280                        }
281                    }
282                }
283                break;
284
285            case FORWARD_SPEED_MATCH_VMID:
286                //Use PID Controller logic to adjust vMid to achieve desired speed
287                if (programmerState == ProgrammerState.IDLE) {
288                    if (stepDuration == 0) {
289                        vMid = INITIAL_VSTART + ((vHigh - INITIAL_VSTART) / 2);
290                        lastVMid = vMid;
291                        vMidMax = vHigh - 1;
292                        writeVMid(vMid);
293
294                        statusLabel.setText(Bundle.getMessage("StatSettingSpeed", "6 (vMid)"));
295                        logger.info("Setting CV 6 (vMid) to {} KPH ({} MPH)", String.valueOf(targetMidSpeedKPH), String.valueOf(Speed.kphToMph(targetMidSpeedKPH)));
296                        setSpeedMatchStateTimerDuration(8000);
297                        setThrottle(true, 14);
298                        stepDuration = 1;
299
300                    } else {
301                        setSpeedMatchError(targetMidSpeedKPH);
302
303                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
304                            initNextSpeedMatcherState(SpeedMatcherState.FORWARD_SPEED_MATCH_VSTART, 3);
305                        } else {
306                            vMid = getNextSpeedMatchValue(lastVMid, vMidMax, VMID_MIN);
307
308                            if (((vMid == vMidMax) || (vMid == VMID_MIN)) && (vMid == lastVMid)) {
309                                statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", "6 (vMid)"));
310                                logger.info("Unable to achieve desired speed for CV 6 (vMid)");
311                                abort();
312                                break;
313                            }
314
315                            lastVMid = vMid;
316                            writeVMid(vMid);
317                        }
318                    }
319                }
320                break;
321
322            case FORWARD_SPEED_MATCH_VSTART: {
323                //Use PID Controller to adjust vStart to achieve desired speed
324                if (programmerState == ProgrammerState.IDLE) {
325                    if (stepDuration == 0) {
326                        vStartMax = vMid - 1;
327                        statusLabel.setText(Bundle.getMessage("StatSettingSpeed", "2 (vStart)"));
328                        logger.info("Setting CV 2 (vStart) to {} KPH ({} MPH)", String.valueOf(targetStartSpeedKPH), String.valueOf(Speed.kphToMph(targetStartSpeedKPH)));
329                        setThrottle(true, 1);
330                        setSpeedMatchStateTimerDuration(15000);
331                        stepDuration = 1;
332                    } else {
333                        setSpeedMatchError(targetStartSpeedKPH);
334
335                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
336                            SpeedMatcherState nextState;
337                            if (trimReverseSpeed) {
338                                if (warmUpReverseSeconds > 0) {
339                                    nextState = SpeedMatcherState.REVERSE_WARM_UP;
340                                } else {
341                                    nextState = SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM;
342                                }
343                            } else {
344                                nextState = SpeedMatcherState.COMPLETE;
345                            }
346                            initNextSpeedMatcherState(nextState);
347                        } else {
348                            vStart = getNextSpeedMatchValue(lastVStart, vStartMax, VSTART_MIN);
349
350                            if (((vStart == vStartMax) || (vStart == VSTART_MIN)) && (vStart == lastVStart)) {
351                                statusLabel.setText(Bundle.getMessage("StatSetSpeedFail", "2 (vStart)"));
352                                logger.info("Unable to achieve desired speed for CV 2 (vStart)");
353                                abort();
354                                break;
355                            }
356
357                            lastVStart = vStart;
358                            writeVStart(vStart);
359                        }
360                    }
361                }
362                break;
363            }
364
365            case REVERSE_WARM_UP:
366                //Run specified reverse warm up time at high speed in reverse
367                statusLabel.setText(Bundle.getMessage("StatReverseWarmUp", warmUpReverseSeconds - stepDuration));
368
369                if (stepDuration >= warmUpReverseSeconds) {
370                    initNextSpeedMatcherState(SpeedMatcherState.REVERSE_SPEED_MATCH_TRIM);
371                } else {
372                    if (stepDuration == 0) {
373                        setSpeedMatchStateTimerDuration(5000);
374                        setThrottle(false, 28);
375                    }
376                    stepDuration += 5;
377                }
378
379                break;
380
381            case REVERSE_SPEED_MATCH_TRIM:
382                //Use PID controller logic to adjust reverse trim until high speed reverse speed matches forward
383                if (programmerState == ProgrammerState.IDLE) {
384                    if (stepDuration == 0) {
385                        statusLabel.setText(Bundle.getMessage("StatSettingReverseTrim"));
386                        setThrottle(false, 28);
387                        setSpeedMatchStateTimerDuration(8000);
388                        stepDuration = 1;
389                    } else {
390                        setSpeedMatchError(targetTopSpeedKPH);
391
392                        if (Math.abs(speedMatchError) < ALLOWED_SPEED_MATCH_ERROR) {
393                            initNextSpeedMatcherState(SpeedMatcherState.COMPLETE);
394                        } else {
395                            reverseTrimValue = getNextSpeedMatchValue(lastReverseTrimValue, REVERSE_TRIM_MAX, REVERSE_TRIM_MIN);
396
397                            if (((lastReverseTrimValue == REVERSE_TRIM_MAX) || (lastReverseTrimValue == REVERSE_TRIM_MIN)) && (reverseTrimValue == lastReverseTrimValue)) {
398                                statusLabel.setText(Bundle.getMessage("StatSetReverseTrimFail"));
399                                logger.info("Unable to trim reverse to match forward");
400                                abort();
401                                break;
402                            }
403
404                            lastReverseTrimValue = reverseTrimValue;
405                            writeReverseTrim(reverseTrimValue);
406                        }
407                    }
408                }
409                break;
410
411            case COMPLETE:
412                if (programmerState == ProgrammerState.IDLE) {
413                    statusLabel.setText(Bundle.getMessage("StatSpeedMatchComplete"));
414                    setThrottle(true, 0);
415                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
416                }
417                break;
418
419            case USER_STOPPED:
420                if (programmerState == ProgrammerState.IDLE) {
421                    statusLabel.setText(Bundle.getMessage("StatUserStoppedSpeedMatch"));
422                    setThrottle(true, 0);
423                    initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
424                }
425                break;
426
427            case CLEAN_UP:
428                //wrap it up
429                if (programmerState == ProgrammerState.IDLE) {
430                    cleanUpSpeedMatcher();
431                }
432                break;
433
434            default:
435                cleanUpSpeedMatcher();
436                logger.error("Unexpected speed match timeout");
437                break;
438        }
439
440        if (speedMatcherState != SpeedMatcherState.IDLE) {
441            startSpeedMatchStateTimer();
442        }
443    }
444    //</editor-fold>
445
446    //<editor-fold defaultstate="collapsed" desc="ThrottleListener Overrides">
447    /**
448     * Called when a throttle is found
449     *
450     * @param t the requested DccThrottle
451     */
452    @Override
453    public void notifyThrottleFound(DccThrottle t) {
454        super.notifyThrottleFound(t);
455
456        if (speedMatcherState == SpeedMatcherState.WAIT_FOR_THROTTLE) {
457            logger.info("Starting speed matching");
458            // using speed matching timer to trigger each phase of speed matching            
459            initNextSpeedMatcherState(SpeedMatcherState.INIT_THROTTLE);
460            startSpeedMatchStateTimer();
461        } else {
462            cleanUpSpeedMatcher();
463        }
464    }
465    //</editor-fold>
466
467    //<editor-fold defaultstate="collapsed" desc="Helper Functions">
468    /**
469     * Aborts the speed matching process programmatically
470     */
471    private void abort() {
472        initNextSpeedMatcherState(SpeedMatcherState.CLEAN_UP);
473    }
474
475    /**
476     * Stops the speed matching process due to user input
477     */
478    private void userStop() {
479        initNextSpeedMatcherState(SpeedMatcherState.USER_STOPPED);
480    }
481
482    /**
483     * Sets up the speed match state by resetting the speed matcher with a value delta of 10,
484     * clearing the step duration, setting the timer duration, and setting the next state
485     *
486     * @param nextState next SpeedMatcherState to set
487     */
488    protected void initNextSpeedMatcherState(SpeedMatcherState nextState) {
489        initNextSpeedMatcherState(nextState, 10); 
490    }
491    
492    /**
493     * Sets up the speed match state by resetting the speed matcher with the given value delta,
494     * clearing the step duration, setting the timer duration, and setting the next state
495     *
496     * @param nextState next SpeedMatcherState to set
497     * @param speedMatchValueDelta the value delta to use when resetting the speed matcher
498     */
499    protected void initNextSpeedMatcherState(SpeedMatcherState nextState, int speedMatchValueDelta) {
500        resetSpeedMatcher(speedMatchValueDelta);
501        stepDuration = 0;
502        speedMatcherState = nextState;
503        setSpeedMatchStateTimerDuration(1800);
504    }
505    //</editor-fold>
506
507    //debugging logger
508    private final static org.slf4j.Logger logger = org.slf4j.LoggerFactory.getLogger(BasicSimpleCVSpeedMatcher.class);
509}