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); 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); 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); 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(8000); 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 clearing the speed match error, clearing 484 * 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 resetSpeedMatchError(); 490 stepDuration = 0; 491 speedMatcherState = nextState; 492 setSpeedMatchStateTimerDuration(1800); 493 } 494 //</editor-fold> 495 496 //debugging logger 497 private final static org.slf4j.Logger logger = org.slf4j.LoggerFactory.getLogger(BasicSimpleCVSpeedMatcher.class); 498}