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