utils.position_calibration
1# Native imports 2import math 3from typing import Any, Callable, Optional 4 5# Third-party imports 6import wpilib 7from ntcore.util import ntproperty 8 9 10# Cache of dynamically-created subclasses keyed by mechanism name. 11# Each subclass has ntproperty descriptors with NT paths unique to 12# that mechanism, so multiple PositionCalibration instances with 13# different names get independent persistent storage. 14_calibration_classes = {} 15 16# Valid callback names accepted by the constructor and set_callbacks() 17_VALID_CALLBACKS = { 18 'get_position', 'get_velocity', 'set_position', 19 'set_motor_output', 'stop_motor', 20 'set_current_limit', 'set_soft_limits', 'disable_soft_limits', 21 'save_config', 'restore_config', 22 'get_forward_limit_switch', 'get_reverse_limit_switch', 23 'on_limit_detected', 24} 25 26# Callbacks that must always be set before homing or calibration 27_CORE_REQUIRED = {'set_motor_output', 'stop_motor', 'set_position'} 28 29 30class PositionCalibration: 31 """ 32 Reusable calibration and homing controller for positional mechanisms. 33 34 Extracts sensorless or sensor homing and two-phase calibration logic 35 from mechanism-specific subsystems into a standalone, motor-agnostic 36 class. All hardware interaction is done through user-provided 37 callbacks, making this compatible with any motor controller. 38 39 For SparkMax users, the ``motor`` convenience parameter auto-populates 40 callbacks via SparkMaxCallbacks. Individual callbacks passed as keyword 41 arguments override the SparkMax defaults. 42 43 Calibration discovers mechanical hard limits by driving into stops, 44 then computes soft limits with a configurable safety margin. Persists 45 discovered limits to NetworkTables via ntproperty so values survive 46 reboots. 47 48 ## Callbacks 49 50 **Core required** (must be set before homing or calibration): 51 52 | Name | Signature | Purpose | 53 |------|-----------|---------| 54 | `set_motor_output` | `(float) -> None` | Drive motor at duty cycle (-1.0 to 1.0) | 55 | `stop_motor` | `() -> None` | Stop motor output | 56 | `set_position` | `(float) -> None` | Reset encoder position | 57 58 **Detection** (at least one required per homing direction): 59 60 | Name | Signature | Purpose | 61 |------|-----------|---------| 62 | `get_velocity` | `() -> float` | Enables stall detection | 63 | `get_forward_limit_switch` | `() -> bool` | Forward limit switch | 64 | `get_reverse_limit_switch` | `() -> bool` | Reverse limit switch | 65 66 **Optional** (enhance safety/features but not required): 67 68 | Name | Signature | Purpose | 69 |------|-----------|---------| 70 | `get_position` | `() -> float` | Required only for calibration phase 2 | 71 | `set_current_limit` | `(float) -> None` | Protective current limit during homing | 72 | `set_soft_limits` | `(min, max) -> None` | Apply soft limits to hardware | 73 | `disable_soft_limits` | `(fwd, rev) -> None` | Disable soft limits for free travel | 74 | `save_config` | `() -> Any` | Snapshot motor config before homing | 75 | `restore_config` | `(Any) -> None` | Restore snapshot after homing | 76 | `on_limit_detected` | `(pos, dir) -> None` | Fires when a hard limit is found | 77 78 Use ``get_callbacks()`` to inspect the current callback dict (returns 79 ``None`` for any callback that has not been set). 80 81 Examples 82 -------- 83 84 SparkMax convenience (auto-generates all callbacks):: 85 86 self.calibration = PositionCalibration( 87 name="Turret", 88 fallback_min=-90, fallback_max=90, 89 motor=self.motor, encoder=self.encoder) 90 91 Pure callbacks with stall detection only:: 92 93 self.calibration = PositionCalibration( 94 name="Elevator", 95 fallback_min=0.0, fallback_max=1.5, 96 set_motor_output=lambda pct: motor.set(pct), 97 stop_motor=lambda: motor.stopMotor(), 98 set_position=lambda v: encoder.setPosition(v), 99 get_velocity=lambda: encoder.getVelocity()) 100 101 Pure callbacks with limit switches only:: 102 103 self.calibration = PositionCalibration( 104 name="Arm", 105 fallback_min=0.0, fallback_max=90.0, 106 set_motor_output=lambda pct: motor.set(pct), 107 stop_motor=lambda: motor.stopMotor(), 108 set_position=lambda v: encoder.setPosition(v), 109 get_forward_limit_switch=lambda: fwd_switch.get(), 110 get_reverse_limit_switch=lambda: rev_switch.get()) 111 112 Deferred setup (set callbacks after construction):: 113 114 cal = PositionCalibration( 115 name="Wrist", 116 fallback_min=-45, fallback_max=45) 117 # ... later, once hardware is ready ... 118 cal.set_callbacks( 119 set_motor_output=motor.set, 120 stop_motor=motor.stopMotor, 121 set_position=encoder.setPosition, 122 get_velocity=encoder.getVelocity) 123 """ 124 125 # -- Callback type declarations -- 126 _cb_get_position: Optional[Callable[[], float]] 127 _cb_get_velocity: Optional[Callable[[], float]] 128 _cb_set_position: Optional[Callable[[float], None]] 129 _cb_set_motor_output: Optional[Callable[[float], None]] 130 _cb_stop_motor: Optional[Callable[[], None]] 131 _cb_set_current_limit: Optional[Callable[[float], None]] 132 _cb_set_soft_limits: Optional[Callable[[float, float], None]] 133 _cb_disable_soft_limits: Optional[Callable[[bool, bool], None]] 134 _cb_save_config: Optional[Callable[[], Any]] 135 _cb_restore_config: Optional[Callable[[Any], None]] 136 _cb_get_forward_limit_switch: Optional[Callable[[], bool]] 137 _cb_get_reverse_limit_switch: Optional[Callable[[], bool]] 138 _cb_on_limit_detected: Optional[Callable[[float, str], None]] 139 140 def __init__( 141 self, 142 name: str, 143 fallback_min: float, 144 fallback_max: float, 145 *, 146 motor=None, 147 encoder=None, 148 **kwargs, 149 ) -> None: 150 """ 151 Create a new PositionCalibration controller. 152 153 Args: 154 name: mechanism name used for NT paths and alerts (e.g. "Turret") 155 fallback_min: reverse soft limit used before calibration data 156 exists. Once calibrated, the persisted hard limits are used 157 instead. The caller is responsible for the far end of travel 158 if only homing (not full calibration) is performed. 159 fallback_max: forward soft limit used before calibration data 160 exists. See fallback_min for details. 161 motor: (optional) rev.SparkMax for convenience callback generation 162 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 163 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 164 """ 165 # Assign this instance to a cached subclass with ntproperty 166 # descriptors keyed to this mechanism's name. ntproperty is a 167 # class-level descriptor, so each mechanism name needs its own 168 # class to avoid key collisions. The cache ensures the 169 # ntproperty is created only once per name so defaults don't 170 # overwrite previously persisted values. 171 if name not in _calibration_classes: 172 prefix = f"/{name}/calibration" 173 attrs = { 174 "_nt_hard_limit_min": ntproperty( 175 f"{prefix}/hard_limit_min", float("nan"), 176 writeDefault=False, persistent=True), 177 "_nt_hard_limit_max": ntproperty( 178 f"{prefix}/hard_limit_max", float("nan"), 179 writeDefault=False, persistent=True), 180 "_nt_soft_limit_margin": ntproperty( 181 f"{prefix}/soft_limit_margin", 0.05, 182 writeDefault=False, persistent=True), 183 } 184 # Non-persistent booleans for callback status 185 for cb_name in sorted(_VALID_CALLBACKS): 186 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 187 f"{prefix}/callbacks/{cb_name}", False, 188 writeDefault=True, persistent=False) 189 _calibration_classes[name] = type( 190 f"PositionCalibration_{name}", 191 (PositionCalibration,), 192 attrs, 193 ) 194 self.__class__ = _calibration_classes[name] 195 196 self._name = name 197 198 # Soft limits — initialized to defaults, overwritten by 199 # _load_from_nt() if persisted calibration data exists 200 self._min_soft_limit = fallback_min 201 self._max_soft_limit = fallback_max 202 203 # Calibration state 204 self._hard_limit_min = None 205 self._hard_limit_max = None 206 self._is_calibrated = False 207 self._is_calibrating = False 208 self._calibration_phase = 0 209 self._soft_limit_margin = 0.05 210 self._position_offset = 0.0 211 212 # Homing state 213 self._is_homing = False 214 self._saved_config = None 215 216 # -- Build callbacks -- 217 # Validate kwargs keys 218 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 219 if unknown: 220 raise ValueError( 221 f"Unknown callback(s): {', '.join(sorted(unknown))}" 222 ) 223 224 # If motor provided, generate SparkMax defaults 225 defaults = {} 226 if motor is not None: 227 from utils.spark_max_callbacks import SparkMaxCallbacks 228 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 229 230 # Merge: explicit kwargs override SparkMax defaults 231 merged = {**defaults, **kwargs} 232 233 # Store each callback as self._cb_{name} (None if not provided) 234 for cb_name in _VALID_CALLBACKS: 235 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 236 237 # Publish callback status to NT 238 self._publish_callback_status() 239 240 # Attempt to load persisted calibration from NT 241 self._load_from_nt() 242 243 # ---- Public properties ---- 244 245 @property 246 def is_busy(self) -> bool: 247 """True when homing or calibrating (blocks motor commands).""" 248 return self._is_homing or self._is_calibrating 249 250 @property 251 def is_homing(self) -> bool: 252 """Whether a homing routine is active.""" 253 return self._is_homing 254 255 @property 256 def is_calibrating(self) -> bool: 257 """Whether a calibration routine is active.""" 258 return self._is_calibrating 259 260 @property 261 def is_calibrated(self) -> bool: 262 """Whether the full mechanical range has been discovered.""" 263 return self._is_calibrated 264 265 @property 266 def is_zeroed(self) -> bool: 267 """Whether the encoder has been aligned to a reference point.""" 268 return self._hard_limit_min is not None 269 270 @property 271 def min_limit(self): 272 """Hard limit min, or None if uncalibrated.""" 273 return self._hard_limit_min 274 275 @property 276 def max_limit(self): 277 """Hard limit max, or None if uncalibrated.""" 278 return self._hard_limit_max 279 280 @property 281 def min_soft_limit(self) -> float: 282 """Current minimum soft limit.""" 283 return self._min_soft_limit 284 285 @property 286 def max_soft_limit(self) -> float: 287 """Current maximum soft limit.""" 288 return self._max_soft_limit 289 290 @property 291 def soft_limit_margin(self) -> float: 292 """Current safety margin as a fraction of the full range.""" 293 return self._soft_limit_margin 294 295 # ---- Public methods ---- 296 297 def get_callbacks(self) -> dict: 298 """ 299 Return a dict of all callback names and their current values. 300 301 Each key is a callback name from _VALID_CALLBACKS. The value is 302 the callable if set, or None if not set. Useful for inspecting 303 which callbacks are configured:: 304 305 cbs = cal.get_callbacks() 306 for name, func in cbs.items(): 307 print(f"{name}: {'set' if func else 'not set'}") 308 309 Returns: 310 dict mapping callback name -> callable or None 311 """ 312 return { 313 name: getattr(self, f'_cb_{name}', None) 314 for name in sorted(_VALID_CALLBACKS) 315 } 316 317 def set_callbacks(self, **kwargs) -> None: 318 """ 319 Set, override, or clear one or more callbacks by name. 320 321 Pass a callable to set a callback, or None to clear it:: 322 323 # Set a callback 324 cal.set_callbacks(get_velocity=encoder.getVelocity) 325 326 # Clear callbacks that don't apply to this mechanism 327 cal.set_callbacks( 328 get_forward_limit_switch=None, 329 get_reverse_limit_switch=None) 330 331 Args: 332 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 333 Pass None to clear a callback. 334 """ 335 for key, value in kwargs.items(): 336 if key not in _VALID_CALLBACKS: 337 raise ValueError(f"Unknown callback: {key}") 338 setattr(self, f'_cb_{key}', value) 339 self._publish_callback_status() 340 341 def homing_init( 342 self, 343 max_current: float, 344 max_power_pct: float, 345 max_homing_time: float, 346 homing_forward: bool, 347 min_velocity: float = None, 348 home_position: float = 0.0 349 ) -> None: 350 """ 351 Initialize the sensorless or sensor homing routine. 352 353 Saves current motor settings (if save_config callback is set), 354 applies homing-specific configuration, and begins driving toward 355 the specified hard stop. 356 357 Args: 358 max_current: current limit in amps during homing 359 max_power_pct: motor duty cycle during homing (0.0-1.0) 360 max_homing_time: maximum time in seconds before timeout 361 homing_forward: True to home toward forward limit, False for reverse 362 min_velocity: stall detection threshold in user units/second. 363 Defaults to 5% of soft limit range over 2 seconds. 364 home_position: encoder value to set when the hard stop is 365 found (default 0.0) 366 """ 367 # Validate callbacks before starting 368 self._validate_homing(homing_forward) 369 370 if min_velocity is None: 371 full_range = self._max_soft_limit - self._min_soft_limit 372 min_velocity = full_range * 0.05 / 2.0 373 374 # Save current motor settings for restoration (if callback exists) 375 if self._cb_save_config is not None: 376 self._saved_config = self._cb_save_config() 377 else: 378 self._saved_config = None 379 380 # Apply homing current limit (if callback exists) 381 if self._cb_set_current_limit is not None: 382 self._cb_set_current_limit(max_current) 383 384 # Disable soft limit in homing direction (if callback exists) 385 if self._cb_disable_soft_limits is not None: 386 if homing_forward: 387 self._cb_disable_soft_limits(True, False) 388 else: 389 self._cb_disable_soft_limits(False, True) 390 391 # Store homing parameters 392 self._homing_forward = homing_forward 393 self._max_power_pct = max_power_pct 394 self._min_velocity = min_velocity 395 self._max_homing_time = max_homing_time 396 self._home_position = home_position 397 398 # Set homing state 399 self._is_homing = True 400 self._stall_detected = False 401 402 # Start timeout timer 403 self._homing_timer = wpilib.Timer() 404 self._homing_timer.start() 405 406 # Stall detection timer 407 self._stall_timer = wpilib.Timer() 408 409 # Alerts 410 self._homing_status_alert = wpilib.Alert( 411 f"{self._name}: homing started", 412 wpilib.Alert.AlertType.kInfo 413 ) 414 self._homing_status_alert.set(True) 415 self._homing_error_alert = wpilib.Alert( 416 f"{self._name}: homing failed", 417 wpilib.Alert.AlertType.kError 418 ) 419 self._homing_error_alert.set(False) 420 421 def calibration_init( 422 self, 423 max_current: float, 424 max_power_pct: float, 425 max_homing_time: float, 426 min_velocity: float = None, 427 known_range: float = None 428 ) -> None: 429 """ 430 Start a calibration routine that discovers the mechanical range. 431 432 Phase 1: Home negative to find the reverse hard limit (set as zero). 433 Phase 2: Home positive to find the forward hard limit (measured). 434 435 If known_range is provided, only phase 1 runs and the forward hard 436 limit is computed as known_range from the zero point. 437 438 Args: 439 max_current: current limit in amps during calibration 440 max_power_pct: motor duty cycle (0.0-1.0) 441 max_homing_time: maximum time per phase before timeout 442 min_velocity: stall detection threshold in user units/second 443 known_range: if provided, skip phase 2 and use this as the 444 full mechanical range from the zero point 445 """ 446 # Validate callbacks before starting 447 self._validate_calibration() 448 449 # Save motor settings for restoration after calibration 450 if self._cb_save_config is not None: 451 self._cal_saved_config = self._cb_save_config() 452 else: 453 self._cal_saved_config = None 454 455 # Disable both soft limits for free travel 456 if self._cb_disable_soft_limits is not None: 457 self._cb_disable_soft_limits(True, True) 458 459 # Store calibration parameters for phase transitions 460 self._cal_max_current = max_current 461 self._cal_max_power_pct = max_power_pct 462 self._cal_max_homing_time = max_homing_time 463 self._cal_min_velocity = min_velocity 464 self._cal_known_range = known_range 465 466 self._is_calibrating = True 467 self._calibration_phase = 1 468 469 # Start phase 1: home negative 470 self.homing_init( 471 max_current, max_power_pct, max_homing_time, 472 homing_forward=False, min_velocity=min_velocity 473 ) 474 475 self._cal_status_alert = wpilib.Alert( 476 f"{self._name}: calibration phase 1 - homing negative", 477 wpilib.Alert.AlertType.kInfo 478 ) 479 self._cal_status_alert.set(True) 480 481 def periodic(self) -> None: 482 """ 483 Run the active homing or calibration state machine. 484 485 Call this every cycle when is_busy is True. 486 """ 487 if self._is_calibrating: 488 self._calibration_periodic() 489 elif self._is_homing: 490 self._homing_periodic() 491 492 def abort(self) -> None: 493 """Abort any active homing or calibration routine.""" 494 if self._is_calibrating: 495 self._calibration_end(abort=True) 496 elif self._is_homing: 497 self._homing_end(abort=True) 498 499 def set_soft_limit_margin(self, margin_pct: float) -> None: 500 """ 501 Set the soft limit safety margin and apply to the motor controller. 502 503 Args: 504 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 505 """ 506 if not self._is_calibrated: 507 return 508 self._soft_limit_margin = margin_pct 509 full_range = self._hard_limit_max - self._hard_limit_min 510 margin = full_range * margin_pct 511 self._min_soft_limit = self._hard_limit_min + margin 512 self._max_soft_limit = self._hard_limit_max - margin 513 514 # Apply to hardware only if callback exists 515 if self._cb_set_soft_limits is not None: 516 self._cb_set_soft_limits( 517 self._min_soft_limit, self._max_soft_limit) 518 519 def update_telemetry(self, prefix: str) -> None: 520 """ 521 Publish calibration data to SmartDashboard. 522 523 Args: 524 prefix: SmartDashboard key prefix (e.g. "Turret/") 525 """ 526 sd = wpilib.SmartDashboard 527 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 528 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 529 sd.putBoolean(prefix + "isHoming", self._is_homing) 530 sd.putNumber( 531 prefix + "hardLimitMin", 532 self._hard_limit_min if self._hard_limit_min is not None 533 else 0.0 534 ) 535 sd.putNumber( 536 prefix + "hardLimitMax", 537 self._hard_limit_max if self._hard_limit_max is not None 538 else 0.0 539 ) 540 sd.putNumber(prefix + "positionOffset", self._position_offset) 541 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin) 542 543 # ---- Validation ---- 544 545 def _validate_homing(self, homing_forward: bool): 546 """Validate callbacks needed for homing in the given direction.""" 547 missing = [n for n in sorted(_CORE_REQUIRED) 548 if getattr(self, f'_cb_{n}', None) is None] 549 if missing: 550 raise ValueError( 551 f"Required callbacks not set: {', '.join(missing)}" 552 ) 553 554 has_velocity = self._cb_get_velocity is not None 555 if homing_forward: 556 has_limit = self._cb_get_forward_limit_switch is not None 557 else: 558 has_limit = self._cb_get_reverse_limit_switch is not None 559 560 if not has_velocity and not has_limit: 561 direction = "forward" if homing_forward else "reverse" 562 raise ValueError( 563 f"No detection method for {direction} homing: " 564 f"provide get_velocity and/or " 565 f"get_{direction}_limit_switch" 566 ) 567 568 def _validate_calibration(self): 569 """Validate callbacks needed for full calibration.""" 570 if self._cb_get_position is None: 571 raise ValueError( 572 "get_position callback required for calibration" 573 ) 574 # Need detection in both directions 575 self._validate_homing(homing_forward=False) # phase 1 576 self._validate_homing(homing_forward=True) # phase 2 577 578 # ---- Internal methods ---- 579 580 def _publish_callback_status(self) -> None: 581 """Write True/False for each callback to NT via ntproperty.""" 582 for cb_name in _VALID_CALLBACKS: 583 is_set = getattr(self, f'_cb_{cb_name}', None) is not None 584 setattr(self, f'_nt_cb_{cb_name}', is_set) 585 586 def _homing_periodic(self) -> bool: 587 """ 588 Periodic homing logic. Drives the motor, monitors for stall or 589 limit switch, and handles timeout. 590 591 Returns: 592 True when homing is complete (success or failure), False if ongoing 593 """ 594 if not self._is_homing: 595 return True 596 597 # Check timeout first 598 if self._homing_timer.hasElapsed(self._max_homing_time): 599 self._homing_error_alert.setText( 600 f"{self._name}: homing failed - timeout" 601 ) 602 self._homing_end(abort=True) 603 return True 604 605 # Check hard limit switch in homing direction (if callback exists) 606 limit_hit = False 607 if (self._homing_forward 608 and self._cb_get_forward_limit_switch is not None): 609 limit_hit = self._cb_get_forward_limit_switch() 610 elif (not self._homing_forward 611 and self._cb_get_reverse_limit_switch is not None): 612 limit_hit = self._cb_get_reverse_limit_switch() 613 614 # Drive motor in homing direction 615 if self._homing_forward: 616 self._cb_set_motor_output(self._max_power_pct) 617 else: 618 self._cb_set_motor_output(-self._max_power_pct) 619 620 # Check for limit switch hit 621 if limit_hit: 622 home_position = self._home_position 623 self._cb_set_position(home_position) 624 if self._cb_on_limit_detected is not None: 625 direction = ( 626 "forward" if self._homing_forward else "reverse" 627 ) 628 self._cb_on_limit_detected(home_position, direction) 629 self._homing_status_alert.setText( 630 f"{self._name}: homing complete" 631 ) 632 self._homing_end(abort=False) 633 return True 634 635 # Stall detection only if velocity callback provided 636 if self._cb_get_velocity is not None: 637 velocity = abs(self._cb_get_velocity()) 638 639 if velocity < self._min_velocity: 640 if not self._stall_detected: 641 self._stall_detected = True 642 self._stall_timer.restart() 643 elif self._stall_timer.hasElapsed(0.1): 644 home_position = self._home_position 645 self._cb_set_position(home_position) 646 if self._cb_on_limit_detected is not None: 647 direction = ( 648 "forward" if self._homing_forward 649 else "reverse" 650 ) 651 self._cb_on_limit_detected( 652 home_position, direction) 653 self._homing_status_alert.setText( 654 f"{self._name}: homing complete" 655 ) 656 self._homing_end(abort=False) 657 return True 658 else: 659 self._stall_detected = False 660 self._stall_timer.stop() 661 self._stall_timer.reset() 662 663 return False 664 665 def _homing_end(self, abort: bool) -> None: 666 """ 667 Clean up after homing. Stops motor, restores saved settings, 668 updates alerts. 669 670 Args: 671 abort: True if homing was aborted or failed 672 """ 673 self._cb_stop_motor() 674 675 # Restore saved motor settings (if we saved them) 676 if (self._saved_config is not None 677 and self._cb_restore_config is not None): 678 self._cb_restore_config(self._saved_config) 679 680 # Clear homing state 681 self._is_homing = False 682 self._homing_succeeded = not abort 683 684 # Stop timers 685 self._homing_timer.stop() 686 self._stall_timer.stop() 687 688 # Update alerts 689 if abort: 690 self._homing_error_alert.setText( 691 f"{self._name}: homing aborted" 692 ) 693 self._homing_error_alert.set(True) 694 self._homing_status_alert.set(False) 695 else: 696 self._homing_error_alert.set(False) 697 # After successful homing, if calibration data exists, the 698 # encoder is now aligned so soft limits can be applied to 699 # hardware 700 if (not self._is_calibrating 701 and self._is_calibrated 702 and self._cb_set_soft_limits is not None): 703 self._cb_set_soft_limits( 704 self._min_soft_limit, self._max_soft_limit) 705 706 def _calibration_periodic(self) -> None: 707 """Manage the two-phase calibration state machine.""" 708 if not self._is_calibrating: 709 return 710 711 if self._calibration_phase == 1: 712 done = self._homing_periodic() 713 if done: 714 if self._homing_succeeded: 715 # Phase 1 complete - set encoder to 0 716 self._cb_set_position(0.0) 717 self._hard_limit_min = 0.0 718 719 if self._cal_known_range is not None: 720 self._hard_limit_max = self._cal_known_range 721 self._calibration_end(abort=False) 722 else: 723 self._cal_status_alert.setText( 724 f"{self._name}: calibration phase 2" 725 " - homing positive" 726 ) 727 self._calibration_phase = 2 728 self.homing_init( 729 self._cal_max_current, 730 self._cal_max_power_pct, 731 self._cal_max_homing_time, 732 homing_forward=True, 733 min_velocity=self._cal_min_velocity 734 ) 735 else: 736 # Homing failed (timeout) - abort calibration 737 self._calibration_end(abort=True) 738 739 elif self._calibration_phase == 2: 740 measured_position = self._cb_get_position() 741 done = self._homing_periodic() 742 if done: 743 if self._homing_succeeded: 744 self._hard_limit_max = measured_position 745 self._calibration_end(abort=False) 746 else: 747 self._calibration_end(abort=True) 748 749 def _calibration_end(self, abort: bool) -> None: 750 """ 751 Clean up after calibration. Restores motor settings and on 752 success computes and applies soft limits. 753 754 Args: 755 abort: True if calibration failed 756 """ 757 self._cb_stop_motor() 758 self._is_calibrating = False 759 self._calibration_phase = 0 760 self._is_homing = False 761 762 if not abort and self._hard_limit_max is not None: 763 self._is_calibrated = True 764 self._cal_status_alert.setText( 765 f"{self._name}: calibration complete" 766 ) 767 self.set_soft_limit_margin(self._soft_limit_margin) 768 self._save_to_nt() 769 else: 770 # Restore original config on abort 771 if (self._cal_saved_config is not None 772 and self._cb_restore_config is not None): 773 self._cb_restore_config(self._cal_saved_config) 774 self._cal_status_alert.setText( 775 f"{self._name}: calibration aborted" 776 ) 777 778 def _save_to_nt(self) -> None: 779 """Persist calibration data to NetworkTables via ntproperty.""" 780 if self._hard_limit_min is not None: 781 self._nt_hard_limit_min = self._hard_limit_min 782 if self._hard_limit_max is not None: 783 self._nt_hard_limit_max = self._hard_limit_max 784 self._nt_soft_limit_margin = self._soft_limit_margin 785 786 def _load_from_nt(self) -> None: 787 """Load persisted calibration data from ntproperty NT entries.""" 788 nt_min = self._nt_hard_limit_min 789 nt_max = self._nt_hard_limit_max 790 if not math.isnan(nt_min) and not math.isnan(nt_max): 791 self._hard_limit_min = nt_min 792 self._hard_limit_max = nt_max 793 self._soft_limit_margin = self._nt_soft_limit_margin 794 self._is_calibrated = True 795 # Compute soft limits in memory only — don't push to hardware 796 # because encoder position is unknown until homing completes 797 full_range = self._hard_limit_max - self._hard_limit_min 798 margin = full_range * self._soft_limit_margin 799 self._min_soft_limit = self._hard_limit_min + margin 800 self._max_soft_limit = self._hard_limit_max - margin
class
PositionCalibration:
31class PositionCalibration: 32 """ 33 Reusable calibration and homing controller for positional mechanisms. 34 35 Extracts sensorless or sensor homing and two-phase calibration logic 36 from mechanism-specific subsystems into a standalone, motor-agnostic 37 class. All hardware interaction is done through user-provided 38 callbacks, making this compatible with any motor controller. 39 40 For SparkMax users, the ``motor`` convenience parameter auto-populates 41 callbacks via SparkMaxCallbacks. Individual callbacks passed as keyword 42 arguments override the SparkMax defaults. 43 44 Calibration discovers mechanical hard limits by driving into stops, 45 then computes soft limits with a configurable safety margin. Persists 46 discovered limits to NetworkTables via ntproperty so values survive 47 reboots. 48 49 ## Callbacks 50 51 **Core required** (must be set before homing or calibration): 52 53 | Name | Signature | Purpose | 54 |------|-----------|---------| 55 | `set_motor_output` | `(float) -> None` | Drive motor at duty cycle (-1.0 to 1.0) | 56 | `stop_motor` | `() -> None` | Stop motor output | 57 | `set_position` | `(float) -> None` | Reset encoder position | 58 59 **Detection** (at least one required per homing direction): 60 61 | Name | Signature | Purpose | 62 |------|-----------|---------| 63 | `get_velocity` | `() -> float` | Enables stall detection | 64 | `get_forward_limit_switch` | `() -> bool` | Forward limit switch | 65 | `get_reverse_limit_switch` | `() -> bool` | Reverse limit switch | 66 67 **Optional** (enhance safety/features but not required): 68 69 | Name | Signature | Purpose | 70 |------|-----------|---------| 71 | `get_position` | `() -> float` | Required only for calibration phase 2 | 72 | `set_current_limit` | `(float) -> None` | Protective current limit during homing | 73 | `set_soft_limits` | `(min, max) -> None` | Apply soft limits to hardware | 74 | `disable_soft_limits` | `(fwd, rev) -> None` | Disable soft limits for free travel | 75 | `save_config` | `() -> Any` | Snapshot motor config before homing | 76 | `restore_config` | `(Any) -> None` | Restore snapshot after homing | 77 | `on_limit_detected` | `(pos, dir) -> None` | Fires when a hard limit is found | 78 79 Use ``get_callbacks()`` to inspect the current callback dict (returns 80 ``None`` for any callback that has not been set). 81 82 Examples 83 -------- 84 85 SparkMax convenience (auto-generates all callbacks):: 86 87 self.calibration = PositionCalibration( 88 name="Turret", 89 fallback_min=-90, fallback_max=90, 90 motor=self.motor, encoder=self.encoder) 91 92 Pure callbacks with stall detection only:: 93 94 self.calibration = PositionCalibration( 95 name="Elevator", 96 fallback_min=0.0, fallback_max=1.5, 97 set_motor_output=lambda pct: motor.set(pct), 98 stop_motor=lambda: motor.stopMotor(), 99 set_position=lambda v: encoder.setPosition(v), 100 get_velocity=lambda: encoder.getVelocity()) 101 102 Pure callbacks with limit switches only:: 103 104 self.calibration = PositionCalibration( 105 name="Arm", 106 fallback_min=0.0, fallback_max=90.0, 107 set_motor_output=lambda pct: motor.set(pct), 108 stop_motor=lambda: motor.stopMotor(), 109 set_position=lambda v: encoder.setPosition(v), 110 get_forward_limit_switch=lambda: fwd_switch.get(), 111 get_reverse_limit_switch=lambda: rev_switch.get()) 112 113 Deferred setup (set callbacks after construction):: 114 115 cal = PositionCalibration( 116 name="Wrist", 117 fallback_min=-45, fallback_max=45) 118 # ... later, once hardware is ready ... 119 cal.set_callbacks( 120 set_motor_output=motor.set, 121 stop_motor=motor.stopMotor, 122 set_position=encoder.setPosition, 123 get_velocity=encoder.getVelocity) 124 """ 125 126 # -- Callback type declarations -- 127 _cb_get_position: Optional[Callable[[], float]] 128 _cb_get_velocity: Optional[Callable[[], float]] 129 _cb_set_position: Optional[Callable[[float], None]] 130 _cb_set_motor_output: Optional[Callable[[float], None]] 131 _cb_stop_motor: Optional[Callable[[], None]] 132 _cb_set_current_limit: Optional[Callable[[float], None]] 133 _cb_set_soft_limits: Optional[Callable[[float, float], None]] 134 _cb_disable_soft_limits: Optional[Callable[[bool, bool], None]] 135 _cb_save_config: Optional[Callable[[], Any]] 136 _cb_restore_config: Optional[Callable[[Any], None]] 137 _cb_get_forward_limit_switch: Optional[Callable[[], bool]] 138 _cb_get_reverse_limit_switch: Optional[Callable[[], bool]] 139 _cb_on_limit_detected: Optional[Callable[[float, str], None]] 140 141 def __init__( 142 self, 143 name: str, 144 fallback_min: float, 145 fallback_max: float, 146 *, 147 motor=None, 148 encoder=None, 149 **kwargs, 150 ) -> None: 151 """ 152 Create a new PositionCalibration controller. 153 154 Args: 155 name: mechanism name used for NT paths and alerts (e.g. "Turret") 156 fallback_min: reverse soft limit used before calibration data 157 exists. Once calibrated, the persisted hard limits are used 158 instead. The caller is responsible for the far end of travel 159 if only homing (not full calibration) is performed. 160 fallback_max: forward soft limit used before calibration data 161 exists. See fallback_min for details. 162 motor: (optional) rev.SparkMax for convenience callback generation 163 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 164 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 165 """ 166 # Assign this instance to a cached subclass with ntproperty 167 # descriptors keyed to this mechanism's name. ntproperty is a 168 # class-level descriptor, so each mechanism name needs its own 169 # class to avoid key collisions. The cache ensures the 170 # ntproperty is created only once per name so defaults don't 171 # overwrite previously persisted values. 172 if name not in _calibration_classes: 173 prefix = f"/{name}/calibration" 174 attrs = { 175 "_nt_hard_limit_min": ntproperty( 176 f"{prefix}/hard_limit_min", float("nan"), 177 writeDefault=False, persistent=True), 178 "_nt_hard_limit_max": ntproperty( 179 f"{prefix}/hard_limit_max", float("nan"), 180 writeDefault=False, persistent=True), 181 "_nt_soft_limit_margin": ntproperty( 182 f"{prefix}/soft_limit_margin", 0.05, 183 writeDefault=False, persistent=True), 184 } 185 # Non-persistent booleans for callback status 186 for cb_name in sorted(_VALID_CALLBACKS): 187 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 188 f"{prefix}/callbacks/{cb_name}", False, 189 writeDefault=True, persistent=False) 190 _calibration_classes[name] = type( 191 f"PositionCalibration_{name}", 192 (PositionCalibration,), 193 attrs, 194 ) 195 self.__class__ = _calibration_classes[name] 196 197 self._name = name 198 199 # Soft limits — initialized to defaults, overwritten by 200 # _load_from_nt() if persisted calibration data exists 201 self._min_soft_limit = fallback_min 202 self._max_soft_limit = fallback_max 203 204 # Calibration state 205 self._hard_limit_min = None 206 self._hard_limit_max = None 207 self._is_calibrated = False 208 self._is_calibrating = False 209 self._calibration_phase = 0 210 self._soft_limit_margin = 0.05 211 self._position_offset = 0.0 212 213 # Homing state 214 self._is_homing = False 215 self._saved_config = None 216 217 # -- Build callbacks -- 218 # Validate kwargs keys 219 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 220 if unknown: 221 raise ValueError( 222 f"Unknown callback(s): {', '.join(sorted(unknown))}" 223 ) 224 225 # If motor provided, generate SparkMax defaults 226 defaults = {} 227 if motor is not None: 228 from utils.spark_max_callbacks import SparkMaxCallbacks 229 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 230 231 # Merge: explicit kwargs override SparkMax defaults 232 merged = {**defaults, **kwargs} 233 234 # Store each callback as self._cb_{name} (None if not provided) 235 for cb_name in _VALID_CALLBACKS: 236 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 237 238 # Publish callback status to NT 239 self._publish_callback_status() 240 241 # Attempt to load persisted calibration from NT 242 self._load_from_nt() 243 244 # ---- Public properties ---- 245 246 @property 247 def is_busy(self) -> bool: 248 """True when homing or calibrating (blocks motor commands).""" 249 return self._is_homing or self._is_calibrating 250 251 @property 252 def is_homing(self) -> bool: 253 """Whether a homing routine is active.""" 254 return self._is_homing 255 256 @property 257 def is_calibrating(self) -> bool: 258 """Whether a calibration routine is active.""" 259 return self._is_calibrating 260 261 @property 262 def is_calibrated(self) -> bool: 263 """Whether the full mechanical range has been discovered.""" 264 return self._is_calibrated 265 266 @property 267 def is_zeroed(self) -> bool: 268 """Whether the encoder has been aligned to a reference point.""" 269 return self._hard_limit_min is not None 270 271 @property 272 def min_limit(self): 273 """Hard limit min, or None if uncalibrated.""" 274 return self._hard_limit_min 275 276 @property 277 def max_limit(self): 278 """Hard limit max, or None if uncalibrated.""" 279 return self._hard_limit_max 280 281 @property 282 def min_soft_limit(self) -> float: 283 """Current minimum soft limit.""" 284 return self._min_soft_limit 285 286 @property 287 def max_soft_limit(self) -> float: 288 """Current maximum soft limit.""" 289 return self._max_soft_limit 290 291 @property 292 def soft_limit_margin(self) -> float: 293 """Current safety margin as a fraction of the full range.""" 294 return self._soft_limit_margin 295 296 # ---- Public methods ---- 297 298 def get_callbacks(self) -> dict: 299 """ 300 Return a dict of all callback names and their current values. 301 302 Each key is a callback name from _VALID_CALLBACKS. The value is 303 the callable if set, or None if not set. Useful for inspecting 304 which callbacks are configured:: 305 306 cbs = cal.get_callbacks() 307 for name, func in cbs.items(): 308 print(f"{name}: {'set' if func else 'not set'}") 309 310 Returns: 311 dict mapping callback name -> callable or None 312 """ 313 return { 314 name: getattr(self, f'_cb_{name}', None) 315 for name in sorted(_VALID_CALLBACKS) 316 } 317 318 def set_callbacks(self, **kwargs) -> None: 319 """ 320 Set, override, or clear one or more callbacks by name. 321 322 Pass a callable to set a callback, or None to clear it:: 323 324 # Set a callback 325 cal.set_callbacks(get_velocity=encoder.getVelocity) 326 327 # Clear callbacks that don't apply to this mechanism 328 cal.set_callbacks( 329 get_forward_limit_switch=None, 330 get_reverse_limit_switch=None) 331 332 Args: 333 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 334 Pass None to clear a callback. 335 """ 336 for key, value in kwargs.items(): 337 if key not in _VALID_CALLBACKS: 338 raise ValueError(f"Unknown callback: {key}") 339 setattr(self, f'_cb_{key}', value) 340 self._publish_callback_status() 341 342 def homing_init( 343 self, 344 max_current: float, 345 max_power_pct: float, 346 max_homing_time: float, 347 homing_forward: bool, 348 min_velocity: float = None, 349 home_position: float = 0.0 350 ) -> None: 351 """ 352 Initialize the sensorless or sensor homing routine. 353 354 Saves current motor settings (if save_config callback is set), 355 applies homing-specific configuration, and begins driving toward 356 the specified hard stop. 357 358 Args: 359 max_current: current limit in amps during homing 360 max_power_pct: motor duty cycle during homing (0.0-1.0) 361 max_homing_time: maximum time in seconds before timeout 362 homing_forward: True to home toward forward limit, False for reverse 363 min_velocity: stall detection threshold in user units/second. 364 Defaults to 5% of soft limit range over 2 seconds. 365 home_position: encoder value to set when the hard stop is 366 found (default 0.0) 367 """ 368 # Validate callbacks before starting 369 self._validate_homing(homing_forward) 370 371 if min_velocity is None: 372 full_range = self._max_soft_limit - self._min_soft_limit 373 min_velocity = full_range * 0.05 / 2.0 374 375 # Save current motor settings for restoration (if callback exists) 376 if self._cb_save_config is not None: 377 self._saved_config = self._cb_save_config() 378 else: 379 self._saved_config = None 380 381 # Apply homing current limit (if callback exists) 382 if self._cb_set_current_limit is not None: 383 self._cb_set_current_limit(max_current) 384 385 # Disable soft limit in homing direction (if callback exists) 386 if self._cb_disable_soft_limits is not None: 387 if homing_forward: 388 self._cb_disable_soft_limits(True, False) 389 else: 390 self._cb_disable_soft_limits(False, True) 391 392 # Store homing parameters 393 self._homing_forward = homing_forward 394 self._max_power_pct = max_power_pct 395 self._min_velocity = min_velocity 396 self._max_homing_time = max_homing_time 397 self._home_position = home_position 398 399 # Set homing state 400 self._is_homing = True 401 self._stall_detected = False 402 403 # Start timeout timer 404 self._homing_timer = wpilib.Timer() 405 self._homing_timer.start() 406 407 # Stall detection timer 408 self._stall_timer = wpilib.Timer() 409 410 # Alerts 411 self._homing_status_alert = wpilib.Alert( 412 f"{self._name}: homing started", 413 wpilib.Alert.AlertType.kInfo 414 ) 415 self._homing_status_alert.set(True) 416 self._homing_error_alert = wpilib.Alert( 417 f"{self._name}: homing failed", 418 wpilib.Alert.AlertType.kError 419 ) 420 self._homing_error_alert.set(False) 421 422 def calibration_init( 423 self, 424 max_current: float, 425 max_power_pct: float, 426 max_homing_time: float, 427 min_velocity: float = None, 428 known_range: float = None 429 ) -> None: 430 """ 431 Start a calibration routine that discovers the mechanical range. 432 433 Phase 1: Home negative to find the reverse hard limit (set as zero). 434 Phase 2: Home positive to find the forward hard limit (measured). 435 436 If known_range is provided, only phase 1 runs and the forward hard 437 limit is computed as known_range from the zero point. 438 439 Args: 440 max_current: current limit in amps during calibration 441 max_power_pct: motor duty cycle (0.0-1.0) 442 max_homing_time: maximum time per phase before timeout 443 min_velocity: stall detection threshold in user units/second 444 known_range: if provided, skip phase 2 and use this as the 445 full mechanical range from the zero point 446 """ 447 # Validate callbacks before starting 448 self._validate_calibration() 449 450 # Save motor settings for restoration after calibration 451 if self._cb_save_config is not None: 452 self._cal_saved_config = self._cb_save_config() 453 else: 454 self._cal_saved_config = None 455 456 # Disable both soft limits for free travel 457 if self._cb_disable_soft_limits is not None: 458 self._cb_disable_soft_limits(True, True) 459 460 # Store calibration parameters for phase transitions 461 self._cal_max_current = max_current 462 self._cal_max_power_pct = max_power_pct 463 self._cal_max_homing_time = max_homing_time 464 self._cal_min_velocity = min_velocity 465 self._cal_known_range = known_range 466 467 self._is_calibrating = True 468 self._calibration_phase = 1 469 470 # Start phase 1: home negative 471 self.homing_init( 472 max_current, max_power_pct, max_homing_time, 473 homing_forward=False, min_velocity=min_velocity 474 ) 475 476 self._cal_status_alert = wpilib.Alert( 477 f"{self._name}: calibration phase 1 - homing negative", 478 wpilib.Alert.AlertType.kInfo 479 ) 480 self._cal_status_alert.set(True) 481 482 def periodic(self) -> None: 483 """ 484 Run the active homing or calibration state machine. 485 486 Call this every cycle when is_busy is True. 487 """ 488 if self._is_calibrating: 489 self._calibration_periodic() 490 elif self._is_homing: 491 self._homing_periodic() 492 493 def abort(self) -> None: 494 """Abort any active homing or calibration routine.""" 495 if self._is_calibrating: 496 self._calibration_end(abort=True) 497 elif self._is_homing: 498 self._homing_end(abort=True) 499 500 def set_soft_limit_margin(self, margin_pct: float) -> None: 501 """ 502 Set the soft limit safety margin and apply to the motor controller. 503 504 Args: 505 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 506 """ 507 if not self._is_calibrated: 508 return 509 self._soft_limit_margin = margin_pct 510 full_range = self._hard_limit_max - self._hard_limit_min 511 margin = full_range * margin_pct 512 self._min_soft_limit = self._hard_limit_min + margin 513 self._max_soft_limit = self._hard_limit_max - margin 514 515 # Apply to hardware only if callback exists 516 if self._cb_set_soft_limits is not None: 517 self._cb_set_soft_limits( 518 self._min_soft_limit, self._max_soft_limit) 519 520 def update_telemetry(self, prefix: str) -> None: 521 """ 522 Publish calibration data to SmartDashboard. 523 524 Args: 525 prefix: SmartDashboard key prefix (e.g. "Turret/") 526 """ 527 sd = wpilib.SmartDashboard 528 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 529 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 530 sd.putBoolean(prefix + "isHoming", self._is_homing) 531 sd.putNumber( 532 prefix + "hardLimitMin", 533 self._hard_limit_min if self._hard_limit_min is not None 534 else 0.0 535 ) 536 sd.putNumber( 537 prefix + "hardLimitMax", 538 self._hard_limit_max if self._hard_limit_max is not None 539 else 0.0 540 ) 541 sd.putNumber(prefix + "positionOffset", self._position_offset) 542 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin) 543 544 # ---- Validation ---- 545 546 def _validate_homing(self, homing_forward: bool): 547 """Validate callbacks needed for homing in the given direction.""" 548 missing = [n for n in sorted(_CORE_REQUIRED) 549 if getattr(self, f'_cb_{n}', None) is None] 550 if missing: 551 raise ValueError( 552 f"Required callbacks not set: {', '.join(missing)}" 553 ) 554 555 has_velocity = self._cb_get_velocity is not None 556 if homing_forward: 557 has_limit = self._cb_get_forward_limit_switch is not None 558 else: 559 has_limit = self._cb_get_reverse_limit_switch is not None 560 561 if not has_velocity and not has_limit: 562 direction = "forward" if homing_forward else "reverse" 563 raise ValueError( 564 f"No detection method for {direction} homing: " 565 f"provide get_velocity and/or " 566 f"get_{direction}_limit_switch" 567 ) 568 569 def _validate_calibration(self): 570 """Validate callbacks needed for full calibration.""" 571 if self._cb_get_position is None: 572 raise ValueError( 573 "get_position callback required for calibration" 574 ) 575 # Need detection in both directions 576 self._validate_homing(homing_forward=False) # phase 1 577 self._validate_homing(homing_forward=True) # phase 2 578 579 # ---- Internal methods ---- 580 581 def _publish_callback_status(self) -> None: 582 """Write True/False for each callback to NT via ntproperty.""" 583 for cb_name in _VALID_CALLBACKS: 584 is_set = getattr(self, f'_cb_{cb_name}', None) is not None 585 setattr(self, f'_nt_cb_{cb_name}', is_set) 586 587 def _homing_periodic(self) -> bool: 588 """ 589 Periodic homing logic. Drives the motor, monitors for stall or 590 limit switch, and handles timeout. 591 592 Returns: 593 True when homing is complete (success or failure), False if ongoing 594 """ 595 if not self._is_homing: 596 return True 597 598 # Check timeout first 599 if self._homing_timer.hasElapsed(self._max_homing_time): 600 self._homing_error_alert.setText( 601 f"{self._name}: homing failed - timeout" 602 ) 603 self._homing_end(abort=True) 604 return True 605 606 # Check hard limit switch in homing direction (if callback exists) 607 limit_hit = False 608 if (self._homing_forward 609 and self._cb_get_forward_limit_switch is not None): 610 limit_hit = self._cb_get_forward_limit_switch() 611 elif (not self._homing_forward 612 and self._cb_get_reverse_limit_switch is not None): 613 limit_hit = self._cb_get_reverse_limit_switch() 614 615 # Drive motor in homing direction 616 if self._homing_forward: 617 self._cb_set_motor_output(self._max_power_pct) 618 else: 619 self._cb_set_motor_output(-self._max_power_pct) 620 621 # Check for limit switch hit 622 if limit_hit: 623 home_position = self._home_position 624 self._cb_set_position(home_position) 625 if self._cb_on_limit_detected is not None: 626 direction = ( 627 "forward" if self._homing_forward else "reverse" 628 ) 629 self._cb_on_limit_detected(home_position, direction) 630 self._homing_status_alert.setText( 631 f"{self._name}: homing complete" 632 ) 633 self._homing_end(abort=False) 634 return True 635 636 # Stall detection only if velocity callback provided 637 if self._cb_get_velocity is not None: 638 velocity = abs(self._cb_get_velocity()) 639 640 if velocity < self._min_velocity: 641 if not self._stall_detected: 642 self._stall_detected = True 643 self._stall_timer.restart() 644 elif self._stall_timer.hasElapsed(0.1): 645 home_position = self._home_position 646 self._cb_set_position(home_position) 647 if self._cb_on_limit_detected is not None: 648 direction = ( 649 "forward" if self._homing_forward 650 else "reverse" 651 ) 652 self._cb_on_limit_detected( 653 home_position, direction) 654 self._homing_status_alert.setText( 655 f"{self._name}: homing complete" 656 ) 657 self._homing_end(abort=False) 658 return True 659 else: 660 self._stall_detected = False 661 self._stall_timer.stop() 662 self._stall_timer.reset() 663 664 return False 665 666 def _homing_end(self, abort: bool) -> None: 667 """ 668 Clean up after homing. Stops motor, restores saved settings, 669 updates alerts. 670 671 Args: 672 abort: True if homing was aborted or failed 673 """ 674 self._cb_stop_motor() 675 676 # Restore saved motor settings (if we saved them) 677 if (self._saved_config is not None 678 and self._cb_restore_config is not None): 679 self._cb_restore_config(self._saved_config) 680 681 # Clear homing state 682 self._is_homing = False 683 self._homing_succeeded = not abort 684 685 # Stop timers 686 self._homing_timer.stop() 687 self._stall_timer.stop() 688 689 # Update alerts 690 if abort: 691 self._homing_error_alert.setText( 692 f"{self._name}: homing aborted" 693 ) 694 self._homing_error_alert.set(True) 695 self._homing_status_alert.set(False) 696 else: 697 self._homing_error_alert.set(False) 698 # After successful homing, if calibration data exists, the 699 # encoder is now aligned so soft limits can be applied to 700 # hardware 701 if (not self._is_calibrating 702 and self._is_calibrated 703 and self._cb_set_soft_limits is not None): 704 self._cb_set_soft_limits( 705 self._min_soft_limit, self._max_soft_limit) 706 707 def _calibration_periodic(self) -> None: 708 """Manage the two-phase calibration state machine.""" 709 if not self._is_calibrating: 710 return 711 712 if self._calibration_phase == 1: 713 done = self._homing_periodic() 714 if done: 715 if self._homing_succeeded: 716 # Phase 1 complete - set encoder to 0 717 self._cb_set_position(0.0) 718 self._hard_limit_min = 0.0 719 720 if self._cal_known_range is not None: 721 self._hard_limit_max = self._cal_known_range 722 self._calibration_end(abort=False) 723 else: 724 self._cal_status_alert.setText( 725 f"{self._name}: calibration phase 2" 726 " - homing positive" 727 ) 728 self._calibration_phase = 2 729 self.homing_init( 730 self._cal_max_current, 731 self._cal_max_power_pct, 732 self._cal_max_homing_time, 733 homing_forward=True, 734 min_velocity=self._cal_min_velocity 735 ) 736 else: 737 # Homing failed (timeout) - abort calibration 738 self._calibration_end(abort=True) 739 740 elif self._calibration_phase == 2: 741 measured_position = self._cb_get_position() 742 done = self._homing_periodic() 743 if done: 744 if self._homing_succeeded: 745 self._hard_limit_max = measured_position 746 self._calibration_end(abort=False) 747 else: 748 self._calibration_end(abort=True) 749 750 def _calibration_end(self, abort: bool) -> None: 751 """ 752 Clean up after calibration. Restores motor settings and on 753 success computes and applies soft limits. 754 755 Args: 756 abort: True if calibration failed 757 """ 758 self._cb_stop_motor() 759 self._is_calibrating = False 760 self._calibration_phase = 0 761 self._is_homing = False 762 763 if not abort and self._hard_limit_max is not None: 764 self._is_calibrated = True 765 self._cal_status_alert.setText( 766 f"{self._name}: calibration complete" 767 ) 768 self.set_soft_limit_margin(self._soft_limit_margin) 769 self._save_to_nt() 770 else: 771 # Restore original config on abort 772 if (self._cal_saved_config is not None 773 and self._cb_restore_config is not None): 774 self._cb_restore_config(self._cal_saved_config) 775 self._cal_status_alert.setText( 776 f"{self._name}: calibration aborted" 777 ) 778 779 def _save_to_nt(self) -> None: 780 """Persist calibration data to NetworkTables via ntproperty.""" 781 if self._hard_limit_min is not None: 782 self._nt_hard_limit_min = self._hard_limit_min 783 if self._hard_limit_max is not None: 784 self._nt_hard_limit_max = self._hard_limit_max 785 self._nt_soft_limit_margin = self._soft_limit_margin 786 787 def _load_from_nt(self) -> None: 788 """Load persisted calibration data from ntproperty NT entries.""" 789 nt_min = self._nt_hard_limit_min 790 nt_max = self._nt_hard_limit_max 791 if not math.isnan(nt_min) and not math.isnan(nt_max): 792 self._hard_limit_min = nt_min 793 self._hard_limit_max = nt_max 794 self._soft_limit_margin = self._nt_soft_limit_margin 795 self._is_calibrated = True 796 # Compute soft limits in memory only — don't push to hardware 797 # because encoder position is unknown until homing completes 798 full_range = self._hard_limit_max - self._hard_limit_min 799 margin = full_range * self._soft_limit_margin 800 self._min_soft_limit = self._hard_limit_min + margin 801 self._max_soft_limit = self._hard_limit_max - margin
Reusable calibration and homing controller for positional mechanisms.
Extracts sensorless or sensor homing and two-phase calibration logic from mechanism-specific subsystems into a standalone, motor-agnostic class. All hardware interaction is done through user-provided callbacks, making this compatible with any motor controller.
For SparkMax users, the motor convenience parameter auto-populates
callbacks via SparkMaxCallbacks. Individual callbacks passed as keyword
arguments override the SparkMax defaults.
Calibration discovers mechanical hard limits by driving into stops, then computes soft limits with a configurable safety margin. Persists discovered limits to NetworkTables via ntproperty so values survive reboots.
Callbacks
Core required (must be set before homing or calibration):
| Name | Signature | Purpose |
|---|---|---|
set_motor_output |
(float) -> None |
Drive motor at duty cycle (-1.0 to 1.0) |
stop_motor |
() -> None |
Stop motor output |
set_position |
(float) -> None |
Reset encoder position |
Detection (at least one required per homing direction):
| Name | Signature | Purpose |
|---|---|---|
get_velocity |
() -> float |
Enables stall detection |
get_forward_limit_switch |
() -> bool |
Forward limit switch |
get_reverse_limit_switch |
() -> bool |
Reverse limit switch |
Optional (enhance safety/features but not required):
| Name | Signature | Purpose |
|---|---|---|
get_position |
() -> float |
Required only for calibration phase 2 |
set_current_limit |
(float) -> None |
Protective current limit during homing |
set_soft_limits |
(min, max) -> None |
Apply soft limits to hardware |
disable_soft_limits |
(fwd, rev) -> None |
Disable soft limits for free travel |
save_config |
() -> Any |
Snapshot motor config before homing |
restore_config |
(Any) -> None |
Restore snapshot after homing |
on_limit_detected |
(pos, dir) -> None |
Fires when a hard limit is found |
Use get_callbacks() to inspect the current callback dict (returns
None for any callback that has not been set).
Examples
SparkMax convenience (auto-generates all callbacks)::
self.calibration = PositionCalibration(
name="Turret",
fallback_min=-90, fallback_max=90,
motor=self.motor, encoder=self.encoder)
Pure callbacks with stall detection only::
self.calibration = PositionCalibration(
name="Elevator",
fallback_min=0.0, fallback_max=1.5,
set_motor_output=lambda pct: motor.set(pct),
stop_motor=lambda: motor.stopMotor(),
set_position=lambda v: encoder.setPosition(v),
get_velocity=lambda: encoder.getVelocity())
Pure callbacks with limit switches only::
self.calibration = PositionCalibration(
name="Arm",
fallback_min=0.0, fallback_max=90.0,
set_motor_output=lambda pct: motor.set(pct),
stop_motor=lambda: motor.stopMotor(),
set_position=lambda v: encoder.setPosition(v),
get_forward_limit_switch=lambda: fwd_switch.get(),
get_reverse_limit_switch=lambda: rev_switch.get())
Deferred setup (set callbacks after construction)::
cal = PositionCalibration(
name="Wrist",
fallback_min=-45, fallback_max=45)
# ... later, once hardware is ready ...
cal.set_callbacks(
set_motor_output=motor.set,
stop_motor=motor.stopMotor,
set_position=encoder.setPosition,
get_velocity=encoder.getVelocity)
PositionCalibration( name: str, fallback_min: float, fallback_max: float, *, motor=None, encoder=None, **kwargs)
141 def __init__( 142 self, 143 name: str, 144 fallback_min: float, 145 fallback_max: float, 146 *, 147 motor=None, 148 encoder=None, 149 **kwargs, 150 ) -> None: 151 """ 152 Create a new PositionCalibration controller. 153 154 Args: 155 name: mechanism name used for NT paths and alerts (e.g. "Turret") 156 fallback_min: reverse soft limit used before calibration data 157 exists. Once calibrated, the persisted hard limits are used 158 instead. The caller is responsible for the far end of travel 159 if only homing (not full calibration) is performed. 160 fallback_max: forward soft limit used before calibration data 161 exists. See fallback_min for details. 162 motor: (optional) rev.SparkMax for convenience callback generation 163 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 164 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 165 """ 166 # Assign this instance to a cached subclass with ntproperty 167 # descriptors keyed to this mechanism's name. ntproperty is a 168 # class-level descriptor, so each mechanism name needs its own 169 # class to avoid key collisions. The cache ensures the 170 # ntproperty is created only once per name so defaults don't 171 # overwrite previously persisted values. 172 if name not in _calibration_classes: 173 prefix = f"/{name}/calibration" 174 attrs = { 175 "_nt_hard_limit_min": ntproperty( 176 f"{prefix}/hard_limit_min", float("nan"), 177 writeDefault=False, persistent=True), 178 "_nt_hard_limit_max": ntproperty( 179 f"{prefix}/hard_limit_max", float("nan"), 180 writeDefault=False, persistent=True), 181 "_nt_soft_limit_margin": ntproperty( 182 f"{prefix}/soft_limit_margin", 0.05, 183 writeDefault=False, persistent=True), 184 } 185 # Non-persistent booleans for callback status 186 for cb_name in sorted(_VALID_CALLBACKS): 187 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 188 f"{prefix}/callbacks/{cb_name}", False, 189 writeDefault=True, persistent=False) 190 _calibration_classes[name] = type( 191 f"PositionCalibration_{name}", 192 (PositionCalibration,), 193 attrs, 194 ) 195 self.__class__ = _calibration_classes[name] 196 197 self._name = name 198 199 # Soft limits — initialized to defaults, overwritten by 200 # _load_from_nt() if persisted calibration data exists 201 self._min_soft_limit = fallback_min 202 self._max_soft_limit = fallback_max 203 204 # Calibration state 205 self._hard_limit_min = None 206 self._hard_limit_max = None 207 self._is_calibrated = False 208 self._is_calibrating = False 209 self._calibration_phase = 0 210 self._soft_limit_margin = 0.05 211 self._position_offset = 0.0 212 213 # Homing state 214 self._is_homing = False 215 self._saved_config = None 216 217 # -- Build callbacks -- 218 # Validate kwargs keys 219 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 220 if unknown: 221 raise ValueError( 222 f"Unknown callback(s): {', '.join(sorted(unknown))}" 223 ) 224 225 # If motor provided, generate SparkMax defaults 226 defaults = {} 227 if motor is not None: 228 from utils.spark_max_callbacks import SparkMaxCallbacks 229 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 230 231 # Merge: explicit kwargs override SparkMax defaults 232 merged = {**defaults, **kwargs} 233 234 # Store each callback as self._cb_{name} (None if not provided) 235 for cb_name in _VALID_CALLBACKS: 236 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 237 238 # Publish callback status to NT 239 self._publish_callback_status() 240 241 # Attempt to load persisted calibration from NT 242 self._load_from_nt()
Create a new PositionCalibration controller.
Args: name: mechanism name used for NT paths and alerts (e.g. "Turret") fallback_min: reverse soft limit used before calibration data exists. Once calibrated, the persisted hard limits are used instead. The caller is responsible for the far end of travel if only homing (not full calibration) is performed. fallback_max: forward soft limit used before calibration data exists. See fallback_min for details. motor: (optional) rev.SparkMax for convenience callback generation encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys)
is_busy: bool
246 @property 247 def is_busy(self) -> bool: 248 """True when homing or calibrating (blocks motor commands).""" 249 return self._is_homing or self._is_calibrating
True when homing or calibrating (blocks motor commands).
is_homing: bool
251 @property 252 def is_homing(self) -> bool: 253 """Whether a homing routine is active.""" 254 return self._is_homing
Whether a homing routine is active.
is_calibrating: bool
256 @property 257 def is_calibrating(self) -> bool: 258 """Whether a calibration routine is active.""" 259 return self._is_calibrating
Whether a calibration routine is active.
is_calibrated: bool
261 @property 262 def is_calibrated(self) -> bool: 263 """Whether the full mechanical range has been discovered.""" 264 return self._is_calibrated
Whether the full mechanical range has been discovered.
is_zeroed: bool
266 @property 267 def is_zeroed(self) -> bool: 268 """Whether the encoder has been aligned to a reference point.""" 269 return self._hard_limit_min is not None
Whether the encoder has been aligned to a reference point.
min_limit
271 @property 272 def min_limit(self): 273 """Hard limit min, or None if uncalibrated.""" 274 return self._hard_limit_min
Hard limit min, or None if uncalibrated.
max_limit
276 @property 277 def max_limit(self): 278 """Hard limit max, or None if uncalibrated.""" 279 return self._hard_limit_max
Hard limit max, or None if uncalibrated.
min_soft_limit: float
281 @property 282 def min_soft_limit(self) -> float: 283 """Current minimum soft limit.""" 284 return self._min_soft_limit
Current minimum soft limit.
max_soft_limit: float
286 @property 287 def max_soft_limit(self) -> float: 288 """Current maximum soft limit.""" 289 return self._max_soft_limit
Current maximum soft limit.
soft_limit_margin: float
291 @property 292 def soft_limit_margin(self) -> float: 293 """Current safety margin as a fraction of the full range.""" 294 return self._soft_limit_margin
Current safety margin as a fraction of the full range.
def
get_callbacks(self) -> dict:
298 def get_callbacks(self) -> dict: 299 """ 300 Return a dict of all callback names and their current values. 301 302 Each key is a callback name from _VALID_CALLBACKS. The value is 303 the callable if set, or None if not set. Useful for inspecting 304 which callbacks are configured:: 305 306 cbs = cal.get_callbacks() 307 for name, func in cbs.items(): 308 print(f"{name}: {'set' if func else 'not set'}") 309 310 Returns: 311 dict mapping callback name -> callable or None 312 """ 313 return { 314 name: getattr(self, f'_cb_{name}', None) 315 for name in sorted(_VALID_CALLBACKS) 316 }
Return a dict of all callback names and their current values.
Each key is a callback name from _VALID_CALLBACKS. The value is the callable if set, or None if not set. Useful for inspecting which callbacks are configured::
cbs = cal.get_callbacks()
for name, func in cbs.items():
print(f"{name}: {'set' if func else 'not set'}")
Returns: dict mapping callback name -> callable or None
def
set_callbacks(self, **kwargs) -> None:
318 def set_callbacks(self, **kwargs) -> None: 319 """ 320 Set, override, or clear one or more callbacks by name. 321 322 Pass a callable to set a callback, or None to clear it:: 323 324 # Set a callback 325 cal.set_callbacks(get_velocity=encoder.getVelocity) 326 327 # Clear callbacks that don't apply to this mechanism 328 cal.set_callbacks( 329 get_forward_limit_switch=None, 330 get_reverse_limit_switch=None) 331 332 Args: 333 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 334 Pass None to clear a callback. 335 """ 336 for key, value in kwargs.items(): 337 if key not in _VALID_CALLBACKS: 338 raise ValueError(f"Unknown callback: {key}") 339 setattr(self, f'_cb_{key}', value) 340 self._publish_callback_status()
Set, override, or clear one or more callbacks by name.
Pass a callable to set a callback, or None to clear it::
# Set a callback
cal.set_callbacks(get_velocity=encoder.getVelocity)
# Clear callbacks that don't apply to this mechanism
cal.set_callbacks(
get_forward_limit_switch=None,
get_reverse_limit_switch=None)
Args: **kwargs: callback name/value pairs (see _VALID_CALLBACKS). Pass None to clear a callback.
def
homing_init( self, max_current: float, max_power_pct: float, max_homing_time: float, homing_forward: bool, min_velocity: float = None, home_position: float = 0.0) -> None:
342 def homing_init( 343 self, 344 max_current: float, 345 max_power_pct: float, 346 max_homing_time: float, 347 homing_forward: bool, 348 min_velocity: float = None, 349 home_position: float = 0.0 350 ) -> None: 351 """ 352 Initialize the sensorless or sensor homing routine. 353 354 Saves current motor settings (if save_config callback is set), 355 applies homing-specific configuration, and begins driving toward 356 the specified hard stop. 357 358 Args: 359 max_current: current limit in amps during homing 360 max_power_pct: motor duty cycle during homing (0.0-1.0) 361 max_homing_time: maximum time in seconds before timeout 362 homing_forward: True to home toward forward limit, False for reverse 363 min_velocity: stall detection threshold in user units/second. 364 Defaults to 5% of soft limit range over 2 seconds. 365 home_position: encoder value to set when the hard stop is 366 found (default 0.0) 367 """ 368 # Validate callbacks before starting 369 self._validate_homing(homing_forward) 370 371 if min_velocity is None: 372 full_range = self._max_soft_limit - self._min_soft_limit 373 min_velocity = full_range * 0.05 / 2.0 374 375 # Save current motor settings for restoration (if callback exists) 376 if self._cb_save_config is not None: 377 self._saved_config = self._cb_save_config() 378 else: 379 self._saved_config = None 380 381 # Apply homing current limit (if callback exists) 382 if self._cb_set_current_limit is not None: 383 self._cb_set_current_limit(max_current) 384 385 # Disable soft limit in homing direction (if callback exists) 386 if self._cb_disable_soft_limits is not None: 387 if homing_forward: 388 self._cb_disable_soft_limits(True, False) 389 else: 390 self._cb_disable_soft_limits(False, True) 391 392 # Store homing parameters 393 self._homing_forward = homing_forward 394 self._max_power_pct = max_power_pct 395 self._min_velocity = min_velocity 396 self._max_homing_time = max_homing_time 397 self._home_position = home_position 398 399 # Set homing state 400 self._is_homing = True 401 self._stall_detected = False 402 403 # Start timeout timer 404 self._homing_timer = wpilib.Timer() 405 self._homing_timer.start() 406 407 # Stall detection timer 408 self._stall_timer = wpilib.Timer() 409 410 # Alerts 411 self._homing_status_alert = wpilib.Alert( 412 f"{self._name}: homing started", 413 wpilib.Alert.AlertType.kInfo 414 ) 415 self._homing_status_alert.set(True) 416 self._homing_error_alert = wpilib.Alert( 417 f"{self._name}: homing failed", 418 wpilib.Alert.AlertType.kError 419 ) 420 self._homing_error_alert.set(False)
Initialize the sensorless or sensor homing routine.
Saves current motor settings (if save_config callback is set), applies homing-specific configuration, and begins driving toward the specified hard stop.
Args: max_current: current limit in amps during homing max_power_pct: motor duty cycle during homing (0.0-1.0) max_homing_time: maximum time in seconds before timeout homing_forward: True to home toward forward limit, False for reverse min_velocity: stall detection threshold in user units/second. Defaults to 5% of soft limit range over 2 seconds. home_position: encoder value to set when the hard stop is found (default 0.0)
def
calibration_init( self, max_current: float, max_power_pct: float, max_homing_time: float, min_velocity: float = None, known_range: float = None) -> None:
422 def calibration_init( 423 self, 424 max_current: float, 425 max_power_pct: float, 426 max_homing_time: float, 427 min_velocity: float = None, 428 known_range: float = None 429 ) -> None: 430 """ 431 Start a calibration routine that discovers the mechanical range. 432 433 Phase 1: Home negative to find the reverse hard limit (set as zero). 434 Phase 2: Home positive to find the forward hard limit (measured). 435 436 If known_range is provided, only phase 1 runs and the forward hard 437 limit is computed as known_range from the zero point. 438 439 Args: 440 max_current: current limit in amps during calibration 441 max_power_pct: motor duty cycle (0.0-1.0) 442 max_homing_time: maximum time per phase before timeout 443 min_velocity: stall detection threshold in user units/second 444 known_range: if provided, skip phase 2 and use this as the 445 full mechanical range from the zero point 446 """ 447 # Validate callbacks before starting 448 self._validate_calibration() 449 450 # Save motor settings for restoration after calibration 451 if self._cb_save_config is not None: 452 self._cal_saved_config = self._cb_save_config() 453 else: 454 self._cal_saved_config = None 455 456 # Disable both soft limits for free travel 457 if self._cb_disable_soft_limits is not None: 458 self._cb_disable_soft_limits(True, True) 459 460 # Store calibration parameters for phase transitions 461 self._cal_max_current = max_current 462 self._cal_max_power_pct = max_power_pct 463 self._cal_max_homing_time = max_homing_time 464 self._cal_min_velocity = min_velocity 465 self._cal_known_range = known_range 466 467 self._is_calibrating = True 468 self._calibration_phase = 1 469 470 # Start phase 1: home negative 471 self.homing_init( 472 max_current, max_power_pct, max_homing_time, 473 homing_forward=False, min_velocity=min_velocity 474 ) 475 476 self._cal_status_alert = wpilib.Alert( 477 f"{self._name}: calibration phase 1 - homing negative", 478 wpilib.Alert.AlertType.kInfo 479 ) 480 self._cal_status_alert.set(True)
Start a calibration routine that discovers the mechanical range.
Phase 1: Home negative to find the reverse hard limit (set as zero). Phase 2: Home positive to find the forward hard limit (measured).
If known_range is provided, only phase 1 runs and the forward hard limit is computed as known_range from the zero point.
Args: max_current: current limit in amps during calibration max_power_pct: motor duty cycle (0.0-1.0) max_homing_time: maximum time per phase before timeout min_velocity: stall detection threshold in user units/second known_range: if provided, skip phase 2 and use this as the full mechanical range from the zero point
def
periodic(self) -> None:
482 def periodic(self) -> None: 483 """ 484 Run the active homing or calibration state machine. 485 486 Call this every cycle when is_busy is True. 487 """ 488 if self._is_calibrating: 489 self._calibration_periodic() 490 elif self._is_homing: 491 self._homing_periodic()
Run the active homing or calibration state machine.
Call this every cycle when is_busy is True.
def
abort(self) -> None:
493 def abort(self) -> None: 494 """Abort any active homing or calibration routine.""" 495 if self._is_calibrating: 496 self._calibration_end(abort=True) 497 elif self._is_homing: 498 self._homing_end(abort=True)
Abort any active homing or calibration routine.
def
set_soft_limit_margin(self, margin_pct: float) -> None:
500 def set_soft_limit_margin(self, margin_pct: float) -> None: 501 """ 502 Set the soft limit safety margin and apply to the motor controller. 503 504 Args: 505 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 506 """ 507 if not self._is_calibrated: 508 return 509 self._soft_limit_margin = margin_pct 510 full_range = self._hard_limit_max - self._hard_limit_min 511 margin = full_range * margin_pct 512 self._min_soft_limit = self._hard_limit_min + margin 513 self._max_soft_limit = self._hard_limit_max - margin 514 515 # Apply to hardware only if callback exists 516 if self._cb_set_soft_limits is not None: 517 self._cb_set_soft_limits( 518 self._min_soft_limit, self._max_soft_limit)
Set the soft limit safety margin and apply to the motor controller.
Args: margin_pct: safety margin as a fraction (e.g. 0.05 for 5%)
def
update_telemetry(self, prefix: str) -> None:
520 def update_telemetry(self, prefix: str) -> None: 521 """ 522 Publish calibration data to SmartDashboard. 523 524 Args: 525 prefix: SmartDashboard key prefix (e.g. "Turret/") 526 """ 527 sd = wpilib.SmartDashboard 528 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 529 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 530 sd.putBoolean(prefix + "isHoming", self._is_homing) 531 sd.putNumber( 532 prefix + "hardLimitMin", 533 self._hard_limit_min if self._hard_limit_min is not None 534 else 0.0 535 ) 536 sd.putNumber( 537 prefix + "hardLimitMax", 538 self._hard_limit_max if self._hard_limit_max is not None 539 else 0.0 540 ) 541 sd.putNumber(prefix + "positionOffset", self._position_offset) 542 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin)
Publish calibration data to SmartDashboard.
Args: prefix: SmartDashboard key prefix (e.g. "Turret/")