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