001package jmri.jmrit.roster; 002 003import java.util.ArrayList; 004import java.util.LinkedList; 005import java.util.List; 006import java.util.Locale; 007import java.util.Map.Entry; 008import java.util.TreeMap; 009 010import javax.annotation.CheckForNull; 011 012import jmri.Block; 013import jmri.DccThrottle; 014import jmri.InstanceManager; 015import jmri.NamedBean; 016import jmri.Section; 017import jmri.implementation.SignalSpeedMap; 018 019import org.jdom2.Element; 020 021/** 022 * A class to store a speed profile for a given loco. 023 * The speed steps against the profile are on a scale of 0 to 1000, 024 * this equates to the float speed x 1000. 025 * This allows a single profile to cover different throttle speed step settings. 026 * A profile generated for a loco using 28 steps can be used for a throttle with 126 steps. 027 */ 028public class RosterSpeedProfile { 029 030 private RosterEntry _re = null; 031 032 private float overRunTimeReverse = 0.0f; 033 private float overRunTimeForward = 0.0f; 034 035 private boolean _hasForwardSpeeds = false; 036 private boolean _hasReverseSpeeds = false; 037 038 /** 039 * Create a new RosterSpeedProfile. 040 * @param re the Roster Entry associated with the profile. 041 */ 042 public RosterSpeedProfile(RosterEntry re) { 043 _re = re; 044 } 045 046 /** 047 * Get the RosterEntry associated with the profile. 048 * @return the RosterEntry. 049 */ 050 public RosterEntry getRosterEntry() { 051 return _re; 052 } 053 054 public float getOverRunTimeForward() { 055 return overRunTimeForward; 056 } 057 058 public void setOverRunTimeForward(float dt) { 059 overRunTimeForward = dt; 060 } 061 062 public float getOverRunTimeReverse() { 063 return overRunTimeReverse; 064 } 065 066 public void setOverRunTimeReverse(float dt) { 067 overRunTimeReverse = dt; 068 } 069 070 public void clearCurrentProfile() { 071 speeds = new TreeMap<>(); 072 } 073 074 public void deleteStep(Integer step) { 075 speeds.remove(step); 076 } 077 078 /** 079 * Check if the Speed Profile contains Forward Speeds. 080 * @return true if forward speeds are present, else false. 081 */ 082 public boolean hasForwardSpeeds() { 083 return _hasForwardSpeeds; 084 } 085 086 /** 087 * Check if the Speed Profile contains Reverse Speeds. 088 * @return true if reverse speeds are present, else false. 089 */ 090 public boolean hasReverseSpeeds() { 091 return _hasReverseSpeeds; 092 } 093 094 /** 095 * place / remove SpeedProfile from test mode. 096 * reinitializes speedstep trace array 097 * @param value true/false 098 */ 099 public void setTestMode(boolean value) { 100 synchronized (this){ 101 profileInTestMode = value; 102 } 103 testSteps = new ArrayList<>(); 104 } 105 106 /** 107 * Gets the speed step trace array. 108 * @return speedstep trace array 109 */ 110 public List<SpeedSetting> getSpeedStepTrace() { 111 return testSteps; 112 } 113 114 /** 115 * Speed conversion Millimetres per second to Miles per hour. 116 */ 117 public static final float MMS_TO_MPH = 0.00223694f; 118 119 /** 120 * Speed conversion Millimetres per second to Kilometres per hour. 121 */ 122 public static final float MMS_TO_KPH = 0.0036f; 123 124 /** 125 * Returns the scale speed. 126 * If Warrant preferences are not a speed, value returns unchanged. 127 * @param mms MilliMetres per second. 128 * @param factorFastClock true to factor in the Fast Clock ratio, else false. 129 * @return scale speed in units specified by Warrant Preferences, 130 * unchanged if Warrant preferences are not a speed. 131 */ 132 public float mmsToScaleSpeed(float mms, boolean factorFastClock) { 133 int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation(); 134 float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale(); 135 float fastClockFactor = ( factorFastClock ? 136 (float)InstanceManager.getDefault(jmri.Timebase.class).userGetRate() : 1 ); 137 138 switch (interp) { 139 case SignalSpeedMap.SPEED_MPH: 140 return mms * scale * MMS_TO_MPH * fastClockFactor; 141 case SignalSpeedMap.SPEED_KMPH: 142 return mms * scale * MMS_TO_KPH * fastClockFactor; 143 case SignalSpeedMap.PERCENT_THROTTLE: 144 case SignalSpeedMap.PERCENT_NORMAL: 145 return mms; 146 default: 147 log.warn("MMSToScaleSpeed: Signal Speed Map is not in a scale speed, not modifing."); 148 return mms; 149 } 150 } 151 152 /** 153 * Returns the scale speed as a numeric. 154 * If Warrant preferences are not a speed, value returns unchanged. 155 * @param mms MilliMetres per second 156 * @return scale speed in units specified by Warrant Preferences, 157 * unchanged if Warrant preferences are not a speed. 158 * @deprecated use {@link #mmsToScaleSpeed(float mms)} 159 */ 160 @Deprecated (since="5.9.6",forRemoval=true) 161 public float MMSToScaleSpeed(float mms) { 162 jmri.util.LoggingUtil.deprecationWarning(log, "MMSToScaleSpeed"); 163 return mmsToScaleSpeed(mms); 164 } 165 166 /** 167 * Returns the scale speed as a numeric. 168 * If Warrant preferences are not a speed, value returns unchanged. 169 * Does not factor Fast Clock ratio. 170 * @param mms MilliMetres per second 171 * @return scale speed in units specified by Warrant Preferences, 172 * unchanged if Warrant preferences are not a speed. 173 */ 174 public float mmsToScaleSpeed(float mms) { 175 return mmsToScaleSpeed(mms, false); 176 } 177 178 /** 179 * Returns the scale speed format as I18N string with the units added given 180 * MilliMetres per Second. 181 * If the warrant preference is a percentage of 182 * normal or throttle will use metres per second. 183 * The Fast Clock Ratio is not used in the calculation. 184 * 185 * @param mms MilliMetres per second 186 * @return a string with scale speed and units 187 */ 188 public static String convertMMSToScaleSpeedWithUnits(float mms) { 189 int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation(); 190 float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale(); 191 String formattedWithUnits; 192 switch (interp) { 193 case SignalSpeedMap.SPEED_MPH: 194 String unitsMph = Bundle.getMessage("mph"); 195 formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms * scale * MMS_TO_MPH, unitsMph); 196 break; 197 case SignalSpeedMap.SPEED_KMPH: 198 String unitsKph = Bundle.getMessage("kph"); 199 formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms * scale * MMS_TO_KPH, unitsKph); 200 break; 201 case SignalSpeedMap.PERCENT_THROTTLE: 202 case SignalSpeedMap.PERCENT_NORMAL: 203 String unitsMms = Bundle.getMessage("mmps"); 204 formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms, unitsMms); 205 break; 206 default: 207 log.warn("ScaleSpeedToMMS: Signal Speed Map has no interp, not modifing."); 208 formattedWithUnits = String.format( Locale.getDefault(), "%.2f", mms); 209 } 210 return formattedWithUnits; 211 } 212 213 /** 214 * Returns the scale speed format as a string with the units added given a 215 * throttle setting. and direction. 216 * The Fast Clock Ratio is not used in the calculation. 217 * 218 * @param throttleSetting as percentage of 1.0 219 * @param isForward true or false 220 * @return a string with scale speed and units 221 */ 222 public String convertThrottleSettingToScaleSpeedWithUnits(float throttleSetting, boolean isForward) { 223 return convertMMSToScaleSpeedWithUnits(getSpeed(throttleSetting, isForward)); 224 } 225 226 /** 227 * MilliMetres per Second given scale speed. 228 * The Fast Clock Ratio is not used in the calculation. 229 * @param scaleSpeed in MPH or KPH 230 * @return MilliMetres per second 231 */ 232 public float convertScaleSpeedToMMS(float scaleSpeed) { 233 int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation(); 234 float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale(); 235 float mmsSpeed; 236 switch (interp) { 237 case SignalSpeedMap.SPEED_MPH: 238 mmsSpeed = scaleSpeed / scale / MMS_TO_MPH; 239 break; 240 case SignalSpeedMap.SPEED_KMPH: 241 mmsSpeed = scaleSpeed / scale / MMS_TO_KPH; 242 break; 243 default: 244 log.warn("ScaleSpeedToMMS: Signal Speed Map is not in a scale speed, not modifing."); 245 mmsSpeed = scaleSpeed; 246 } 247 return mmsSpeed; 248 } 249 250 /** 251 * Converts from signal map speed to a throttle setting. 252 * The Fast Clock Ratio is not used in the calculation. 253 * @param signalMapSpeed value from warrants preferences 254 * @param isForward direction of travel 255 * @return throttle setting 256 */ 257 public float getThrottleSettingFromSignalMapSpeed(float signalMapSpeed, boolean isForward) { 258 int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation(); 259 float throttleSetting = 0.0f; 260 switch (interp) { 261 case SignalSpeedMap.PERCENT_NORMAL: 262 case SignalSpeedMap.PERCENT_THROTTLE: 263 throttleSetting = signalMapSpeed / 100.0f; 264 break; 265 case SignalSpeedMap.SPEED_KMPH: 266 case SignalSpeedMap.SPEED_MPH: 267 throttleSetting = getThrottleSetting(convertScaleSpeedToMMS(signalMapSpeed), isForward); 268 break; 269 default: 270 log.warn("getThrottleSettingFromSignalMapSpeed: Signal Speed Map interp not supported."); 271 } 272 return throttleSetting; 273 } 274 275 /** 276 * Set the speed for the given speed step. 277 * 278 * @param speedStep the speed step to set 279 * @param forward speed in meters per second for running forward at 280 * speedStep 281 * @param reverse speed in meters per second for running in reverse at 282 * speedStep 283 */ 284 public void setSpeed(int speedStep, float forward, float reverse) { 285 SpeedStep ss = speeds.computeIfAbsent(speedStep, k -> new SpeedStep()); 286 ss.setForwardSpeed(forward); 287 ss.setReverseSpeed(reverse); 288 if (forward > 0.0f) { 289 _hasForwardSpeeds = true; 290 } 291 if (reverse > 0.0f) { 292 _hasReverseSpeeds = true; 293 } 294 } 295 296 public SpeedStep getSpeedStep(float speed) { 297 int iSpeedStep = Math.round(speed * 1000); 298 return speeds.get(iSpeedStep); 299 } 300 301 public void setForwardSpeed(float speedStep, float forward) { 302 if (forward > 0.0f) { 303 _hasForwardSpeeds = true; 304 } else { 305 return; 306 } 307 int iSpeedStep = Math.round(speedStep * 1000); 308 speeds.computeIfAbsent(iSpeedStep, k -> new SpeedStep()).setForwardSpeed(forward); 309 } 310 311 /** 312 * Merge raw throttleSetting value with an existing profile SpeedStep if 313 * key for the throttleSetting is within the speedIncrement of the SpeedStep. 314 * @param throttleSetting raw throttle setting value 315 * @param speed track speed 316 * @param speedIncrement throttle's speed step increment. 317 */ 318 public void setForwardSpeed(float throttleSetting, float speed, float speedIncrement) { 319 if (throttleSetting> 0.0f) { 320 _hasForwardSpeeds = true; 321 } else { 322 return; 323 } 324 int key; 325 Entry<Integer, SpeedStep> entry = findEquivalentEntry (throttleSetting, speedIncrement); 326 if (entry != null) { // close keys. i.e. resolve to same throttle step 327 float value = entry.getValue().getForwardSpeed(); 328 speed = (speed + value) / 2; 329 key = entry.getKey(); 330 } else { // nothing close. make new entry 331 key = Math.round(throttleSetting * 1000); 332 } 333 speeds.computeIfAbsent(key, k -> new SpeedStep()).setForwardSpeed(speed); 334 } 335 336 @CheckForNull 337 private Entry<Integer, SpeedStep> findEquivalentEntry (float throttleSetting, float speedIncrement) { 338 // search through table until end for an entry is found whose key / 1000 339 // is within the speedIncrement of the throttleSetting 340 // Note there may be zero values interspersed in the tree 341 Entry<Integer, SpeedStep> entry = speeds.firstEntry(); 342 if (entry == null) { 343 return null; 344 } 345 int key = entry.getKey(); 346 while (entry != null) { 347 entry = speeds.higherEntry(key); 348 if (entry != null) { 349 float speed = entry.getKey(); 350 if (Math.abs(speed/1000.0f - throttleSetting) <= speedIncrement) { 351 return entry; 352 } 353 key = entry.getKey(); 354 } 355 } 356 return null; 357 } 358 359 /** 360 * Merge raw throttleSetting value with an existing profile SpeedStep if 361 * key for the throttleSetting is within the speedIncrement of the SpeedStep. 362 * @param throttleSetting raw throttle setting value 363 * @param speed track speed 364 * @param speedIncrement throttle's speed step increment. 365 */ 366 public void setReverseSpeed(float throttleSetting, float speed, float speedIncrement) { 367 if (throttleSetting> 0.0f) { 368 _hasReverseSpeeds = true; 369 } else { 370 return; 371 } 372 int key; 373 Entry<Integer, SpeedStep> entry = findEquivalentEntry (throttleSetting, speedIncrement); 374 if (entry != null) { // close keys. i.e. resolve to same throttle step 375 float value = entry.getValue().getReverseSpeed(); 376 speed = (speed + value) / 2; 377 key = entry.getKey(); 378 } else { // nothing close. make new entry 379 key = Math.round(throttleSetting * 1000); 380 } 381 speeds.computeIfAbsent(key, k -> new SpeedStep()).setReverseSpeed(speed); 382 } 383 384 public void setReverseSpeed(float speedStep, float reverse) { 385 if (reverse > 0.0f) { 386 _hasReverseSpeeds = true; 387 } else { 388 return; 389 } 390 int iSpeedStep = Math.round(speedStep * 1000); 391 speeds.computeIfAbsent(iSpeedStep, k -> new SpeedStep()).setReverseSpeed(reverse); 392 } 393 394 /** 395 * return the forward speed in milli-meters per second for a given 396 * percentage throttle 397 * 398 * @param speedStep which is actual percentage throttle 399 * @return MilliMetres per second using straight line interpolation for 400 * missing points 401 */ 402 public float getForwardSpeed(float speedStep) { 403 int iSpeedStep = Math.round(speedStep * 1000); 404 if (iSpeedStep <= 0 || !_hasForwardSpeeds) { 405 return 0.0f; 406 } 407 // Note there may be zero values interspersed in the tree 408 if (speeds.containsKey(iSpeedStep)) { 409 float speed = speeds.get(iSpeedStep).getForwardSpeed(); 410 if (speed > 0.0f) { 411 return speed; 412 } 413 } 414 log.trace("no exact match forward for {}", iSpeedStep); 415 float lower = 0.0f; 416 float higher = 0.0f; 417 int highStep = iSpeedStep; 418 int lowStep = iSpeedStep; 419 420 Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep); 421 while (entry != null && higher <= 0.0f) { 422 highStep = entry.getKey(); 423 float value = entry.getValue().getForwardSpeed(); 424 if (value > 0.0f) { 425 higher = value; 426 } 427 entry = speeds.higherEntry(highStep); 428 } 429 boolean nothingHigher = (higher <= 0.0f); 430 431 entry = speeds.lowerEntry(lowStep); 432 while (entry != null && lower <= 0.0f) { 433 lowStep = entry.getKey(); 434 float value = entry.getValue().getForwardSpeed(); 435 if (value > 0.0f) { 436 lower = value; 437 } 438 entry = speeds.lowerEntry(lowStep); 439 } 440 log.trace("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}", 441 lowStep, lower, highStep, higher, iSpeedStep); 442 if (lower <= 0.0f) { // nothing lower 443 if (nothingHigher) { 444 log.error("Nothing in speed Profile"); 445 return 0.0f; // no forward speeds at all 446 } 447 return higher * iSpeedStep / highStep; 448 } 449 if (nothingHigher) { 450// return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep)); 451 return lower + (iSpeedStep - lowStep) * lower / lowStep; 452 } 453 454 float valperstep = (higher - lower) / (highStep - lowStep); 455 456 return lower + (valperstep * (iSpeedStep - lowStep)); 457 } 458 459 /** 460 * return the reverse speed in millimetres per second for a given percentage 461 * throttle 462 * 463 * @param speedStep percentage of throttle 0.nnn 464 * @return millimetres per second 465 */ 466 public float getReverseSpeed(float speedStep) { 467 int iSpeedStep = Math.round(speedStep * 1000); 468 if (iSpeedStep <= 0 || !_hasReverseSpeeds) { 469 return 0.0f; 470 } 471 if (speeds.containsKey(iSpeedStep)) { 472 float speed = speeds.get(iSpeedStep).getReverseSpeed(); 473 if (speed > 0.0f) { 474 return speed; 475 } 476 } 477 log.trace("no exact match reverse for {}", iSpeedStep); 478 float lower = 0.0f; 479 float higher = 0.0f; 480 int highStep = iSpeedStep; 481 int lowStep = iSpeedStep; 482 // Note there may be zero values interspersed in the tree 483 484 Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep); 485 while (entry != null && higher <= 0.0f) { 486 highStep = entry.getKey(); 487 float value = entry.getValue().getReverseSpeed(); 488 if (value > 0.0f) { 489 higher = value; 490 } 491 entry = speeds.higherEntry(highStep); 492 } 493 boolean nothingHigher = (higher <= 0.0f); 494 entry = speeds.lowerEntry(lowStep); 495 while (entry != null && lower <= 0.0f) { 496 lowStep = entry.getKey(); 497 float value = entry.getValue().getReverseSpeed(); 498 if (value > 0.0f) { 499 lower = value; 500 } 501 entry = speeds.lowerEntry(lowStep); 502 } 503 log.trace("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}", 504 lowStep, lower, highStep, higher, iSpeedStep); 505 if (lower <= 0.0f) { // nothing lower 506 if (nothingHigher) { 507 log.error("Nothing in speed Profile"); 508 return 0.0f; // no reverse speeds at all 509 } 510 return higher * iSpeedStep / highStep; 511 } 512 if (nothingHigher) { 513 return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep)); 514 } 515 516 float valperstep = (higher - lower) / (highStep - lowStep); 517 518 return lower + (valperstep * (iSpeedStep - lowStep)); 519 } 520 521 /** 522 * Get the approximate time a loco may travel a given distance at a given 523 * speed step. 524 * 525 * @param isForward true if loco is running forward; false otherwise 526 * @param speedStep the desired speed step 527 * @param distance the desired distance in millimeters 528 * @return the approximate time in seconds 529 */ 530 public float getDurationOfTravelInSeconds(boolean isForward, float speedStep, int distance) { 531 float spd; 532 if (isForward) { 533 spd = getForwardSpeed(speedStep); 534 } else { 535 spd = getReverseSpeed(speedStep); 536 } 537 if (spd < 0.0f) { 538 log.error("Speed not available to compute duration of travel"); 539 return 0.0f; 540 } 541 return (distance / spd); 542 } 543 544 /** 545 * Get the approximate distance a loco may travel a given duration at a 546 * given speed step. 547 * 548 * @param isForward true if loco is running forward; false otherwise 549 * @param speedStep the desired speed step 550 * @param duration the desired time in seconds 551 * @return the approximate distance in millimeters 552 */ 553 public float getDistanceTravelled(boolean isForward, float speedStep, float duration) { 554 float spd; 555 if (isForward) { 556 spd = getForwardSpeed(speedStep); 557 } else { 558 spd = getReverseSpeed(speedStep); 559 } 560 if (spd < 0.0f) { 561 log.error("Speed not available to compute distance travelled"); 562 return 0.0f; 563 } 564 return Math.abs(spd * duration); 565 } 566 567 private float distanceRemaining = 0; 568 private float distanceTravelled = 0; 569 570 private TreeMap<Integer, SpeedStep> speeds = new TreeMap<>(); 571 572 private DccThrottle _throttle; 573 574 private float desiredSpeedStep = -1; 575 576 private float extraDelay = 0.0f; 577 578 private float minReliableOperatingSpeed = 0.0f; 579 580 private float maxOperatingSpeed = 1.0f; 581 582 private NamedBean referenced = null; 583 584 private javax.swing.Timer stopTimer = null; 585 586 private long lastTimeTimerStarted = 0L; 587 588 /** 589 * reset everything back to default once the change has finished. 590 */ 591 void finishChange() { 592 if (stopTimer != null) { 593 stopTimer.stop(); 594 } 595 stopTimer = null; 596 _throttle = null; 597 distanceRemaining = 0; 598 desiredSpeedStep = -1; 599 extraDelay = 0.0f; 600 minReliableOperatingSpeed = 0.0f; 601 maxOperatingSpeed = 1.0f; 602 referenced = null; 603 synchronized (this) { 604 distanceTravelled = 0; 605 stepQueue = new LinkedList<>(); 606 } 607 _throttle = null; 608 } 609 610 public void setExtraInitialDelay(float eDelay) { 611 extraDelay = eDelay; 612 } 613 614 public void setMinMaxLimits(float minReliableOperatingSpeed, float maxOperatingSpeed) { 615 this.minReliableOperatingSpeed = minReliableOperatingSpeed; 616 this.maxOperatingSpeed = maxOperatingSpeed; 617 if (minReliableOperatingSpeed > maxOperatingSpeed) { 618 log.warn("MaxOperatingSpeed [{}] < minReliableOperatingSpeed [{}] setting Max = Min", 619 minReliableOperatingSpeed, maxOperatingSpeed); 620 this.maxOperatingSpeed = this.minReliableOperatingSpeed; 621 } 622 } 623 624 /** 625 * Set speed of a throttle. 626 * 627 * @param t the throttle to set 628 * @param blk the block used for length details 629 * @param speed the speed to set 630 */ 631 public void changeLocoSpeed(DccThrottle t, Block blk, float speed) { 632 if (blk == referenced && Float.compare(speed, desiredSpeedStep) == 0) { 633 //log.debug("Already setting to desired speed step for this block"); 634 return; 635 } 636 float blockLength = blk.getLengthMm(); 637 if (blk == referenced) { 638 distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000)); 639 blockLength = distanceRemaining; 640 //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run 641 log.debug("Block passed is the same as we are currently processing"); 642 } else { 643 referenced = blk; 644 } 645 changeLocoSpeed(t, blockLength, speed); 646 } 647 648 /** 649 * Set speed of a throttle. 650 * 651 * @param t the throttle to set 652 * @param sec the section used for length details 653 * @param speed the speed to set 654 * @param usePercentage the percentage of the block to be used for stopping 655 */ 656 @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY", 657 justification = "OK to compare floats, as even tiny differences should trigger update") 658 public void changeLocoSpeed(DccThrottle t, Section sec, float speed, float usePercentage) { 659 if (sec == referenced && speed == desiredSpeedStep) { 660 log.debug("Already setting to desired speed step for this Section"); 661 return; 662 } 663 float sectionLength = sec.getActualLength() * usePercentage; 664 if (sec == referenced) { 665 distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000)); 666 sectionLength = distanceRemaining; 667 //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run 668 log.debug("Block passed is the same as we are currently processing"); 669 } else { 670 referenced = sec; 671 } 672 changeLocoSpeed(t, sectionLength, speed); 673 } 674 675 /** 676 * Set speed of a throttle. 677 * 678 * @param t the throttle to set 679 * @param blk the block used for length details 680 * @param speed the speed to set 681 * @param usePercentage the percentage of the block to be used for stopping 682 */ 683 @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY", 684 justification = "OK to compare floats, as even tiny differences should trigger update") 685 public void changeLocoSpeed(DccThrottle t, Block blk, float speed, float usePercentage) { 686 if (blk == referenced && speed == desiredSpeedStep) { 687 //if(log.isDebugEnabled()) log.debug("Already setting to desired speed step for this block"); 688 return; 689 } 690 float blockLength = blk.getLengthMm() * usePercentage; 691 if (blk == referenced) { 692 distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000)); 693 blockLength = distanceRemaining; 694 //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run 695 log.debug("Block passed is the same as we are currently processing"); 696 } else { 697 referenced = blk; 698 } 699 changeLocoSpeed(t, blockLength, speed); 700 701 } 702 703 /** 704 * Set speed of a throttle to a speeed set by a float, using the section for 705 * the length details 706 * Set speed of a throttle. 707 * 708 * @param t the throttle to set 709 * @param sec the section used for length details 710 * @param speed the speed to set 711 */ 712 //@TODO if a section contains multiple blocks then we could calibrate the change of speed based upon the block status change. 713 public void changeLocoSpeed(DccThrottle t, Section sec, float speed) { 714 if (sec == referenced && Float.compare(speed, desiredSpeedStep) == 0) { 715 log.debug("Already setting to desired speed step for this section"); 716 return; 717 } 718 float sectionLength = sec.getActualLength(); 719 log.debug("call to change speed via section {}", sec.getDisplayName()); 720 if (sec == referenced) { 721 distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000)); 722 sectionLength = distanceRemaining; 723 } else { 724 referenced = sec; 725 } 726 727 changeLocoSpeed(t, sectionLength, speed); 728 } 729 730 /** 731 * Set speed of a throttle. 732 * 733 * @param t the throttle to set 734 * @param distance the distance in meters 735 * @param requestedSpeed the speed to set 736 */ 737 public void changeLocoSpeed(DccThrottle t, float distance, float requestedSpeed) { 738 float speed = 0.0f; 739 log.debug("Call to change speed over specific distance: speed {} distance {}", requestedSpeed, distance); 740 if (requestedSpeed > maxOperatingSpeed) { 741 speed = maxOperatingSpeed; 742 } else { 743 speed = requestedSpeed; 744 } 745 if (Float.compare(speed, desiredSpeedStep) == 0) { 746 // This requires no checks for min/max. 747 log.debug("Already setting to desired speed step"); 748 return; 749 } 750 log.debug("public change speed step by float {}", speed); 751 log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed); 752 753 if (stopTimer != null) { 754 log.debug("stop timer valid so will cancel"); 755 cancelSpeedChange(); 756 } 757 _throttle = t; 758 desiredSpeedStep = speed; 759 760 log.debug("Speed current {} required {} ", 761 _throttle.getSpeedSetting(), speed); 762 if (_throttle.getSpeedSetting() < speed) { 763 log.debug("Going for acceleration"); 764 } else { 765 log.debug("Going for deceleration"); 766 } 767 768 float adjSpeed = speed; 769 boolean andStop = false; 770 if (speed <= 0.0) { 771 andStop = true; 772 } 773 if (speed < minReliableOperatingSpeed) { 774 adjSpeed = minReliableOperatingSpeed; 775 } 776 log.debug("Speed[{}] adjSpeed[{}] MinSpeed[{}]", 777 speed,adjSpeed, minReliableOperatingSpeed); 778 779 if (!andStop 780 && (Float.compare(adjSpeed, t.getSpeedSetting()) == 0 781 || (Math.round(adjSpeed/t.getSpeedIncrement()) == 782 Math.round(t.getSpeedSetting()/t.getSpeedIncrement())))) { 783 log.debug("Throttle and request speed setting are the same {} {} so will quit", speed, t.getSpeedSetting()); 784 //Already at correct speed setting 785 finishChange(); 786 return; 787 } 788 calculateStepDetails(adjSpeed, distance, andStop); 789 } 790 791 private List<SpeedSetting> testSteps = new ArrayList<>(); 792 private boolean profileInTestMode = false; 793 794 void calculateStepDetails(float speedStep, float distance, boolean andStop) { 795 796 float stepIncrement = _throttle.getSpeedIncrement(); 797 log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speedStep); 798 desiredSpeedStep = speedStep; 799 log.debug("calculated current step {} required {} current {} increment {}", _throttle.getSpeedSetting(), speedStep, _throttle.getSpeedSetting(), stepIncrement); 800 boolean increaseSpeed = false; 801 if (_throttle.getSpeedSetting() < speedStep) { 802 increaseSpeed = true; 803 log.debug("Going for acceleration"); 804 } else { 805 log.debug("Going for deceleration"); 806 } 807 808 if (distance <= 0) { 809 log.debug("Distance is less than 0 {}", distance); 810 _throttle.setSpeedSetting(speedStep); 811 finishChange(); 812 return; 813 } 814 815 float calculatedDistance = distance; 816 817 if (stopTimer != null) { 818 stopTimer.stop(); 819 distanceRemaining = distance; 820 } else { 821 calculatedDistance = calculateInitialOverRun(distance); 822 distanceRemaining = calculatedDistance; 823 } 824 if (distanceRemaining < 0.0f) { 825 if (andStop) { 826 _throttle.setSpeedSetting(0.0f); 827 } else { 828 _throttle.setSpeedSetting(speedStep); 829 } 830 log.warn("There is insufficient distance [{}] after adjustments, setting speed immediately", distanceRemaining); 831 return; 832 } 833 834 float calculatingStep = _throttle.getSpeedSetting(); 835 if (increaseSpeed) { 836 if (calculatingStep < minReliableOperatingSpeed) { 837 calculatingStep = minReliableOperatingSpeed; 838 } 839 } 840 841 float endspd = 0; 842 if (calculatingStep != 0.0 && desiredSpeedStep > 0) { // current speed 843 if (_throttle.getIsForward()) { 844 endspd = getForwardSpeed(desiredSpeedStep); 845 } else { 846 endspd = getReverseSpeed(desiredSpeedStep); 847 } 848 } else if (desiredSpeedStep != 0.0) { 849 if (_throttle.getIsForward()) { 850 endspd = getForwardSpeed(desiredSpeedStep); 851 } else { 852 endspd = getReverseSpeed(desiredSpeedStep); 853 } 854 } 855 856 boolean calculated = false; 857 while (!calculated) { 858 float spd = 0; 859 if (calculatingStep != 0.0) { // current speed 860 if (_throttle.getIsForward()) { 861 spd = getForwardSpeed(calculatingStep); 862 } else { 863 spd = getReverseSpeed(calculatingStep); 864 } 865 } 866 867 log.debug("end spd {} spd {}", endspd, spd); 868 double avgSpeed = Math.abs((endspd + spd) * 0.5); 869 log.debug("avg Speed {}", avgSpeed); 870 871 double time = (calculatedDistance / avgSpeed); //in seconds 872 time = time * 1000; //covert it to milli seconds 873 float speeddiff = calculatingStep - desiredSpeedStep; 874 if (increaseSpeed) { 875 speeddiff = desiredSpeedStep - calculatingStep; 876 } 877 float noSteps = speeddiff / stepIncrement; 878 log.debug("Speed diff {} number of Steps {} step increment {}", speeddiff, noSteps, stepIncrement); 879 880 int timePerStep = (int) (time / noSteps); 881 if (timePerStep < 0) { 882 log.error("Time per speed went to zero or below, setting finale speed immediatly."); 883 if (_throttle != null) { 884 addSpeedStepItem(calculated,new SpeedSetting(desiredSpeedStep, 10, andStop)); 885 setNextStep(); 886 } 887 break; 888 } 889 float calculatedStepInc = stepIncrement; 890 boolean lastStep = false; 891 if (Math.abs(speeddiff) > (stepIncrement * 2)) { 892 //We do not get reliable time results if the duration per speed step is less than 500ms 893 //therefore we calculate how many speed steps will fit in to 750ms. 894 if (timePerStep <= 500 && timePerStep > 0) { 895 float newTime = 750.0f; 896 float tmp =(float) Math.floor(newTime / timePerStep); 897 // To avoid the lack of a stub ensure resultant speed is less than final speed by at least a step. 898 if (increaseSpeed) { 899 while (desiredSpeedStep - ( calculatingStep + (stepIncrement * tmp)) <= stepIncrement) { 900 tmp = tmp - 1; 901 } 902 903 if (tmp > 0 && calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), 904 calculatingStep + (stepIncrement * tmp), 905 ((float) (newTime / 1000.0))) > 0) { 906 calculatedStepInc = stepIncrement * tmp; 907 timePerStep = (int)newTime; 908 } 909 } else { 910 while (calculatingStep - (stepIncrement * tmp) - desiredSpeedStep <= stepIncrement) { 911 tmp = tmp - 1; 912 } 913 if ( tmp > 0 && (calculatedDistance 914 - getDistanceTravelled(_throttle.getIsForward(), 915 calculatingStep - (stepIncrement * tmp), 916 ((float) (newTime / 1000.0)))) > 0) { 917 calculatedStepInc = stepIncrement * tmp; 918 timePerStep = (int)newTime; 919 } 920 } 921 log.debug("time per step was {} no of increments in 750 ms is {} new step increment in {}", timePerStep, tmp, calculatedStepInc); 922 } 923 } else { 924 // last bit calculate duration from distance remaining 925 if (increaseSpeed && calculatingStep == 0) { 926 calculatingStep+=calculatedStepInc; 927 } 928 timePerStep = Math.round(calculatedDistance/getSpeed(calculatingStep,_throttle.getIsForward())*1000); 929 if (!increaseSpeed) { 930 calculatedStepInc = calculatingStep - desiredSpeedStep; 931 } else { 932 calculatedStepInc = desiredSpeedStep - calculatingStep ; 933 } 934 lastStep=true; 935 } 936 calculatedStepInc=Math.abs(calculatedStepInc); 937 log.debug("per interval {}, increase {} lastStep {}", timePerStep, increaseSpeed,lastStep); 938 //Calculate the new speed setting 939 if (increaseSpeed) { 940 //if (calculatingStep + calculatedStepInc == desiredSpeedStep) { 941 if (lastStep) { 942 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 943 addSpeedStepItem(calculated,ss); 944 calculated = true; 945 if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;} 946 else { 947 calculatingStep = 0.0f;timePerStep=2; 948 } 949 ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 950 addSpeedStepItem(calculated,ss); 951 if (stopTimer == null) { 952 setNextStep(); 953 } 954 break; 955 } 956 calculatingStep = calculatingStep + calculatedStepInc; 957 } else { 958 if (lastStep) { 959 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 960 addSpeedStepItem(calculated,ss); 961 calculated = true; 962 if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;} 963 else { 964 calculatingStep = 0.0f;timePerStep=2; 965 } 966 ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 967 addSpeedStepItem(calculated,ss); 968 if (stopTimer == null) { //If this is the first time round then kick off the speed change 969 setNextStep(); 970 } 971 break; 972 } 973 calculatingStep = calculatingStep - calculatedStepInc; 974 } 975 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 976 addSpeedStepItem(calculated,ss); 977 if (stopTimer == null) { //If this is the first time round then kick off the speed change 978 setNextStep(); 979 } 980 if (calculated) { 981 if (andStop) { 982 ss = new SpeedSetting(0.0f, 10, andStop); 983 } else { 984 ss = new SpeedSetting(desiredSpeedStep, 10, andStop); 985 } 986 addSpeedStepItem(calculated,ss); } 987 // The throttle can disappear during a stop situation 988 if (_throttle != null) { 989 calculatedDistance = calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), calculatingStep, ((float) (timePerStep / 1000.0))); 990 } else { 991 log.warn("Throttle destroyed before zero length[{}] remaining.",calculatedDistance); 992 calculatedDistance = 0; 993 } 994 995 if (calculatedDistance <= 0 && !calculated) { 996 log.warn("distance remaining is now 0, but we have not reached desired speed setting {} v {}", desiredSpeedStep, calculatingStep); 997 calculated = true; 998 } 999 } 1000 } 1001 1002 private void addSpeedStepItem(Boolean calculated, SpeedSetting ss) { 1003 synchronized (this) { 1004 stepQueue.addLast(ss); 1005 if (profileInTestMode) { 1006 testSteps.add(ss); 1007 } 1008 if (ss.andStop && calculated) { 1009 ss = new SpeedSetting( 0.0f, 0, ss.andStop); 1010 stepQueue.addLast(ss); 1011 if (profileInTestMode) { 1012 testSteps.add(ss); 1013 } 1014 } 1015 } 1016 } 1017 1018 //The bit with the distance is not used 1019 float calculateInitialOverRun(float distance) { 1020 log.debug("Stop timer not configured so will add overrun {}", distance); 1021 if (_throttle.getIsForward()) { 1022 float extraAsDouble = (getOverRunTimeForward() + extraDelay) / 1000; 1023 if (log.isDebugEnabled()) { 1024 log.debug("Over run time to remove (Forward) {} {}", getOverRunTimeForward(), extraAsDouble); 1025 } 1026 float olddistance = getDistanceTravelled(true, _throttle.getSpeedSetting(), extraAsDouble); 1027 distance = distance - olddistance; 1028 //time = time-getOverRunTimeForward(); 1029 //time = time-(extraAsDouble*1000); 1030 } else { 1031 float extraAsDouble = (getOverRunTimeReverse() + extraDelay) / 1000; 1032 if (log.isDebugEnabled()) { 1033 log.debug("Over run time to remove (Reverse) {} {}", getOverRunTimeReverse(), extraAsDouble); 1034 } 1035 float olddistance = getDistanceTravelled(false, _throttle.getSpeedSetting(), extraAsDouble); 1036 distance = distance - olddistance; 1037 //time = time-getOverRunTimeReverse(); 1038 //time = time-(extraAsDouble*1000); 1039 } 1040 log.debug("Distance remaining {}", distance); 1041 //log.debug("Time after overrun removed " + time); 1042 return distance; 1043 1044 } 1045 1046 /** 1047 * This method is called to cancel the existing change in speed. 1048 */ 1049 public void cancelSpeedChange() { 1050 if (stopTimer != null && stopTimer.isRunning()) { 1051 stopTimer.stop(); 1052 } 1053 finishChange(); 1054 } 1055 1056 synchronized void setNextStep() { 1057 //if (profileInTestMode) { 1058 // return; 1059 //} 1060 if (stepQueue.isEmpty()) { 1061 log.debug("No more results"); 1062 finishChange(); 1063 return; 1064 } 1065 SpeedSetting ss = stepQueue.getFirst(); 1066 if (ss.getDuration() == 0) { 1067 if (ss.getAndStop()) { 1068 _throttle.setSpeedSetting(0.0f); 1069 } else { 1070 _throttle.setSpeedSetting(desiredSpeedStep); 1071 } 1072 finishChange(); 1073 return; 1074 } 1075 if (stopTimer != null) { 1076 //Reduce the distanceRemaining and calculate the distance travelling 1077 float distanceTravelledThisStep = getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (stopTimer.getDelay() / 1000.0))); 1078 distanceTravelled = distanceTravelled + distanceTravelledThisStep; 1079 distanceRemaining = distanceRemaining - distanceTravelledThisStep; 1080 } 1081 stepQueue.removeFirst(); 1082 _throttle.setSpeedSetting(ss.getSpeedStep()); 1083 stopTimer = new javax.swing.Timer(ss.getDuration(), (java.awt.event.ActionEvent e) -> { 1084 setNextStep(); 1085 }); 1086 stopTimer.setRepeats(false); 1087 lastTimeTimerStarted = System.nanoTime(); 1088 stopTimer.start(); 1089 1090 } 1091 1092 private LinkedList<SpeedSetting> stepQueue = new LinkedList<>(); 1093 1094 public static class SpeedSetting { 1095 1096 private float step = 0.0f; 1097 private int duration = 0; 1098 private boolean andStop; 1099 1100 public SpeedSetting(float step, int duration, boolean andStop) { 1101 log.debug("Adding step {} duration {} andStop{}", step, duration, andStop); 1102 this.step = step; 1103 this.duration = duration; 1104 this.andStop = andStop; 1105 } 1106 1107 public float getSpeedStep() { 1108 return step; 1109 } 1110 1111 public int getDuration() { 1112 return duration; 1113 } 1114 1115 public boolean getAndStop() { 1116 return andStop; 1117 } 1118 } 1119 1120 /* 1121 * The follow deals with the storage and loading of the speed profile for a roster entry. 1122 */ 1123 public void store(Element e) { 1124 Element d = new Element("speedprofile"); 1125 d.addContent(new Element("overRunTimeForward").addContent(Float.toString(getOverRunTimeForward()))); 1126 d.addContent(new Element("overRunTimeReverse").addContent(Float.toString(getOverRunTimeReverse()))); 1127 Element s = new Element("speeds"); 1128 speeds.keySet().stream().forEachOrdered( i -> { 1129 Element ss = new Element("speed"); 1130 ss.addContent(new Element("step").addContent(Integer.toString(i))); 1131 ss.addContent(new Element("forward").addContent(Float.toString(speeds.get(i).getForwardSpeed()))); 1132 ss.addContent(new Element("reverse").addContent(Float.toString(speeds.get(i).getReverseSpeed()))); 1133 s.addContent(ss); 1134 }); 1135 d.addContent(s); 1136 e.addContent(d); 1137 } 1138 1139 public void load(Element e) { 1140 try { 1141 setOverRunTimeForward(Float.parseFloat(e.getChild("overRunTimeForward").getText())); 1142 } catch (NumberFormatException ex) { 1143 log.error("Over run Error For {}", _re.getId()); 1144 } 1145 try { 1146 setOverRunTimeReverse(Float.parseFloat(e.getChild("overRunTimeReverse").getText())); 1147 } catch (NumberFormatException ex) { 1148 log.error("Over Run Error Rev {}", _re.getId()); 1149 } 1150 e.getChild("speeds").getChildren("speed").forEach( spd -> { 1151 try { 1152 String step = spd.getChild("step").getText(); 1153 String forward = spd.getChild("forward").getText(); 1154 String reverse = spd.getChild("reverse").getText(); 1155 float forwardSpeed = Float.parseFloat(forward); 1156 if (forwardSpeed > 0.0f) { 1157 _hasForwardSpeeds = true; 1158 } 1159 float reverseSpeed = Float.parseFloat(reverse); 1160 if (reverseSpeed > 0.0f) { 1161 _hasReverseSpeeds = true; 1162 } 1163 setSpeed(Integer.parseInt(step), forwardSpeed, reverseSpeed); 1164 } catch (NumberFormatException ex) { 1165 log.error("Not loaded {}", ex.getMessage()); 1166 } 1167 }); 1168 } 1169 1170 public static class SpeedStep { 1171 1172 private float forward = 0.0f; 1173 private float reverse = 0.0f; 1174 1175 /** 1176 * Create a new SpeedStep, Reverse and Forward speeds are 0. 1177 */ 1178 public SpeedStep() { 1179 } 1180 1181 /** 1182 * Set the Forward speed for the step. 1183 * @param speed the forward speed for the Step. 1184 */ 1185 public void setForwardSpeed(float speed) { 1186 forward = speed; 1187 } 1188 1189 /** 1190 * Set the Reverse speed for the step. 1191 * @param speed the reverse speed for the Step. 1192 */ 1193 public void setReverseSpeed(float speed) { 1194 reverse = speed; 1195 } 1196 1197 /** 1198 * Get the Forward Speed for the Step. 1199 * @return the forward speed. 1200 */ 1201 public float getForwardSpeed() { 1202 return forward; 1203 } 1204 1205 /** 1206 * Get the Reverse Speed for the Step. 1207 * @return the reverse speed. 1208 */ 1209 public float getReverseSpeed() { 1210 return reverse; 1211 } 1212 1213 @Override 1214 public boolean equals(Object obj) { 1215 if (this == obj) { 1216 return true; 1217 } 1218 if (obj == null || getClass() != obj.getClass()) { 1219 return false; 1220 } 1221 SpeedStep ss = (SpeedStep) obj; 1222 return Float.compare(ss.getForwardSpeed(), forward) == 0 1223 && Float.compare(ss.getReverseSpeed(), reverse) == 0; 1224 } 1225 1226 @Override 1227 public int hashCode() { 1228 int result = 17; 1229 result = 31 * result + Float.floatToIntBits(forward); 1230 result = 31 * result + Float.floatToIntBits(reverse); 1231 return result; 1232 } 1233 1234 } 1235 1236 /** 1237 * Get the number of SpeedSteps. 1238 * If there are too few SpeedSteps, it may be difficult to get reasonable 1239 * distances and speeds over a large range of throttle settings. 1240 * @return the number of Speed Steps in the profile. 1241 */ 1242 public int getProfileSize() { 1243 return speeds.size(); 1244 } 1245 1246 public TreeMap<Integer, SpeedStep> getProfileSpeeds() { 1247 return speeds; 1248 } 1249 1250 /** 1251 * Get the throttle setting to achieve a track speed 1252 * 1253 * @param speed desired track speed in mm/sec 1254 * @param isForward direction 1255 * @return throttle setting 1256 */ 1257 public float getThrottleSetting(float speed, boolean isForward) { 1258 if ((isForward && !_hasForwardSpeeds) || (!isForward && !_hasReverseSpeeds)) { 1259 return 0.0f; 1260 } 1261 int slowerKey = 0; 1262 float slowerValue = 0; 1263 float fasterKey; 1264 float fasterValue; 1265 Entry<Integer, SpeedStep> entry = speeds.firstEntry(); 1266 if (entry == null) { 1267 log.warn("There is no speedprofile entries for [{}]", this.getRosterEntry().getId()); 1268 return (0.0f); 1269 } 1270 // search through table until end or the entry is greater than 1271 // what we are looking for. This leaves the previous lower value in key. and slower 1272 // Note there may be zero values interspersed in the tree 1273 if (isForward) { 1274 fasterKey = entry.getKey(); 1275 fasterValue = entry.getValue().getForwardSpeed(); 1276 while (entry != null && entry.getValue().getForwardSpeed() < speed) { 1277 slowerKey = entry.getKey(); 1278 float value = entry.getValue().getForwardSpeed(); 1279 if (value > 0.0f) { 1280 slowerValue = value; 1281 } 1282 entry = speeds.higherEntry(slowerKey); 1283 if (entry != null) { 1284 fasterKey = entry.getKey(); 1285 value = entry.getValue().getForwardSpeed(); 1286 if (value > 0.0f) { 1287 fasterValue = value; 1288 } 1289 } 1290 } 1291 } else { 1292 fasterKey = entry.getKey(); 1293 fasterValue = entry.getValue().getReverseSpeed(); 1294 while (entry != null && entry.getValue().getReverseSpeed() < speed) { 1295 slowerKey = entry.getKey(); 1296 float value = entry.getValue().getReverseSpeed(); 1297 if (value > 0.0f) { 1298 slowerValue = value; 1299 } 1300 entry = speeds.higherEntry(slowerKey); 1301 if (entry != null) { 1302 fasterKey = entry.getKey(); 1303 value = entry.getValue().getReverseSpeed(); 1304 if (value > 0.0f) { 1305 fasterValue = value; 1306 } 1307 } 1308 } 1309 } 1310 log.trace("slowerKey={}, slowerValue={} fasterKey={} fasterValue={} for speed={}", 1311 slowerKey, slowerValue, fasterKey, fasterValue, speed); 1312 if (entry == null) { 1313 // faster does not exists use slower... 1314 if (slowerValue <= 0.0f) { // neither does slower 1315 return (0.0f); 1316 } 1317 1318 // extrapolate 1319 float key = slowerKey * speed / slowerValue; 1320 if (key < 1000.0f) { 1321 return key / 1000.0f; 1322 } else { 1323 return 1.0f; 1324 } 1325 } 1326 if (Float.compare(slowerValue, speed) == 0 || fasterValue <= slowerValue) { 1327 return slowerKey / 1000.0f; 1328 } 1329 if (slowerValue <= 0.0f) { // no entry had a slower speed, therefore key is invalid 1330 slowerKey = 0; 1331 if (fasterValue <= 0.0f) { // neither is there a faster speed 1332 return (0.0f); 1333 } 1334 } 1335 // we need to interpolate 1336 float ratio = (speed - slowerValue) / (fasterValue - slowerValue); 1337 return (slowerKey + ((fasterKey - slowerKey) * ratio)) / 1000.0f; 1338 } 1339 1340 /** 1341 * Get track speed in millimeters per second from throttle setting 1342 * 1343 * @param speedStep throttle setting 1344 * @param isForward direction 1345 * @return track speed 1346 */ 1347 public float getSpeed(float speedStep, boolean isForward) { 1348 if (speedStep < 0.00001f) { 1349 return 0.0f; 1350 } 1351 float speed; 1352 if (isForward) { 1353 speed = getForwardSpeed(speedStep); 1354 } else { 1355 speed = getReverseSpeed(speedStep); 1356 } 1357 return speed; 1358 } 1359 1360 private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(RosterSpeedProfile.class); 1361 1362}