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