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}