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