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.debug("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.debug("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.debug("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.debug("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 float {} 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 759 log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed); 760 desiredSpeedStep = speed; 761 762 log.debug("calculated current step {} required {} current {}", 763 _throttle.getSpeedSetting(), speed, _throttle.getSpeedSetting()); 764 if (_throttle.getSpeedSetting() < speed) { 765 log.debug("Going for acceleration"); 766 } else { 767 log.debug("Going for deceleration"); 768 } 769 770 float adjSpeed = speed; 771 boolean andStop = false; 772 if (speed <= 0.0) { 773 andStop = true; 774 } 775 if (speed < minReliableOperatingSpeed) { 776 adjSpeed = minReliableOperatingSpeed; 777 } 778 log.debug("Speed[{}] adjSpeed[{}] MinSpeed[{}]", 779 speed,adjSpeed, minReliableOperatingSpeed); 780 781 if (!andStop 782 && (Float.compare(adjSpeed, t.getSpeedSetting()) == 0 783 || (Math.round(adjSpeed/t.getSpeedIncrement()) == 784 Math.round(t.getSpeedSetting()/t.getSpeedIncrement())))) { 785 log.debug("Throttle and request speed setting are the same {} {} so will quit", speed, t.getSpeedSetting()); 786 //Already at correct speed setting 787 finishChange(); 788 return; 789 } 790 calculateStepDetails(adjSpeed, distance, andStop); 791 } 792 793 private List<SpeedSetting> testSteps = new ArrayList<>(); 794 private boolean profileInTestMode = false; 795 796 void calculateStepDetails(float speedStep, float distance, boolean andStop) { 797 798 float stepIncrement = _throttle.getSpeedIncrement(); 799 log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speedStep); 800 desiredSpeedStep = speedStep; 801 log.debug("calculated current step {} required {} current {} increment {}", _throttle.getSpeedSetting(), speedStep, _throttle.getSpeedSetting(), stepIncrement); 802 boolean increaseSpeed = false; 803 if (_throttle.getSpeedSetting() < speedStep) { 804 increaseSpeed = true; 805 log.debug("Going for acceleration"); 806 } else { 807 log.debug("Going for deceleration"); 808 } 809 810 if (distance <= 0) { 811 log.debug("Distance is less than 0 {}", distance); 812 _throttle.setSpeedSetting(speedStep); 813 finishChange(); 814 return; 815 } 816 817 float calculatedDistance = distance; 818 819 if (stopTimer != null) { 820 stopTimer.stop(); 821 distanceRemaining = distance; 822 } else { 823 calculatedDistance = calculateInitialOverRun(distance); 824 distanceRemaining = calculatedDistance; 825 } 826 if (distanceRemaining < 0.0f) { 827 if (andStop) { 828 _throttle.setSpeedSetting(0.0f); 829 } else { 830 _throttle.setSpeedSetting(speedStep); 831 } 832 log.warn("There is insufficient distance [{}] after adjustments, setting speed immediately", distanceRemaining); 833 return; 834 } 835 836 float calculatingStep = _throttle.getSpeedSetting(); 837 if (increaseSpeed) { 838 if (calculatingStep < minReliableOperatingSpeed) { 839 calculatingStep = minReliableOperatingSpeed; 840 } 841 } 842 843 float endspd = 0; 844 if (calculatingStep != 0.0 && desiredSpeedStep > 0) { // current speed 845 if (_throttle.getIsForward()) { 846 endspd = getForwardSpeed(desiredSpeedStep); 847 } else { 848 endspd = getReverseSpeed(desiredSpeedStep); 849 } 850 } else if (desiredSpeedStep != 0.0) { 851 if (_throttle.getIsForward()) { 852 endspd = getForwardSpeed(desiredSpeedStep); 853 } else { 854 endspd = getReverseSpeed(desiredSpeedStep); 855 } 856 } 857 858 boolean calculated = false; 859 while (!calculated) { 860 float spd = 0; 861 if (calculatingStep != 0.0) { // current speed 862 if (_throttle.getIsForward()) { 863 spd = getForwardSpeed(calculatingStep); 864 } else { 865 spd = getReverseSpeed(calculatingStep); 866 } 867 } 868 869 log.debug("end spd {} spd {}", endspd, spd); 870 double avgSpeed = Math.abs((endspd + spd) * 0.5); 871 log.debug("avg Speed {}", avgSpeed); 872 873 double time = (calculatedDistance / avgSpeed); //in seconds 874 time = time * 1000; //covert it to milli seconds 875 float speeddiff = calculatingStep - desiredSpeedStep; 876 if (increaseSpeed) { 877 speeddiff = desiredSpeedStep - calculatingStep; 878 } 879 float noSteps = speeddiff / stepIncrement; 880 log.debug("Speed diff {} number of Steps {} step increment {}", speeddiff, noSteps, stepIncrement); 881 882 int timePerStep = (int) (time / noSteps); 883 if (timePerStep < 0) { 884 log.error("Time per speed went to zero or below, setting finale speed immediatly."); 885 if (_throttle != null) { 886 addSpeedStepItem(calculated,new SpeedSetting(desiredSpeedStep, 10, andStop)); 887 setNextStep(); 888 } 889 break; 890 } 891 float calculatedStepInc = stepIncrement; 892 boolean lastStep = false; 893 if (Math.abs(speeddiff) > (stepIncrement * 2)) { 894 //We do not get reliable time results if the duration per speed step is less than 500ms 895 //therefore we calculate how many speed steps will fit in to 750ms. 896 if (timePerStep <= 500 && timePerStep > 0) { 897 float newTime = 750.0f; 898 float tmp =(float) Math.floor(newTime / timePerStep); 899 // To avoid the lack of a stub ensure resultant speed is less than final speed by at least a step. 900 if (increaseSpeed) { 901 while (desiredSpeedStep - ( calculatingStep + (stepIncrement * tmp)) <= stepIncrement) { 902 tmp = tmp - 1; 903 } 904 905 if (tmp > 0 && calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), 906 calculatingStep + (stepIncrement * tmp), 907 ((float) (newTime / 1000.0))) > 0) { 908 calculatedStepInc = stepIncrement * tmp; 909 timePerStep = (int)newTime; 910 } 911 } else { 912 while (calculatingStep - (stepIncrement * tmp) - desiredSpeedStep <= stepIncrement) { 913 tmp = tmp - 1; 914 } 915 if ( tmp > 0 && (calculatedDistance 916 - getDistanceTravelled(_throttle.getIsForward(), 917 calculatingStep - (stepIncrement * tmp), 918 ((float) (newTime / 1000.0)))) > 0) { 919 calculatedStepInc = stepIncrement * tmp; 920 timePerStep = (int)newTime; 921 } 922 } 923 log.debug("time per step was {} no of increments in 750 ms is {} new step increment in {}", timePerStep, tmp, calculatedStepInc); 924 } 925 } else { 926 // last bit calculate duration from distance remaining 927 if (increaseSpeed && calculatingStep == 0) { 928 calculatingStep+=calculatedStepInc; 929 } 930 timePerStep = Math.round(calculatedDistance/getSpeed(calculatingStep,_throttle.getIsForward())*1000); 931 if (!increaseSpeed) { 932 calculatedStepInc = calculatingStep - desiredSpeedStep; 933 } else { 934 calculatedStepInc = desiredSpeedStep - calculatingStep ; 935 } 936 lastStep=true; 937 } 938 calculatedStepInc=Math.abs(calculatedStepInc); 939 log.debug("per interval {}", timePerStep); 940 //Calculate the new speed setting 941 if (increaseSpeed) { 942 //if (calculatingStep + calculatedStepInc == desiredSpeedStep) { 943 if (lastStep) { 944 // last step(s) 945 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 946 addSpeedStepItem(calculated,ss); 947 calculated = true; 948 if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;} 949 else { 950 calculatingStep = 0.0f;timePerStep=2; 951 } 952 ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 953 addSpeedStepItem(calculated,ss); 954 if (stopTimer == null) { 955 setNextStep(); 956 } 957 break; 958 } 959 calculatingStep = calculatingStep + calculatedStepInc; 960 } else { 961 if (lastStep) { 962 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 963 addSpeedStepItem(calculated,ss); 964 calculated = true; 965 if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;} 966 else { 967 calculatingStep = 0.0f;timePerStep=2; 968 } 969 ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 970 addSpeedStepItem(calculated,ss); 971 if (stopTimer == null) { //If this is the first time round then kick off the speed change 972 setNextStep(); 973 } 974 break; 975 } 976 calculatingStep = calculatingStep - calculatedStepInc; 977 } 978 log.debug("Speed Step current {} speed to set {}", _throttle.getSpeedSetting(), calculatingStep); 979 SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop); 980 addSpeedStepItem(calculated,ss); 981 if (stopTimer == null) { //If this is the first time round then kick off the speed change 982 setNextStep(); 983 } 984 if (calculated) { 985 if (andStop) { 986 ss = new SpeedSetting(0.0f, 10, andStop); 987 } else { 988 ss = new SpeedSetting(desiredSpeedStep, 10, andStop); 989 } 990 addSpeedStepItem(calculated,ss); } 991 // The throttle can disappear during a stop situation 992 if (_throttle != null) { 993 calculatedDistance = calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), calculatingStep, ((float) (timePerStep / 1000.0))); 994 } else { 995 log.warn("Throttle destroyed before zero length[{}] remaining.",calculatedDistance); 996 calculatedDistance = 0; 997 } 998 999 if (calculatedDistance <= 0 && !calculated) { 1000 log.warn("distance remaining is now 0, but we have not reached desired speed setting {} v {}", desiredSpeedStep, calculatingStep); 1001 calculated = true; 1002 } 1003 } 1004 } 1005 1006 private void addSpeedStepItem(Boolean calculated, SpeedSetting ss) { 1007 synchronized (this) { 1008 stepQueue.addLast(ss); 1009 if (profileInTestMode) { 1010 testSteps.add(ss); 1011 } 1012 if (ss.andStop && calculated) { 1013 ss = new SpeedSetting( 0.0f, 0, ss.andStop); 1014 stepQueue.addLast(ss); 1015 if (profileInTestMode) { 1016 testSteps.add(ss); 1017 } 1018 } 1019 } 1020 } 1021 1022 //The bit with the distance is not used 1023 float calculateInitialOverRun(float distance) { 1024 log.debug("Stop timer not configured so will add overrun {}", distance); 1025 if (_throttle.getIsForward()) { 1026 float extraAsDouble = (getOverRunTimeForward() + extraDelay) / 1000; 1027 if (log.isDebugEnabled()) { 1028 log.debug("Over run time to remove (Forward) {} {}", getOverRunTimeForward(), extraAsDouble); 1029 } 1030 float olddistance = getDistanceTravelled(true, _throttle.getSpeedSetting(), extraAsDouble); 1031 distance = distance - olddistance; 1032 //time = time-getOverRunTimeForward(); 1033 //time = time-(extraAsDouble*1000); 1034 } else { 1035 float extraAsDouble = (getOverRunTimeReverse() + extraDelay) / 1000; 1036 if (log.isDebugEnabled()) { 1037 log.debug("Over run time to remove (Reverse) {} {}", getOverRunTimeReverse(), extraAsDouble); 1038 } 1039 float olddistance = getDistanceTravelled(false, _throttle.getSpeedSetting(), extraAsDouble); 1040 distance = distance - olddistance; 1041 //time = time-getOverRunTimeReverse(); 1042 //time = time-(extraAsDouble*1000); 1043 } 1044 log.debug("Distance remaining {}", distance); 1045 //log.debug("Time after overrun removed " + time); 1046 return distance; 1047 1048 } 1049 1050 /** 1051 * This method is called to cancel the existing change in speed. 1052 */ 1053 public void cancelSpeedChange() { 1054 if (stopTimer != null && stopTimer.isRunning()) { 1055 stopTimer.stop(); 1056 } 1057 finishChange(); 1058 } 1059 1060 synchronized void setNextStep() { 1061 //if (profileInTestMode) { 1062 // return; 1063 //} 1064 if (stepQueue.isEmpty()) { 1065 log.debug("No more results"); 1066 finishChange(); 1067 return; 1068 } 1069 SpeedSetting ss = stepQueue.getFirst(); 1070 if (ss.getDuration() == 0) { 1071 if (ss.getAndStop()) { 1072 _throttle.setSpeedSetting(0.0f); 1073 } else { 1074 _throttle.setSpeedSetting(desiredSpeedStep); 1075 } 1076 finishChange(); 1077 return; 1078 } 1079 if (stopTimer != null) { 1080 //Reduce the distanceRemaining and calculate the distance travelling 1081 float distanceTravelledThisStep = getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (stopTimer.getDelay() / 1000.0))); 1082 distanceTravelled = distanceTravelled + distanceTravelledThisStep; 1083 distanceRemaining = distanceRemaining - distanceTravelledThisStep; 1084 } 1085 stepQueue.removeFirst(); 1086 _throttle.setSpeedSetting(ss.getSpeedStep()); 1087 stopTimer = new javax.swing.Timer(ss.getDuration(), (java.awt.event.ActionEvent e) -> { 1088 setNextStep(); 1089 }); 1090 stopTimer.setRepeats(false); 1091 lastTimeTimerStarted = System.nanoTime(); 1092 stopTimer.start(); 1093 1094 } 1095 1096 private LinkedList<SpeedSetting> stepQueue = new LinkedList<>(); 1097 1098 public static class SpeedSetting { 1099 1100 private float step = 0.0f; 1101 private int duration = 0; 1102 private boolean andStop; 1103 1104 public SpeedSetting(float step, int duration, boolean andStop) { 1105 this.step = step; 1106 this.duration = duration; 1107 this.andStop = andStop; 1108 } 1109 1110 public float getSpeedStep() { 1111 return step; 1112 } 1113 1114 public int getDuration() { 1115 return duration; 1116 } 1117 1118 public boolean getAndStop() { 1119 return andStop; 1120 } 1121 } 1122 1123 /* 1124 * The follow deals with the storage and loading of the speed profile for a roster entry. 1125 */ 1126 public void store(Element e) { 1127 Element d = new Element("speedprofile"); 1128 d.addContent(new Element("overRunTimeForward").addContent(Float.toString(getOverRunTimeForward()))); 1129 d.addContent(new Element("overRunTimeReverse").addContent(Float.toString(getOverRunTimeReverse()))); 1130 Element s = new Element("speeds"); 1131 speeds.keySet().stream().forEachOrdered( i -> { 1132 Element ss = new Element("speed"); 1133 ss.addContent(new Element("step").addContent(Integer.toString(i))); 1134 ss.addContent(new Element("forward").addContent(Float.toString(speeds.get(i).getForwardSpeed()))); 1135 ss.addContent(new Element("reverse").addContent(Float.toString(speeds.get(i).getReverseSpeed()))); 1136 s.addContent(ss); 1137 }); 1138 d.addContent(s); 1139 e.addContent(d); 1140 } 1141 1142 public void load(Element e) { 1143 try { 1144 setOverRunTimeForward(Float.parseFloat(e.getChild("overRunTimeForward").getText())); 1145 } catch (NumberFormatException ex) { 1146 log.error("Over run Error For {}", _re.getId()); 1147 } 1148 try { 1149 setOverRunTimeReverse(Float.parseFloat(e.getChild("overRunTimeReverse").getText())); 1150 } catch (NumberFormatException ex) { 1151 log.error("Over Run Error Rev {}", _re.getId()); 1152 } 1153 e.getChild("speeds").getChildren("speed").forEach( spd -> { 1154 try { 1155 String step = spd.getChild("step").getText(); 1156 String forward = spd.getChild("forward").getText(); 1157 String reverse = spd.getChild("reverse").getText(); 1158 float forwardSpeed = Float.parseFloat(forward); 1159 if (forwardSpeed > 0.0f) { 1160 _hasForwardSpeeds = true; 1161 } 1162 float reverseSpeed = Float.parseFloat(reverse); 1163 if (reverseSpeed > 0.0f) { 1164 _hasReverseSpeeds = true; 1165 } 1166 setSpeed(Integer.parseInt(step), forwardSpeed, reverseSpeed); 1167 } catch (NumberFormatException ex) { 1168 log.error("Not loaded {}", ex.getMessage()); 1169 } 1170 }); 1171 } 1172 1173 public static class SpeedStep { 1174 1175 private float forward = 0.0f; 1176 private float reverse = 0.0f; 1177 1178 /** 1179 * Create a new SpeedStep, Reverse and Forward speeds are 0. 1180 */ 1181 public SpeedStep() { 1182 } 1183 1184 /** 1185 * Set the Forward speed for the step. 1186 * @param speed the forward speed for the Step. 1187 */ 1188 public void setForwardSpeed(float speed) { 1189 forward = speed; 1190 } 1191 1192 /** 1193 * Set the Reverse speed for the step. 1194 * @param speed the reverse speed for the Step. 1195 */ 1196 public void setReverseSpeed(float speed) { 1197 reverse = speed; 1198 } 1199 1200 /** 1201 * Get the Forward Speed for the Step. 1202 * @return the forward speed. 1203 */ 1204 public float getForwardSpeed() { 1205 return forward; 1206 } 1207 1208 /** 1209 * Get the Reverse Speed for the Step. 1210 * @return the reverse speed. 1211 */ 1212 public float getReverseSpeed() { 1213 return reverse; 1214 } 1215 1216 @Override 1217 public boolean equals(Object obj) { 1218 if (this == obj) { 1219 return true; 1220 } 1221 if (obj == null || getClass() != obj.getClass()) { 1222 return false; 1223 } 1224 SpeedStep ss = (SpeedStep) obj; 1225 return Float.compare(ss.getForwardSpeed(), forward) == 0 1226 && Float.compare(ss.getReverseSpeed(), reverse) == 0; 1227 } 1228 1229 @Override 1230 public int hashCode() { 1231 int result = 17; 1232 result = 31 * result + Float.floatToIntBits(forward); 1233 result = 31 * result + Float.floatToIntBits(reverse); 1234 return result; 1235 } 1236 1237 } 1238 1239 /** 1240 * Get the number of SpeedSteps. 1241 * If there are too few SpeedSteps, it may be difficult to get reasonable 1242 * distances and speeds over a large range of throttle settings. 1243 * @return the number of Speed Steps in the profile. 1244 */ 1245 public int getProfileSize() { 1246 return speeds.size(); 1247 } 1248 1249 public TreeMap<Integer, SpeedStep> getProfileSpeeds() { 1250 return speeds; 1251 } 1252 1253 /** 1254 * Get the throttle setting to achieve a track speed 1255 * 1256 * @param speed desired track speed in mm/sec 1257 * @param isForward direction 1258 * @return throttle setting 1259 */ 1260 public float getThrottleSetting(float speed, boolean isForward) { 1261 if ((isForward && !_hasForwardSpeeds) || (!isForward && !_hasReverseSpeeds)) { 1262 return 0.0f; 1263 } 1264 int slowerKey = 0; 1265 float slowerValue = 0; 1266 float fasterKey; 1267 float fasterValue; 1268 Entry<Integer, SpeedStep> entry = speeds.firstEntry(); 1269 if (entry == null) { 1270 log.warn("There is no speedprofile entries for [{}]", this.getRosterEntry().getId()); 1271 return (0.0f); 1272 } 1273 // search through table until end or the entry is greater than 1274 // what we are looking for. This leaves the previous lower value in key. and slower 1275 // Note there may be zero values interspersed in the tree 1276 if (isForward) { 1277 fasterKey = entry.getKey(); 1278 fasterValue = entry.getValue().getForwardSpeed(); 1279 while (entry != null && entry.getValue().getForwardSpeed() < speed) { 1280 slowerKey = entry.getKey(); 1281 float value = entry.getValue().getForwardSpeed(); 1282 if (value > 0.0f) { 1283 slowerValue = value; 1284 } 1285 entry = speeds.higherEntry(slowerKey); 1286 if (entry != null) { 1287 fasterKey = entry.getKey(); 1288 value = entry.getValue().getForwardSpeed(); 1289 if (value > 0.0f) { 1290 fasterValue = value; 1291 } 1292 } 1293 } 1294 } else { 1295 fasterKey = entry.getKey(); 1296 fasterValue = entry.getValue().getReverseSpeed(); 1297 while (entry != null && entry.getValue().getReverseSpeed() < speed) { 1298 slowerKey = entry.getKey(); 1299 float value = entry.getValue().getReverseSpeed(); 1300 if (value > 0.0f) { 1301 slowerValue = value; 1302 } 1303 entry = speeds.higherEntry(slowerKey); 1304 if (entry != null) { 1305 fasterKey = entry.getKey(); 1306 value = entry.getValue().getReverseSpeed(); 1307 if (value > 0.0f) { 1308 fasterValue = value; 1309 } 1310 } 1311 } 1312 } 1313 log.debug("slowerKey={}, slowerValue={} fasterKey={} fasterValue={} for speed={}", 1314 slowerKey, slowerValue, fasterKey, fasterValue, speed); 1315 if (entry == null) { 1316 // faster does not exists use slower... 1317 if (slowerValue <= 0.0f) { // neither does slower 1318 return (0.0f); 1319 } 1320 1321 // extrapolate 1322 float key = slowerKey * speed / slowerValue; 1323 if (key < 1000.0f) { 1324 return key / 1000.0f; 1325 } else { 1326 return 1.0f; 1327 } 1328 } 1329 if (Float.compare(slowerValue, speed) == 0 || fasterValue <= slowerValue) { 1330 return slowerKey / 1000.0f; 1331 } 1332 if (slowerValue <= 0.0f) { // no entry had a slower speed, therefore key is invalid 1333 slowerKey = 0; 1334 if (fasterValue <= 0.0f) { // neither is there a faster speed 1335 return (0.0f); 1336 } 1337 } 1338 // we need to interpolate 1339 float ratio = (speed - slowerValue) / (fasterValue - slowerValue); 1340 return (slowerKey + ((fasterKey - slowerKey) * ratio)) / 1000.0f; 1341 } 1342 1343 /** 1344 * Get track speed in millimeters per second from throttle setting 1345 * 1346 * @param speedStep throttle setting 1347 * @param isForward direction 1348 * @return track speed 1349 */ 1350 public float getSpeed(float speedStep, boolean isForward) { 1351 if (speedStep < 0.00001f) { 1352 return 0.0f; 1353 } 1354 float speed; 1355 if (isForward) { 1356 speed = getForwardSpeed(speedStep); 1357 } else { 1358 speed = getReverseSpeed(speedStep); 1359 } 1360 return speed; 1361 } 1362 1363 private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(RosterSpeedProfile.class); 1364 1365}