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 'set_conversion_factor', 25} 26 27# Callbacks that must always be set before homing or calibration 28_CORE_REQUIRED = {'set_motor_output', 'stop_motor', 'set_position'} 29 30 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 | `set_conversion_factor` | `(float) -> None` | Apply a new encoder position conversion factor | 79 80 Use ``get_callbacks()`` to inspect the current callback dict (returns 81 ``None`` for any callback that has not been set). 82 83 Examples 84 -------- 85 86 SparkMax convenience (auto-generates all callbacks):: 87 88 self.calibration = PositionCalibration( 89 name="Turret", 90 fallback_min=-90, fallback_max=90, 91 motor=self.motor, encoder=self.encoder) 92 93 Pure callbacks with stall detection only:: 94 95 self.calibration = PositionCalibration( 96 name="Elevator", 97 fallback_min=0.0, fallback_max=1.5, 98 set_motor_output=lambda pct: motor.set(pct), 99 stop_motor=lambda: motor.stopMotor(), 100 set_position=lambda v: encoder.setPosition(v), 101 get_velocity=lambda: encoder.getVelocity()) 102 103 Pure callbacks with limit switches only:: 104 105 self.calibration = PositionCalibration( 106 name="Arm", 107 fallback_min=0.0, fallback_max=90.0, 108 set_motor_output=lambda pct: motor.set(pct), 109 stop_motor=lambda: motor.stopMotor(), 110 set_position=lambda v: encoder.setPosition(v), 111 get_forward_limit_switch=lambda: fwd_switch.get(), 112 get_reverse_limit_switch=lambda: rev_switch.get()) 113 114 Deferred setup (set callbacks after construction):: 115 116 cal = PositionCalibration( 117 name="Wrist", 118 fallback_min=-45, fallback_max=45) 119 # ... later, once hardware is ready ... 120 cal.set_callbacks( 121 set_motor_output=motor.set, 122 stop_motor=motor.stopMotor, 123 set_position=encoder.setPosition, 124 get_velocity=encoder.getVelocity) 125 """ 126 127 # -- Callback type declarations -- 128 _cb_get_position: Optional[Callable[[], float]] 129 _cb_get_velocity: Optional[Callable[[], float]] 130 _cb_set_position: Optional[Callable[[float], None]] 131 _cb_set_motor_output: Optional[Callable[[float], None]] 132 _cb_stop_motor: Optional[Callable[[], None]] 133 _cb_set_current_limit: Optional[Callable[[float], None]] 134 _cb_set_soft_limits: Optional[Callable[[float, float], None]] 135 _cb_disable_soft_limits: Optional[Callable[[bool, bool], None]] 136 _cb_save_config: Optional[Callable[[], Any]] 137 _cb_restore_config: Optional[Callable[[Any], None]] 138 _cb_get_forward_limit_switch: Optional[Callable[[], bool]] 139 _cb_get_reverse_limit_switch: Optional[Callable[[], bool]] 140 _cb_on_limit_detected: Optional[Callable[[float, str], None]] 141 _cb_set_conversion_factor: Optional[Callable[[float], None]] 142 143 def __init__( 144 self, 145 name: str, 146 fallback_min: float, 147 fallback_max: float, 148 *, 149 motor=None, 150 encoder=None, 151 **kwargs, 152 ) -> None: 153 """ 154 Create a new PositionCalibration controller. 155 156 Args: 157 name: mechanism name used for NT paths and alerts (e.g. "Turret") 158 fallback_min: reverse soft limit used before calibration data 159 exists. Once calibrated, the persisted hard limits are used 160 instead. The caller is responsible for the far end of travel 161 if only homing (not full calibration) is performed. 162 fallback_max: forward soft limit used before calibration data 163 exists. See fallback_min for details. 164 motor: (optional) rev.SparkMax for convenience callback generation 165 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 166 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 167 """ 168 # Assign this instance to a cached subclass with ntproperty 169 # descriptors keyed to this mechanism's name. ntproperty is a 170 # class-level descriptor, so each mechanism name needs its own 171 # class to avoid key collisions. The cache ensures the 172 # ntproperty is created only once per name so defaults don't 173 # overwrite previously persisted values. 174 if name not in _calibration_classes: 175 prefix = f"/{name}/calibration" 176 attrs = { 177 "_nt_hard_limit_min": ntproperty( 178 f"{prefix}/hard_limit_min", float("nan"), 179 writeDefault=False, persistent=True), 180 "_nt_hard_limit_max": ntproperty( 181 f"{prefix}/hard_limit_max", float("nan"), 182 writeDefault=False, persistent=True), 183 "_nt_soft_limit_margin": ntproperty( 184 f"{prefix}/soft_limit_margin", 0.05, 185 writeDefault=False, persistent=True), 186 } 187 # Non-persistent booleans for callback status 188 for cb_name in sorted(_VALID_CALLBACKS): 189 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 190 f"{prefix}/callbacks/{cb_name}", False, 191 writeDefault=True, persistent=False) 192 _calibration_classes[name] = type( 193 f"PositionCalibration_{name}", 194 (PositionCalibration,), 195 attrs, 196 ) 197 self.__class__ = _calibration_classes[name] 198 199 self._name = name 200 201 # Soft limits — initialized to defaults, overwritten by 202 # _load_from_nt() if persisted calibration data exists 203 self._min_soft_limit = fallback_min 204 self._max_soft_limit = fallback_max 205 206 # Calibration state 207 self._hard_limit_min = None 208 self._hard_limit_max = None 209 self._is_calibrated = False 210 self._is_calibrating = False 211 self._calibration_phase = 0 212 self._soft_limit_margin = 0.05 213 self._position_offset = 0.0 214 215 # Homing state 216 self._is_homing = False 217 self._saved_config = None 218 219 # -- Build callbacks -- 220 # Validate kwargs keys 221 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 222 if unknown: 223 raise ValueError( 224 f"Unknown callback(s): {', '.join(sorted(unknown))}" 225 ) 226 227 # If motor provided, generate SparkMax defaults 228 defaults = {} 229 if motor is not None: 230 from utils.spark_max_callbacks import SparkMaxCallbacks 231 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 232 233 # Merge: explicit kwargs override SparkMax defaults 234 merged = {**defaults, **kwargs} 235 236 # Store each callback as self._cb_{name} (None if not provided) 237 for cb_name in _VALID_CALLBACKS: 238 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 239 240 # Publish callback status to NT 241 self._publish_callback_status() 242 243 # Attempt to load persisted calibration from NT 244 self._load_from_nt() 245 246 # ---- Public properties ---- 247 248 @property 249 def is_busy(self) -> bool: 250 """True when homing or calibrating (blocks motor commands).""" 251 return self._is_homing or self._is_calibrating 252 253 @property 254 def is_homing(self) -> bool: 255 """Whether a homing routine is active.""" 256 return self._is_homing 257 258 @property 259 def is_calibrating(self) -> bool: 260 """Whether a calibration routine is active.""" 261 return self._is_calibrating 262 263 @property 264 def is_calibrated(self) -> bool: 265 """Whether the full mechanical range has been discovered.""" 266 return self._is_calibrated 267 268 @property 269 def is_zeroed(self) -> bool: 270 """Whether the encoder has been aligned to a reference point.""" 271 return self._hard_limit_min is not None 272 273 @property 274 def min_limit(self): 275 """Hard limit min, or None if uncalibrated.""" 276 return self._hard_limit_min 277 278 @property 279 def max_limit(self): 280 """Hard limit max, or None if uncalibrated.""" 281 return self._hard_limit_max 282 283 @property 284 def min_soft_limit(self) -> float: 285 """Current minimum soft limit.""" 286 return self._min_soft_limit 287 288 @property 289 def max_soft_limit(self) -> float: 290 """Current maximum soft limit.""" 291 return self._max_soft_limit 292 293 @property 294 def soft_limit_margin(self) -> float: 295 """Current safety margin as a fraction of the full range.""" 296 return self._soft_limit_margin 297 298 # ---- Public methods ---- 299 300 def get_callbacks(self) -> dict: 301 """ 302 Return a dict of all callback names and their current values. 303 304 Each key is a callback name from _VALID_CALLBACKS. The value is 305 the callable if set, or None if not set. Useful for inspecting 306 which callbacks are configured:: 307 308 cbs = cal.get_callbacks() 309 for name, func in cbs.items(): 310 print(f"{name}: {'set' if func else 'not set'}") 311 312 Returns: 313 dict mapping callback name -> callable or None 314 """ 315 return { 316 name: getattr(self, f'_cb_{name}', None) 317 for name in sorted(_VALID_CALLBACKS) 318 } 319 320 def set_callbacks(self, **kwargs) -> None: 321 """ 322 Set, override, or clear one or more callbacks by name. 323 324 Pass a callable to set a callback, or None to clear it:: 325 326 # Set a callback 327 cal.set_callbacks(get_velocity=encoder.getVelocity) 328 329 # Clear callbacks that don't apply to this mechanism 330 cal.set_callbacks( 331 get_forward_limit_switch=None, 332 get_reverse_limit_switch=None) 333 334 Args: 335 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 336 Pass None to clear a callback. 337 """ 338 for key, value in kwargs.items(): 339 if key not in _VALID_CALLBACKS: 340 raise ValueError(f"Unknown callback: {key}") 341 setattr(self, f'_cb_{key}', value) 342 self._publish_callback_status() 343 344 def homing_init( 345 self, 346 max_current: float, 347 max_power_pct: float, 348 max_homing_time: float, 349 homing_forward: bool, 350 min_velocity: float = None, 351 home_position: float = 0.0, 352 max_travel: Optional[float] = None, 353 min_trigger_travel: float = 0.0, 354 ) -> None: 355 """ 356 Initialize the sensorless or sensor homing routine. 357 358 Saves current motor settings (if save_config callback is set), 359 applies homing-specific configuration, and begins driving toward 360 the specified hard stop. 361 362 Args: 363 max_current: current limit in amps during homing 364 max_power_pct: motor duty cycle during homing (0.0-1.0) 365 max_homing_time: maximum time in seconds before timeout 366 homing_forward: True to home toward forward limit, False for reverse 367 min_velocity: stall detection threshold in user units/second. 368 Defaults to 5% of soft limit range over 2 seconds. 369 home_position: encoder value to set when the hard stop is 370 found (default 0.0) 371 max_travel: abort if encoder travels more than this many user 372 units from the start position. Catches a stuck-open limit 373 switch that never triggers. None disables the check — 374 useful when the mechanical range is unknown. Requires 375 ``get_position`` callback to take effect. 376 min_trigger_travel: minimum encoder travel (user units) 377 required before a limit switch trigger is trusted. If 378 the switch fires before this much movement, abort with 379 a stuck-closed-switch alert. 0.0 disables (default). 380 Requires ``get_position`` callback to take effect. 381 """ 382 # Validate callbacks before starting 383 self._validate_homing(homing_forward) 384 385 if min_velocity is None: 386 full_range = self._max_soft_limit - self._min_soft_limit 387 min_velocity = full_range * 0.05 / 2.0 388 389 # Save current motor settings for restoration (if callback exists) 390 if self._cb_save_config is not None: 391 self._saved_config = self._cb_save_config() 392 else: 393 self._saved_config = None 394 395 # Apply homing current limit (if callback exists) 396 if self._cb_set_current_limit is not None: 397 self._cb_set_current_limit(max_current) 398 399 # Disable soft limit in homing direction (if callback exists) 400 if self._cb_disable_soft_limits is not None: 401 if homing_forward: 402 self._cb_disable_soft_limits(True, False) 403 else: 404 self._cb_disable_soft_limits(False, True) 405 406 # Store homing parameters 407 self._homing_forward = homing_forward 408 self._max_power_pct = max_power_pct 409 self._min_velocity = min_velocity 410 self._max_homing_time = max_homing_time 411 self._home_position = home_position 412 self._max_travel = max_travel 413 self._min_trigger_travel = min_trigger_travel 414 415 # Capture start position for travel tracking. Only meaningful if 416 # get_position is wired; otherwise both travel-based guards are 417 # silently no-ops. 418 if self._cb_get_position is not None: 419 self._homing_start_pos = self._cb_get_position() 420 else: 421 self._homing_start_pos = None 422 423 # Set homing state 424 self._is_homing = True 425 self._stall_detected = False 426 427 # Start timeout timer 428 self._homing_timer = wpilib.Timer() 429 self._homing_timer.start() 430 431 # Stall detection timer 432 self._stall_timer = wpilib.Timer() 433 434 # Alerts 435 self._homing_status_alert = wpilib.Alert( 436 f"{self._name}: homing started", 437 wpilib.Alert.AlertType.kInfo 438 ) 439 self._homing_status_alert.set(True) 440 self._homing_error_alert = wpilib.Alert( 441 f"{self._name}: homing failed", 442 wpilib.Alert.AlertType.kError 443 ) 444 self._homing_error_alert.set(False) 445 446 def calibration_init( 447 self, 448 max_current: float, 449 max_power_pct: float, 450 max_homing_time: float, 451 min_velocity: float = None, 452 known_range: float = None, 453 max_travel: Optional[float] = None, 454 min_trigger_travel: float = 0.0, 455 ) -> None: 456 """ 457 Start a calibration routine that discovers the mechanical range. 458 459 Phase 1: Home negative to find the reverse hard limit (set as zero). 460 Phase 2: Home positive to find the forward hard limit (measured). 461 462 If known_range is provided, only phase 1 runs and the forward hard 463 limit is computed as known_range from the zero point. 464 465 Args: 466 max_current: current limit in amps during calibration 467 max_power_pct: motor duty cycle (0.0-1.0) 468 max_homing_time: maximum time per phase before timeout 469 min_velocity: stall detection threshold in user units/second 470 known_range: if provided, skip phase 2 and use this as the 471 full mechanical range from the zero point 472 max_travel: abort a homing phase if encoder travels more 473 than this from its phase start. Defaults to 110% of the 474 current soft-limit range (a sane bound for calibration 475 because the expected range is known). Pass an explicit 476 None via the kwarg to disable the check entirely. 477 min_trigger_travel: minimum travel required before a limit 478 switch trigger is trusted per phase. Guards against a 479 stuck-closed switch firing immediately. 0.0 disables. 480 """ 481 # Validate callbacks before starting 482 self._validate_calibration() 483 484 # Save motor settings for restoration after calibration 485 if self._cb_save_config is not None: 486 self._cal_saved_config = self._cb_save_config() 487 else: 488 self._cal_saved_config = None 489 490 # Disable both soft limits for free travel 491 if self._cb_disable_soft_limits is not None: 492 self._cb_disable_soft_limits(True, True) 493 494 # Default max_travel to 110% of the expected range. Each phase 495 # homes one direction, so the full range is the per-phase ceiling. 496 if max_travel is None: 497 expected_range = abs(self._max_soft_limit - self._min_soft_limit) 498 if expected_range > 0: 499 max_travel = expected_range * 1.1 500 501 # Store calibration parameters for phase transitions 502 self._cal_max_current = max_current 503 self._cal_max_power_pct = max_power_pct 504 self._cal_max_homing_time = max_homing_time 505 self._cal_min_velocity = min_velocity 506 self._cal_known_range = known_range 507 self._cal_max_travel = max_travel 508 self._cal_min_trigger_travel = min_trigger_travel 509 510 self._is_calibrating = True 511 self._calibration_phase = 1 512 513 # Start phase 1: home negative 514 self.homing_init( 515 max_current, max_power_pct, max_homing_time, 516 homing_forward=False, min_velocity=min_velocity, 517 max_travel=max_travel, 518 min_trigger_travel=min_trigger_travel, 519 ) 520 521 self._cal_status_alert = wpilib.Alert( 522 f"{self._name}: calibration phase 1 - homing negative", 523 wpilib.Alert.AlertType.kInfo 524 ) 525 self._cal_status_alert.set(True) 526 527 def periodic(self) -> None: 528 """ 529 Run the active homing or calibration state machine. 530 531 Call this every cycle when is_busy is True. 532 """ 533 if self._is_calibrating: 534 self._calibration_periodic() 535 elif self._is_homing: 536 self._homing_periodic() 537 538 def abort(self) -> None: 539 """Abort any active homing or calibration routine.""" 540 if self._is_calibrating: 541 self._calibration_end(abort=True) 542 elif self._is_homing: 543 self._homing_end(abort=True) 544 545 def set_soft_limit_margin(self, margin_pct: float) -> None: 546 """ 547 Set the soft limit safety margin and apply to the motor controller. 548 549 Args: 550 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 551 """ 552 if not self._is_calibrated: 553 return 554 self._soft_limit_margin = margin_pct 555 full_range = self._hard_limit_max - self._hard_limit_min 556 margin = full_range * margin_pct 557 self._min_soft_limit = self._hard_limit_min + margin 558 self._max_soft_limit = self._hard_limit_max - margin 559 560 # Apply to hardware only if callback exists 561 if self._cb_set_soft_limits is not None: 562 self._cb_set_soft_limits( 563 self._min_soft_limit, self._max_soft_limit) 564 565 def compute_conversion_factor( 566 self, 567 current_factor: float, 568 desired_min: float, 569 desired_max: float, 570 ) -> float: 571 """ 572 Compute a corrected encoder conversion factor after calibration. 573 574 After a full calibration discovers the raw mechanical range, this 575 method computes the conversion factor needed so the measured 576 range maps to ``[desired_min, desired_max]``. 577 578 Example: if calibration measured 500 user-units of travel with 579 the initial conversion factor and you want -90..90 (180°), the 580 corrected factor scales the encoder so full travel reads 180°. 581 582 Args: 583 current_factor: the position conversion factor currently set 584 on the encoder 585 desired_min: target minimum position in user units 586 desired_max: target maximum position in user units 587 588 Returns: 589 The corrected conversion factor to apply to the encoder. 590 591 Raises: 592 RuntimeError: if calibration has not completed, or the 593 measured range is zero. 594 """ 595 if not self._is_calibrated: 596 raise RuntimeError( 597 f"{self._name}: cannot compute conversion factor " 598 "before calibration is complete" 599 ) 600 measured_range = self._hard_limit_max - self._hard_limit_min 601 if measured_range == 0: 602 raise RuntimeError( 603 f"{self._name}: measured range is zero, " 604 "calibration data invalid" 605 ) 606 desired_range = desired_max - desired_min 607 return current_factor * (desired_range / measured_range) 608 609 def apply_conversion_factor( 610 self, 611 current_factor: float, 612 desired_min: float, 613 desired_max: float, 614 ) -> float: 615 """ 616 Compute and apply a corrected encoder conversion factor. 617 618 Calls :meth:`compute_conversion_factor` for the new value, then 619 applies it via the ``set_conversion_factor`` callback, resets 620 the encoder to ``desired_min`` (the new zero reference), and 621 rewrites the internal hard/soft limits to match the desired 622 range. 623 624 This is intended to be called immediately after a full 625 calibration completes (encoder at the reverse hard stop, 626 reading 0). A guard rejects the call if the encoder reports 627 more than 5% of the current soft-limit range away from zero — 628 the mechanism has moved since calibration, so the post-rescale 629 encoder zero would not correspond to a known physical position. 630 631 Args: 632 current_factor: the position conversion factor currently set 633 on the encoder 634 desired_min: target minimum position in user units (becomes 635 the new zero reference) 636 desired_max: target maximum position in user units 637 638 Returns: 639 The corrected conversion factor that was applied. 640 641 Raises: 642 ValueError: if the ``set_conversion_factor`` callback is 643 not configured. 644 RuntimeError: if calibration has not produced a zero 645 reference, or the mechanism is not at that zero point. 646 """ 647 if self._cb_set_conversion_factor is None: 648 raise ValueError( 649 "set_conversion_factor callback not set" 650 ) 651 652 if not self.is_zeroed: 653 raise RuntimeError( 654 f"{self._name}: cannot apply conversion factor " 655 "before homing to zero position" 656 ) 657 658 # Verify the mechanism is still at the reverse hard stop 659 # (encoder reads ~0) before rescaling. Without this, the post- 660 # rescale encoder zero corresponds to no known physical point. 661 if self._cb_get_position is not None: 662 pos = abs(self._cb_get_position()) 663 full_range = self._max_soft_limit - self._min_soft_limit 664 tolerance = abs(full_range) * 0.05 665 if pos > tolerance: 666 raise RuntimeError( 667 f"{self._name}: mechanism is not at the zero " 668 f"position (current: {pos:.1f}, tolerance: " 669 f"{tolerance:.1f}). Home the mechanism before " 670 "applying a new conversion factor" 671 ) 672 673 new_factor = self.compute_conversion_factor( 674 current_factor, desired_min, desired_max 675 ) 676 self._cb_set_conversion_factor(new_factor) 677 678 # The encoder unit-scale just changed, so its raw reading no 679 # longer maps to the same physical position. Reseed to 680 # desired_min (we calibrated from the reverse hard stop) and 681 # rewrite limits to match the desired range. 682 self._cb_set_position(desired_min) 683 self._hard_limit_min = desired_min 684 self._hard_limit_max = desired_max 685 self.set_soft_limit_margin(self._soft_limit_margin) 686 self._save_to_nt() 687 688 return new_factor 689 690 def update_telemetry(self, prefix: str) -> None: 691 """ 692 Publish calibration data to SmartDashboard. 693 694 Args: 695 prefix: SmartDashboard key prefix (e.g. "Turret/") 696 """ 697 sd = wpilib.SmartDashboard 698 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 699 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 700 sd.putBoolean(prefix + "isHoming", self._is_homing) 701 sd.putNumber( 702 prefix + "hardLimitMin", 703 self._hard_limit_min if self._hard_limit_min is not None 704 else 0.0 705 ) 706 sd.putNumber( 707 prefix + "hardLimitMax", 708 self._hard_limit_max if self._hard_limit_max is not None 709 else 0.0 710 ) 711 sd.putNumber(prefix + "positionOffset", self._position_offset) 712 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin) 713 714 # ---- Validation ---- 715 716 def _validate_homing(self, homing_forward: bool): 717 """Validate callbacks needed for homing in the given direction.""" 718 missing = [n for n in sorted(_CORE_REQUIRED) 719 if getattr(self, f'_cb_{n}', None) is None] 720 if missing: 721 raise ValueError( 722 f"Required callbacks not set: {', '.join(missing)}" 723 ) 724 725 has_velocity = self._cb_get_velocity is not None 726 if homing_forward: 727 has_limit = self._cb_get_forward_limit_switch is not None 728 else: 729 has_limit = self._cb_get_reverse_limit_switch is not None 730 731 if not has_velocity and not has_limit: 732 direction = "forward" if homing_forward else "reverse" 733 raise ValueError( 734 f"No detection method for {direction} homing: " 735 f"provide get_velocity and/or " 736 f"get_{direction}_limit_switch" 737 ) 738 739 def _validate_calibration(self): 740 """Validate callbacks needed for full calibration.""" 741 if self._cb_get_position is None: 742 raise ValueError( 743 "get_position callback required for calibration" 744 ) 745 # Need detection in both directions 746 self._validate_homing(homing_forward=False) # phase 1 747 self._validate_homing(homing_forward=True) # phase 2 748 749 # ---- Internal methods ---- 750 751 def _publish_callback_status(self) -> None: 752 """Write True/False for each callback to NT via ntproperty.""" 753 for cb_name in _VALID_CALLBACKS: 754 is_set = getattr(self, f'_cb_{cb_name}', None) is not None 755 setattr(self, f'_nt_cb_{cb_name}', is_set) 756 757 def _homing_periodic(self) -> bool: 758 """ 759 Periodic homing logic. Drives the motor, monitors for stall or 760 limit switch, and handles timeout. 761 762 Returns: 763 True when homing is complete (success or failure), False if ongoing 764 """ 765 if not self._is_homing: 766 return True 767 768 # Check timeout first 769 if self._homing_timer.hasElapsed(self._max_homing_time): 770 self._homing_error_alert.setText( 771 f"{self._name}: homing failed - timeout" 772 ) 773 self._homing_end(abort=True) 774 return True 775 776 # Overtravel guard: abort if encoder has moved more than expected. 777 # Catches a stuck-open limit switch that never triggers and a 778 # broken stall-detection path. Requires get_position to take 779 # effect; otherwise it's a silent no-op. 780 if (self._max_travel is not None 781 and self._homing_start_pos is not None 782 and self._cb_get_position is not None): 783 travel = abs(self._cb_get_position() - self._homing_start_pos) 784 if travel > self._max_travel: 785 self._homing_error_alert.setText( 786 f"{self._name}: homing aborted - overtravel " 787 f"({travel:.1f} > {self._max_travel:.1f})" 788 ) 789 self._homing_end(abort=True) 790 return True 791 792 # Check hard limit switch in homing direction (if callback exists) 793 limit_hit = False 794 if (self._homing_forward 795 and self._cb_get_forward_limit_switch is not None): 796 limit_hit = self._cb_get_forward_limit_switch() 797 elif (not self._homing_forward 798 and self._cb_get_reverse_limit_switch is not None): 799 limit_hit = self._cb_get_reverse_limit_switch() 800 801 # Drive motor in homing direction 802 if self._homing_forward: 803 self._cb_set_motor_output(self._max_power_pct) 804 else: 805 self._cb_set_motor_output(-self._max_power_pct) 806 807 # Check for limit switch hit 808 if limit_hit: 809 # Stuck-closed guard: if the switch fires before we've moved 810 # enough to be credible, treat it as a false trigger. 811 if (self._min_trigger_travel > 0.0 812 and self._homing_start_pos is not None 813 and self._cb_get_position is not None): 814 travel = abs( 815 self._cb_get_position() - self._homing_start_pos) 816 if travel < self._min_trigger_travel: 817 self._homing_error_alert.setText( 818 f"{self._name}: homing aborted - limit switch " 819 f"triggered after only {travel:.1f} of " 820 f"{self._min_trigger_travel:.1f} min travel " 821 "(may be stuck closed)" 822 ) 823 self._homing_end(abort=True) 824 return True 825 826 home_position = self._home_position 827 self._cb_set_position(home_position) 828 if self._cb_on_limit_detected is not None: 829 direction = ( 830 "forward" if self._homing_forward else "reverse" 831 ) 832 self._cb_on_limit_detected(home_position, direction) 833 self._homing_status_alert.setText( 834 f"{self._name}: homing complete" 835 ) 836 self._homing_end(abort=False) 837 return True 838 839 # Stall detection only if velocity callback provided 840 if self._cb_get_velocity is not None: 841 velocity = abs(self._cb_get_velocity()) 842 843 if velocity < self._min_velocity: 844 if not self._stall_detected: 845 self._stall_detected = True 846 self._stall_timer.restart() 847 elif self._stall_timer.hasElapsed(0.1): 848 home_position = self._home_position 849 self._cb_set_position(home_position) 850 if self._cb_on_limit_detected is not None: 851 direction = ( 852 "forward" if self._homing_forward 853 else "reverse" 854 ) 855 self._cb_on_limit_detected( 856 home_position, direction) 857 self._homing_status_alert.setText( 858 f"{self._name}: homing complete" 859 ) 860 self._homing_end(abort=False) 861 return True 862 else: 863 self._stall_detected = False 864 self._stall_timer.stop() 865 self._stall_timer.reset() 866 867 return False 868 869 def _homing_end(self, abort: bool) -> None: 870 """ 871 Clean up after homing. Stops motor, restores saved settings, 872 updates alerts. 873 874 Args: 875 abort: True if homing was aborted or failed 876 """ 877 self._cb_stop_motor() 878 879 # Restore saved motor settings (if we saved them) 880 if (self._saved_config is not None 881 and self._cb_restore_config is not None): 882 self._cb_restore_config(self._saved_config) 883 884 # Clear homing state 885 self._is_homing = False 886 self._homing_succeeded = not abort 887 888 # Stop timers 889 self._homing_timer.stop() 890 self._stall_timer.stop() 891 892 # Update alerts 893 if abort: 894 self._homing_error_alert.setText( 895 f"{self._name}: homing aborted" 896 ) 897 self._homing_error_alert.set(True) 898 self._homing_status_alert.set(False) 899 else: 900 self._homing_error_alert.set(False) 901 # After successful homing, if calibration data exists, the 902 # encoder is now aligned so soft limits can be applied to 903 # hardware 904 if (not self._is_calibrating 905 and self._is_calibrated 906 and self._cb_set_soft_limits is not None): 907 self._cb_set_soft_limits( 908 self._min_soft_limit, self._max_soft_limit) 909 910 def _calibration_periodic(self) -> None: 911 """Manage the two-phase calibration state machine.""" 912 if not self._is_calibrating: 913 return 914 915 if self._calibration_phase == 1: 916 done = self._homing_periodic() 917 if done: 918 if self._homing_succeeded: 919 # Phase 1 complete - set encoder to 0 920 self._cb_set_position(0.0) 921 self._hard_limit_min = 0.0 922 923 if self._cal_known_range is not None: 924 self._hard_limit_max = self._cal_known_range 925 self._calibration_end(abort=False) 926 else: 927 self._cal_status_alert.setText( 928 f"{self._name}: calibration phase 2" 929 " - homing positive" 930 ) 931 self._calibration_phase = 2 932 self.homing_init( 933 self._cal_max_current, 934 self._cal_max_power_pct, 935 self._cal_max_homing_time, 936 homing_forward=True, 937 min_velocity=self._cal_min_velocity, 938 max_travel=self._cal_max_travel, 939 min_trigger_travel=( 940 self._cal_min_trigger_travel), 941 ) 942 else: 943 # Homing failed (timeout) - abort calibration 944 self._calibration_end(abort=True) 945 946 elif self._calibration_phase == 2: 947 measured_position = self._cb_get_position() 948 done = self._homing_periodic() 949 if done: 950 if self._homing_succeeded: 951 self._hard_limit_max = measured_position 952 self._calibration_end(abort=False) 953 else: 954 self._calibration_end(abort=True) 955 956 def _calibration_end(self, abort: bool) -> None: 957 """ 958 Clean up after calibration. Restores motor settings and on 959 success computes and applies soft limits. 960 961 Args: 962 abort: True if calibration failed 963 """ 964 self._cb_stop_motor() 965 self._is_calibrating = False 966 self._calibration_phase = 0 967 self._is_homing = False 968 969 if not abort and self._hard_limit_max is not None: 970 self._is_calibrated = True 971 self._cal_status_alert.setText( 972 f"{self._name}: calibration complete" 973 ) 974 self.set_soft_limit_margin(self._soft_limit_margin) 975 self._save_to_nt() 976 else: 977 # Restore original config on abort 978 if (self._cal_saved_config is not None 979 and self._cb_restore_config is not None): 980 self._cb_restore_config(self._cal_saved_config) 981 self._cal_status_alert.setText( 982 f"{self._name}: calibration aborted" 983 ) 984 985 def _save_to_nt(self) -> None: 986 """Persist calibration data to NetworkTables via ntproperty.""" 987 if self._hard_limit_min is not None: 988 self._nt_hard_limit_min = self._hard_limit_min 989 if self._hard_limit_max is not None: 990 self._nt_hard_limit_max = self._hard_limit_max 991 self._nt_soft_limit_margin = self._soft_limit_margin 992 993 def _load_from_nt(self) -> None: 994 """Load persisted calibration data from ntproperty NT entries.""" 995 nt_min = self._nt_hard_limit_min 996 nt_max = self._nt_hard_limit_max 997 if not math.isnan(nt_min) and not math.isnan(nt_max): 998 self._hard_limit_min = nt_min 999 self._hard_limit_max = nt_max 1000 self._soft_limit_margin = self._nt_soft_limit_margin 1001 self._is_calibrated = True 1002 # Compute soft limits in memory only — don't push to hardware 1003 # because encoder position is unknown until homing completes 1004 full_range = self._hard_limit_max - self._hard_limit_min 1005 margin = full_range * self._soft_limit_margin 1006 self._min_soft_limit = self._hard_limit_min + margin 1007 self._max_soft_limit = self._hard_limit_max - margin
class
PositionCalibration:
32class PositionCalibration: 33 """ 34 Reusable calibration and homing controller for positional mechanisms. 35 36 Extracts sensorless or sensor homing and two-phase calibration logic 37 from mechanism-specific subsystems into a standalone, motor-agnostic 38 class. All hardware interaction is done through user-provided 39 callbacks, making this compatible with any motor controller. 40 41 For SparkMax users, the ``motor`` convenience parameter auto-populates 42 callbacks via SparkMaxCallbacks. Individual callbacks passed as keyword 43 arguments override the SparkMax defaults. 44 45 Calibration discovers mechanical hard limits by driving into stops, 46 then computes soft limits with a configurable safety margin. Persists 47 discovered limits to NetworkTables via ntproperty so values survive 48 reboots. 49 50 ## Callbacks 51 52 **Core required** (must be set before homing or calibration): 53 54 | Name | Signature | Purpose | 55 |------|-----------|---------| 56 | `set_motor_output` | `(float) -> None` | Drive motor at duty cycle (-1.0 to 1.0) | 57 | `stop_motor` | `() -> None` | Stop motor output | 58 | `set_position` | `(float) -> None` | Reset encoder position | 59 60 **Detection** (at least one required per homing direction): 61 62 | Name | Signature | Purpose | 63 |------|-----------|---------| 64 | `get_velocity` | `() -> float` | Enables stall detection | 65 | `get_forward_limit_switch` | `() -> bool` | Forward limit switch | 66 | `get_reverse_limit_switch` | `() -> bool` | Reverse limit switch | 67 68 **Optional** (enhance safety/features but not required): 69 70 | Name | Signature | Purpose | 71 |------|-----------|---------| 72 | `get_position` | `() -> float` | Required only for calibration phase 2 | 73 | `set_current_limit` | `(float) -> None` | Protective current limit during homing | 74 | `set_soft_limits` | `(min, max) -> None` | Apply soft limits to hardware | 75 | `disable_soft_limits` | `(fwd, rev) -> None` | Disable soft limits for free travel | 76 | `save_config` | `() -> Any` | Snapshot motor config before homing | 77 | `restore_config` | `(Any) -> None` | Restore snapshot after homing | 78 | `on_limit_detected` | `(pos, dir) -> None` | Fires when a hard limit is found | 79 | `set_conversion_factor` | `(float) -> None` | Apply a new encoder position conversion factor | 80 81 Use ``get_callbacks()`` to inspect the current callback dict (returns 82 ``None`` for any callback that has not been set). 83 84 Examples 85 -------- 86 87 SparkMax convenience (auto-generates all callbacks):: 88 89 self.calibration = PositionCalibration( 90 name="Turret", 91 fallback_min=-90, fallback_max=90, 92 motor=self.motor, encoder=self.encoder) 93 94 Pure callbacks with stall detection only:: 95 96 self.calibration = PositionCalibration( 97 name="Elevator", 98 fallback_min=0.0, fallback_max=1.5, 99 set_motor_output=lambda pct: motor.set(pct), 100 stop_motor=lambda: motor.stopMotor(), 101 set_position=lambda v: encoder.setPosition(v), 102 get_velocity=lambda: encoder.getVelocity()) 103 104 Pure callbacks with limit switches only:: 105 106 self.calibration = PositionCalibration( 107 name="Arm", 108 fallback_min=0.0, fallback_max=90.0, 109 set_motor_output=lambda pct: motor.set(pct), 110 stop_motor=lambda: motor.stopMotor(), 111 set_position=lambda v: encoder.setPosition(v), 112 get_forward_limit_switch=lambda: fwd_switch.get(), 113 get_reverse_limit_switch=lambda: rev_switch.get()) 114 115 Deferred setup (set callbacks after construction):: 116 117 cal = PositionCalibration( 118 name="Wrist", 119 fallback_min=-45, fallback_max=45) 120 # ... later, once hardware is ready ... 121 cal.set_callbacks( 122 set_motor_output=motor.set, 123 stop_motor=motor.stopMotor, 124 set_position=encoder.setPosition, 125 get_velocity=encoder.getVelocity) 126 """ 127 128 # -- Callback type declarations -- 129 _cb_get_position: Optional[Callable[[], float]] 130 _cb_get_velocity: Optional[Callable[[], float]] 131 _cb_set_position: Optional[Callable[[float], None]] 132 _cb_set_motor_output: Optional[Callable[[float], None]] 133 _cb_stop_motor: Optional[Callable[[], None]] 134 _cb_set_current_limit: Optional[Callable[[float], None]] 135 _cb_set_soft_limits: Optional[Callable[[float, float], None]] 136 _cb_disable_soft_limits: Optional[Callable[[bool, bool], None]] 137 _cb_save_config: Optional[Callable[[], Any]] 138 _cb_restore_config: Optional[Callable[[Any], None]] 139 _cb_get_forward_limit_switch: Optional[Callable[[], bool]] 140 _cb_get_reverse_limit_switch: Optional[Callable[[], bool]] 141 _cb_on_limit_detected: Optional[Callable[[float, str], None]] 142 _cb_set_conversion_factor: Optional[Callable[[float], None]] 143 144 def __init__( 145 self, 146 name: str, 147 fallback_min: float, 148 fallback_max: float, 149 *, 150 motor=None, 151 encoder=None, 152 **kwargs, 153 ) -> None: 154 """ 155 Create a new PositionCalibration controller. 156 157 Args: 158 name: mechanism name used for NT paths and alerts (e.g. "Turret") 159 fallback_min: reverse soft limit used before calibration data 160 exists. Once calibrated, the persisted hard limits are used 161 instead. The caller is responsible for the far end of travel 162 if only homing (not full calibration) is performed. 163 fallback_max: forward soft limit used before calibration data 164 exists. See fallback_min for details. 165 motor: (optional) rev.SparkMax for convenience callback generation 166 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 167 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 168 """ 169 # Assign this instance to a cached subclass with ntproperty 170 # descriptors keyed to this mechanism's name. ntproperty is a 171 # class-level descriptor, so each mechanism name needs its own 172 # class to avoid key collisions. The cache ensures the 173 # ntproperty is created only once per name so defaults don't 174 # overwrite previously persisted values. 175 if name not in _calibration_classes: 176 prefix = f"/{name}/calibration" 177 attrs = { 178 "_nt_hard_limit_min": ntproperty( 179 f"{prefix}/hard_limit_min", float("nan"), 180 writeDefault=False, persistent=True), 181 "_nt_hard_limit_max": ntproperty( 182 f"{prefix}/hard_limit_max", float("nan"), 183 writeDefault=False, persistent=True), 184 "_nt_soft_limit_margin": ntproperty( 185 f"{prefix}/soft_limit_margin", 0.05, 186 writeDefault=False, persistent=True), 187 } 188 # Non-persistent booleans for callback status 189 for cb_name in sorted(_VALID_CALLBACKS): 190 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 191 f"{prefix}/callbacks/{cb_name}", False, 192 writeDefault=True, persistent=False) 193 _calibration_classes[name] = type( 194 f"PositionCalibration_{name}", 195 (PositionCalibration,), 196 attrs, 197 ) 198 self.__class__ = _calibration_classes[name] 199 200 self._name = name 201 202 # Soft limits — initialized to defaults, overwritten by 203 # _load_from_nt() if persisted calibration data exists 204 self._min_soft_limit = fallback_min 205 self._max_soft_limit = fallback_max 206 207 # Calibration state 208 self._hard_limit_min = None 209 self._hard_limit_max = None 210 self._is_calibrated = False 211 self._is_calibrating = False 212 self._calibration_phase = 0 213 self._soft_limit_margin = 0.05 214 self._position_offset = 0.0 215 216 # Homing state 217 self._is_homing = False 218 self._saved_config = None 219 220 # -- Build callbacks -- 221 # Validate kwargs keys 222 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 223 if unknown: 224 raise ValueError( 225 f"Unknown callback(s): {', '.join(sorted(unknown))}" 226 ) 227 228 # If motor provided, generate SparkMax defaults 229 defaults = {} 230 if motor is not None: 231 from utils.spark_max_callbacks import SparkMaxCallbacks 232 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 233 234 # Merge: explicit kwargs override SparkMax defaults 235 merged = {**defaults, **kwargs} 236 237 # Store each callback as self._cb_{name} (None if not provided) 238 for cb_name in _VALID_CALLBACKS: 239 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 240 241 # Publish callback status to NT 242 self._publish_callback_status() 243 244 # Attempt to load persisted calibration from NT 245 self._load_from_nt() 246 247 # ---- Public properties ---- 248 249 @property 250 def is_busy(self) -> bool: 251 """True when homing or calibrating (blocks motor commands).""" 252 return self._is_homing or self._is_calibrating 253 254 @property 255 def is_homing(self) -> bool: 256 """Whether a homing routine is active.""" 257 return self._is_homing 258 259 @property 260 def is_calibrating(self) -> bool: 261 """Whether a calibration routine is active.""" 262 return self._is_calibrating 263 264 @property 265 def is_calibrated(self) -> bool: 266 """Whether the full mechanical range has been discovered.""" 267 return self._is_calibrated 268 269 @property 270 def is_zeroed(self) -> bool: 271 """Whether the encoder has been aligned to a reference point.""" 272 return self._hard_limit_min is not None 273 274 @property 275 def min_limit(self): 276 """Hard limit min, or None if uncalibrated.""" 277 return self._hard_limit_min 278 279 @property 280 def max_limit(self): 281 """Hard limit max, or None if uncalibrated.""" 282 return self._hard_limit_max 283 284 @property 285 def min_soft_limit(self) -> float: 286 """Current minimum soft limit.""" 287 return self._min_soft_limit 288 289 @property 290 def max_soft_limit(self) -> float: 291 """Current maximum soft limit.""" 292 return self._max_soft_limit 293 294 @property 295 def soft_limit_margin(self) -> float: 296 """Current safety margin as a fraction of the full range.""" 297 return self._soft_limit_margin 298 299 # ---- Public methods ---- 300 301 def get_callbacks(self) -> dict: 302 """ 303 Return a dict of all callback names and their current values. 304 305 Each key is a callback name from _VALID_CALLBACKS. The value is 306 the callable if set, or None if not set. Useful for inspecting 307 which callbacks are configured:: 308 309 cbs = cal.get_callbacks() 310 for name, func in cbs.items(): 311 print(f"{name}: {'set' if func else 'not set'}") 312 313 Returns: 314 dict mapping callback name -> callable or None 315 """ 316 return { 317 name: getattr(self, f'_cb_{name}', None) 318 for name in sorted(_VALID_CALLBACKS) 319 } 320 321 def set_callbacks(self, **kwargs) -> None: 322 """ 323 Set, override, or clear one or more callbacks by name. 324 325 Pass a callable to set a callback, or None to clear it:: 326 327 # Set a callback 328 cal.set_callbacks(get_velocity=encoder.getVelocity) 329 330 # Clear callbacks that don't apply to this mechanism 331 cal.set_callbacks( 332 get_forward_limit_switch=None, 333 get_reverse_limit_switch=None) 334 335 Args: 336 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 337 Pass None to clear a callback. 338 """ 339 for key, value in kwargs.items(): 340 if key not in _VALID_CALLBACKS: 341 raise ValueError(f"Unknown callback: {key}") 342 setattr(self, f'_cb_{key}', value) 343 self._publish_callback_status() 344 345 def homing_init( 346 self, 347 max_current: float, 348 max_power_pct: float, 349 max_homing_time: float, 350 homing_forward: bool, 351 min_velocity: float = None, 352 home_position: float = 0.0, 353 max_travel: Optional[float] = None, 354 min_trigger_travel: float = 0.0, 355 ) -> None: 356 """ 357 Initialize the sensorless or sensor homing routine. 358 359 Saves current motor settings (if save_config callback is set), 360 applies homing-specific configuration, and begins driving toward 361 the specified hard stop. 362 363 Args: 364 max_current: current limit in amps during homing 365 max_power_pct: motor duty cycle during homing (0.0-1.0) 366 max_homing_time: maximum time in seconds before timeout 367 homing_forward: True to home toward forward limit, False for reverse 368 min_velocity: stall detection threshold in user units/second. 369 Defaults to 5% of soft limit range over 2 seconds. 370 home_position: encoder value to set when the hard stop is 371 found (default 0.0) 372 max_travel: abort if encoder travels more than this many user 373 units from the start position. Catches a stuck-open limit 374 switch that never triggers. None disables the check — 375 useful when the mechanical range is unknown. Requires 376 ``get_position`` callback to take effect. 377 min_trigger_travel: minimum encoder travel (user units) 378 required before a limit switch trigger is trusted. If 379 the switch fires before this much movement, abort with 380 a stuck-closed-switch alert. 0.0 disables (default). 381 Requires ``get_position`` callback to take effect. 382 """ 383 # Validate callbacks before starting 384 self._validate_homing(homing_forward) 385 386 if min_velocity is None: 387 full_range = self._max_soft_limit - self._min_soft_limit 388 min_velocity = full_range * 0.05 / 2.0 389 390 # Save current motor settings for restoration (if callback exists) 391 if self._cb_save_config is not None: 392 self._saved_config = self._cb_save_config() 393 else: 394 self._saved_config = None 395 396 # Apply homing current limit (if callback exists) 397 if self._cb_set_current_limit is not None: 398 self._cb_set_current_limit(max_current) 399 400 # Disable soft limit in homing direction (if callback exists) 401 if self._cb_disable_soft_limits is not None: 402 if homing_forward: 403 self._cb_disable_soft_limits(True, False) 404 else: 405 self._cb_disable_soft_limits(False, True) 406 407 # Store homing parameters 408 self._homing_forward = homing_forward 409 self._max_power_pct = max_power_pct 410 self._min_velocity = min_velocity 411 self._max_homing_time = max_homing_time 412 self._home_position = home_position 413 self._max_travel = max_travel 414 self._min_trigger_travel = min_trigger_travel 415 416 # Capture start position for travel tracking. Only meaningful if 417 # get_position is wired; otherwise both travel-based guards are 418 # silently no-ops. 419 if self._cb_get_position is not None: 420 self._homing_start_pos = self._cb_get_position() 421 else: 422 self._homing_start_pos = None 423 424 # Set homing state 425 self._is_homing = True 426 self._stall_detected = False 427 428 # Start timeout timer 429 self._homing_timer = wpilib.Timer() 430 self._homing_timer.start() 431 432 # Stall detection timer 433 self._stall_timer = wpilib.Timer() 434 435 # Alerts 436 self._homing_status_alert = wpilib.Alert( 437 f"{self._name}: homing started", 438 wpilib.Alert.AlertType.kInfo 439 ) 440 self._homing_status_alert.set(True) 441 self._homing_error_alert = wpilib.Alert( 442 f"{self._name}: homing failed", 443 wpilib.Alert.AlertType.kError 444 ) 445 self._homing_error_alert.set(False) 446 447 def calibration_init( 448 self, 449 max_current: float, 450 max_power_pct: float, 451 max_homing_time: float, 452 min_velocity: float = None, 453 known_range: float = None, 454 max_travel: Optional[float] = None, 455 min_trigger_travel: float = 0.0, 456 ) -> None: 457 """ 458 Start a calibration routine that discovers the mechanical range. 459 460 Phase 1: Home negative to find the reverse hard limit (set as zero). 461 Phase 2: Home positive to find the forward hard limit (measured). 462 463 If known_range is provided, only phase 1 runs and the forward hard 464 limit is computed as known_range from the zero point. 465 466 Args: 467 max_current: current limit in amps during calibration 468 max_power_pct: motor duty cycle (0.0-1.0) 469 max_homing_time: maximum time per phase before timeout 470 min_velocity: stall detection threshold in user units/second 471 known_range: if provided, skip phase 2 and use this as the 472 full mechanical range from the zero point 473 max_travel: abort a homing phase if encoder travels more 474 than this from its phase start. Defaults to 110% of the 475 current soft-limit range (a sane bound for calibration 476 because the expected range is known). Pass an explicit 477 None via the kwarg to disable the check entirely. 478 min_trigger_travel: minimum travel required before a limit 479 switch trigger is trusted per phase. Guards against a 480 stuck-closed switch firing immediately. 0.0 disables. 481 """ 482 # Validate callbacks before starting 483 self._validate_calibration() 484 485 # Save motor settings for restoration after calibration 486 if self._cb_save_config is not None: 487 self._cal_saved_config = self._cb_save_config() 488 else: 489 self._cal_saved_config = None 490 491 # Disable both soft limits for free travel 492 if self._cb_disable_soft_limits is not None: 493 self._cb_disable_soft_limits(True, True) 494 495 # Default max_travel to 110% of the expected range. Each phase 496 # homes one direction, so the full range is the per-phase ceiling. 497 if max_travel is None: 498 expected_range = abs(self._max_soft_limit - self._min_soft_limit) 499 if expected_range > 0: 500 max_travel = expected_range * 1.1 501 502 # Store calibration parameters for phase transitions 503 self._cal_max_current = max_current 504 self._cal_max_power_pct = max_power_pct 505 self._cal_max_homing_time = max_homing_time 506 self._cal_min_velocity = min_velocity 507 self._cal_known_range = known_range 508 self._cal_max_travel = max_travel 509 self._cal_min_trigger_travel = min_trigger_travel 510 511 self._is_calibrating = True 512 self._calibration_phase = 1 513 514 # Start phase 1: home negative 515 self.homing_init( 516 max_current, max_power_pct, max_homing_time, 517 homing_forward=False, min_velocity=min_velocity, 518 max_travel=max_travel, 519 min_trigger_travel=min_trigger_travel, 520 ) 521 522 self._cal_status_alert = wpilib.Alert( 523 f"{self._name}: calibration phase 1 - homing negative", 524 wpilib.Alert.AlertType.kInfo 525 ) 526 self._cal_status_alert.set(True) 527 528 def periodic(self) -> None: 529 """ 530 Run the active homing or calibration state machine. 531 532 Call this every cycle when is_busy is True. 533 """ 534 if self._is_calibrating: 535 self._calibration_periodic() 536 elif self._is_homing: 537 self._homing_periodic() 538 539 def abort(self) -> None: 540 """Abort any active homing or calibration routine.""" 541 if self._is_calibrating: 542 self._calibration_end(abort=True) 543 elif self._is_homing: 544 self._homing_end(abort=True) 545 546 def set_soft_limit_margin(self, margin_pct: float) -> None: 547 """ 548 Set the soft limit safety margin and apply to the motor controller. 549 550 Args: 551 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 552 """ 553 if not self._is_calibrated: 554 return 555 self._soft_limit_margin = margin_pct 556 full_range = self._hard_limit_max - self._hard_limit_min 557 margin = full_range * margin_pct 558 self._min_soft_limit = self._hard_limit_min + margin 559 self._max_soft_limit = self._hard_limit_max - margin 560 561 # Apply to hardware only if callback exists 562 if self._cb_set_soft_limits is not None: 563 self._cb_set_soft_limits( 564 self._min_soft_limit, self._max_soft_limit) 565 566 def compute_conversion_factor( 567 self, 568 current_factor: float, 569 desired_min: float, 570 desired_max: float, 571 ) -> float: 572 """ 573 Compute a corrected encoder conversion factor after calibration. 574 575 After a full calibration discovers the raw mechanical range, this 576 method computes the conversion factor needed so the measured 577 range maps to ``[desired_min, desired_max]``. 578 579 Example: if calibration measured 500 user-units of travel with 580 the initial conversion factor and you want -90..90 (180°), the 581 corrected factor scales the encoder so full travel reads 180°. 582 583 Args: 584 current_factor: the position conversion factor currently set 585 on the encoder 586 desired_min: target minimum position in user units 587 desired_max: target maximum position in user units 588 589 Returns: 590 The corrected conversion factor to apply to the encoder. 591 592 Raises: 593 RuntimeError: if calibration has not completed, or the 594 measured range is zero. 595 """ 596 if not self._is_calibrated: 597 raise RuntimeError( 598 f"{self._name}: cannot compute conversion factor " 599 "before calibration is complete" 600 ) 601 measured_range = self._hard_limit_max - self._hard_limit_min 602 if measured_range == 0: 603 raise RuntimeError( 604 f"{self._name}: measured range is zero, " 605 "calibration data invalid" 606 ) 607 desired_range = desired_max - desired_min 608 return current_factor * (desired_range / measured_range) 609 610 def apply_conversion_factor( 611 self, 612 current_factor: float, 613 desired_min: float, 614 desired_max: float, 615 ) -> float: 616 """ 617 Compute and apply a corrected encoder conversion factor. 618 619 Calls :meth:`compute_conversion_factor` for the new value, then 620 applies it via the ``set_conversion_factor`` callback, resets 621 the encoder to ``desired_min`` (the new zero reference), and 622 rewrites the internal hard/soft limits to match the desired 623 range. 624 625 This is intended to be called immediately after a full 626 calibration completes (encoder at the reverse hard stop, 627 reading 0). A guard rejects the call if the encoder reports 628 more than 5% of the current soft-limit range away from zero — 629 the mechanism has moved since calibration, so the post-rescale 630 encoder zero would not correspond to a known physical position. 631 632 Args: 633 current_factor: the position conversion factor currently set 634 on the encoder 635 desired_min: target minimum position in user units (becomes 636 the new zero reference) 637 desired_max: target maximum position in user units 638 639 Returns: 640 The corrected conversion factor that was applied. 641 642 Raises: 643 ValueError: if the ``set_conversion_factor`` callback is 644 not configured. 645 RuntimeError: if calibration has not produced a zero 646 reference, or the mechanism is not at that zero point. 647 """ 648 if self._cb_set_conversion_factor is None: 649 raise ValueError( 650 "set_conversion_factor callback not set" 651 ) 652 653 if not self.is_zeroed: 654 raise RuntimeError( 655 f"{self._name}: cannot apply conversion factor " 656 "before homing to zero position" 657 ) 658 659 # Verify the mechanism is still at the reverse hard stop 660 # (encoder reads ~0) before rescaling. Without this, the post- 661 # rescale encoder zero corresponds to no known physical point. 662 if self._cb_get_position is not None: 663 pos = abs(self._cb_get_position()) 664 full_range = self._max_soft_limit - self._min_soft_limit 665 tolerance = abs(full_range) * 0.05 666 if pos > tolerance: 667 raise RuntimeError( 668 f"{self._name}: mechanism is not at the zero " 669 f"position (current: {pos:.1f}, tolerance: " 670 f"{tolerance:.1f}). Home the mechanism before " 671 "applying a new conversion factor" 672 ) 673 674 new_factor = self.compute_conversion_factor( 675 current_factor, desired_min, desired_max 676 ) 677 self._cb_set_conversion_factor(new_factor) 678 679 # The encoder unit-scale just changed, so its raw reading no 680 # longer maps to the same physical position. Reseed to 681 # desired_min (we calibrated from the reverse hard stop) and 682 # rewrite limits to match the desired range. 683 self._cb_set_position(desired_min) 684 self._hard_limit_min = desired_min 685 self._hard_limit_max = desired_max 686 self.set_soft_limit_margin(self._soft_limit_margin) 687 self._save_to_nt() 688 689 return new_factor 690 691 def update_telemetry(self, prefix: str) -> None: 692 """ 693 Publish calibration data to SmartDashboard. 694 695 Args: 696 prefix: SmartDashboard key prefix (e.g. "Turret/") 697 """ 698 sd = wpilib.SmartDashboard 699 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 700 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 701 sd.putBoolean(prefix + "isHoming", self._is_homing) 702 sd.putNumber( 703 prefix + "hardLimitMin", 704 self._hard_limit_min if self._hard_limit_min is not None 705 else 0.0 706 ) 707 sd.putNumber( 708 prefix + "hardLimitMax", 709 self._hard_limit_max if self._hard_limit_max is not None 710 else 0.0 711 ) 712 sd.putNumber(prefix + "positionOffset", self._position_offset) 713 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin) 714 715 # ---- Validation ---- 716 717 def _validate_homing(self, homing_forward: bool): 718 """Validate callbacks needed for homing in the given direction.""" 719 missing = [n for n in sorted(_CORE_REQUIRED) 720 if getattr(self, f'_cb_{n}', None) is None] 721 if missing: 722 raise ValueError( 723 f"Required callbacks not set: {', '.join(missing)}" 724 ) 725 726 has_velocity = self._cb_get_velocity is not None 727 if homing_forward: 728 has_limit = self._cb_get_forward_limit_switch is not None 729 else: 730 has_limit = self._cb_get_reverse_limit_switch is not None 731 732 if not has_velocity and not has_limit: 733 direction = "forward" if homing_forward else "reverse" 734 raise ValueError( 735 f"No detection method for {direction} homing: " 736 f"provide get_velocity and/or " 737 f"get_{direction}_limit_switch" 738 ) 739 740 def _validate_calibration(self): 741 """Validate callbacks needed for full calibration.""" 742 if self._cb_get_position is None: 743 raise ValueError( 744 "get_position callback required for calibration" 745 ) 746 # Need detection in both directions 747 self._validate_homing(homing_forward=False) # phase 1 748 self._validate_homing(homing_forward=True) # phase 2 749 750 # ---- Internal methods ---- 751 752 def _publish_callback_status(self) -> None: 753 """Write True/False for each callback to NT via ntproperty.""" 754 for cb_name in _VALID_CALLBACKS: 755 is_set = getattr(self, f'_cb_{cb_name}', None) is not None 756 setattr(self, f'_nt_cb_{cb_name}', is_set) 757 758 def _homing_periodic(self) -> bool: 759 """ 760 Periodic homing logic. Drives the motor, monitors for stall or 761 limit switch, and handles timeout. 762 763 Returns: 764 True when homing is complete (success or failure), False if ongoing 765 """ 766 if not self._is_homing: 767 return True 768 769 # Check timeout first 770 if self._homing_timer.hasElapsed(self._max_homing_time): 771 self._homing_error_alert.setText( 772 f"{self._name}: homing failed - timeout" 773 ) 774 self._homing_end(abort=True) 775 return True 776 777 # Overtravel guard: abort if encoder has moved more than expected. 778 # Catches a stuck-open limit switch that never triggers and a 779 # broken stall-detection path. Requires get_position to take 780 # effect; otherwise it's a silent no-op. 781 if (self._max_travel is not None 782 and self._homing_start_pos is not None 783 and self._cb_get_position is not None): 784 travel = abs(self._cb_get_position() - self._homing_start_pos) 785 if travel > self._max_travel: 786 self._homing_error_alert.setText( 787 f"{self._name}: homing aborted - overtravel " 788 f"({travel:.1f} > {self._max_travel:.1f})" 789 ) 790 self._homing_end(abort=True) 791 return True 792 793 # Check hard limit switch in homing direction (if callback exists) 794 limit_hit = False 795 if (self._homing_forward 796 and self._cb_get_forward_limit_switch is not None): 797 limit_hit = self._cb_get_forward_limit_switch() 798 elif (not self._homing_forward 799 and self._cb_get_reverse_limit_switch is not None): 800 limit_hit = self._cb_get_reverse_limit_switch() 801 802 # Drive motor in homing direction 803 if self._homing_forward: 804 self._cb_set_motor_output(self._max_power_pct) 805 else: 806 self._cb_set_motor_output(-self._max_power_pct) 807 808 # Check for limit switch hit 809 if limit_hit: 810 # Stuck-closed guard: if the switch fires before we've moved 811 # enough to be credible, treat it as a false trigger. 812 if (self._min_trigger_travel > 0.0 813 and self._homing_start_pos is not None 814 and self._cb_get_position is not None): 815 travel = abs( 816 self._cb_get_position() - self._homing_start_pos) 817 if travel < self._min_trigger_travel: 818 self._homing_error_alert.setText( 819 f"{self._name}: homing aborted - limit switch " 820 f"triggered after only {travel:.1f} of " 821 f"{self._min_trigger_travel:.1f} min travel " 822 "(may be stuck closed)" 823 ) 824 self._homing_end(abort=True) 825 return True 826 827 home_position = self._home_position 828 self._cb_set_position(home_position) 829 if self._cb_on_limit_detected is not None: 830 direction = ( 831 "forward" if self._homing_forward else "reverse" 832 ) 833 self._cb_on_limit_detected(home_position, direction) 834 self._homing_status_alert.setText( 835 f"{self._name}: homing complete" 836 ) 837 self._homing_end(abort=False) 838 return True 839 840 # Stall detection only if velocity callback provided 841 if self._cb_get_velocity is not None: 842 velocity = abs(self._cb_get_velocity()) 843 844 if velocity < self._min_velocity: 845 if not self._stall_detected: 846 self._stall_detected = True 847 self._stall_timer.restart() 848 elif self._stall_timer.hasElapsed(0.1): 849 home_position = self._home_position 850 self._cb_set_position(home_position) 851 if self._cb_on_limit_detected is not None: 852 direction = ( 853 "forward" if self._homing_forward 854 else "reverse" 855 ) 856 self._cb_on_limit_detected( 857 home_position, direction) 858 self._homing_status_alert.setText( 859 f"{self._name}: homing complete" 860 ) 861 self._homing_end(abort=False) 862 return True 863 else: 864 self._stall_detected = False 865 self._stall_timer.stop() 866 self._stall_timer.reset() 867 868 return False 869 870 def _homing_end(self, abort: bool) -> None: 871 """ 872 Clean up after homing. Stops motor, restores saved settings, 873 updates alerts. 874 875 Args: 876 abort: True if homing was aborted or failed 877 """ 878 self._cb_stop_motor() 879 880 # Restore saved motor settings (if we saved them) 881 if (self._saved_config is not None 882 and self._cb_restore_config is not None): 883 self._cb_restore_config(self._saved_config) 884 885 # Clear homing state 886 self._is_homing = False 887 self._homing_succeeded = not abort 888 889 # Stop timers 890 self._homing_timer.stop() 891 self._stall_timer.stop() 892 893 # Update alerts 894 if abort: 895 self._homing_error_alert.setText( 896 f"{self._name}: homing aborted" 897 ) 898 self._homing_error_alert.set(True) 899 self._homing_status_alert.set(False) 900 else: 901 self._homing_error_alert.set(False) 902 # After successful homing, if calibration data exists, the 903 # encoder is now aligned so soft limits can be applied to 904 # hardware 905 if (not self._is_calibrating 906 and self._is_calibrated 907 and self._cb_set_soft_limits is not None): 908 self._cb_set_soft_limits( 909 self._min_soft_limit, self._max_soft_limit) 910 911 def _calibration_periodic(self) -> None: 912 """Manage the two-phase calibration state machine.""" 913 if not self._is_calibrating: 914 return 915 916 if self._calibration_phase == 1: 917 done = self._homing_periodic() 918 if done: 919 if self._homing_succeeded: 920 # Phase 1 complete - set encoder to 0 921 self._cb_set_position(0.0) 922 self._hard_limit_min = 0.0 923 924 if self._cal_known_range is not None: 925 self._hard_limit_max = self._cal_known_range 926 self._calibration_end(abort=False) 927 else: 928 self._cal_status_alert.setText( 929 f"{self._name}: calibration phase 2" 930 " - homing positive" 931 ) 932 self._calibration_phase = 2 933 self.homing_init( 934 self._cal_max_current, 935 self._cal_max_power_pct, 936 self._cal_max_homing_time, 937 homing_forward=True, 938 min_velocity=self._cal_min_velocity, 939 max_travel=self._cal_max_travel, 940 min_trigger_travel=( 941 self._cal_min_trigger_travel), 942 ) 943 else: 944 # Homing failed (timeout) - abort calibration 945 self._calibration_end(abort=True) 946 947 elif self._calibration_phase == 2: 948 measured_position = self._cb_get_position() 949 done = self._homing_periodic() 950 if done: 951 if self._homing_succeeded: 952 self._hard_limit_max = measured_position 953 self._calibration_end(abort=False) 954 else: 955 self._calibration_end(abort=True) 956 957 def _calibration_end(self, abort: bool) -> None: 958 """ 959 Clean up after calibration. Restores motor settings and on 960 success computes and applies soft limits. 961 962 Args: 963 abort: True if calibration failed 964 """ 965 self._cb_stop_motor() 966 self._is_calibrating = False 967 self._calibration_phase = 0 968 self._is_homing = False 969 970 if not abort and self._hard_limit_max is not None: 971 self._is_calibrated = True 972 self._cal_status_alert.setText( 973 f"{self._name}: calibration complete" 974 ) 975 self.set_soft_limit_margin(self._soft_limit_margin) 976 self._save_to_nt() 977 else: 978 # Restore original config on abort 979 if (self._cal_saved_config is not None 980 and self._cb_restore_config is not None): 981 self._cb_restore_config(self._cal_saved_config) 982 self._cal_status_alert.setText( 983 f"{self._name}: calibration aborted" 984 ) 985 986 def _save_to_nt(self) -> None: 987 """Persist calibration data to NetworkTables via ntproperty.""" 988 if self._hard_limit_min is not None: 989 self._nt_hard_limit_min = self._hard_limit_min 990 if self._hard_limit_max is not None: 991 self._nt_hard_limit_max = self._hard_limit_max 992 self._nt_soft_limit_margin = self._soft_limit_margin 993 994 def _load_from_nt(self) -> None: 995 """Load persisted calibration data from ntproperty NT entries.""" 996 nt_min = self._nt_hard_limit_min 997 nt_max = self._nt_hard_limit_max 998 if not math.isnan(nt_min) and not math.isnan(nt_max): 999 self._hard_limit_min = nt_min 1000 self._hard_limit_max = nt_max 1001 self._soft_limit_margin = self._nt_soft_limit_margin 1002 self._is_calibrated = True 1003 # Compute soft limits in memory only — don't push to hardware 1004 # because encoder position is unknown until homing completes 1005 full_range = self._hard_limit_max - self._hard_limit_min 1006 margin = full_range * self._soft_limit_margin 1007 self._min_soft_limit = self._hard_limit_min + margin 1008 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 |
set_conversion_factor |
(float) -> None |
Apply a new encoder position conversion factor |
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)
144 def __init__( 145 self, 146 name: str, 147 fallback_min: float, 148 fallback_max: float, 149 *, 150 motor=None, 151 encoder=None, 152 **kwargs, 153 ) -> None: 154 """ 155 Create a new PositionCalibration controller. 156 157 Args: 158 name: mechanism name used for NT paths and alerts (e.g. "Turret") 159 fallback_min: reverse soft limit used before calibration data 160 exists. Once calibrated, the persisted hard limits are used 161 instead. The caller is responsible for the far end of travel 162 if only homing (not full calibration) is performed. 163 fallback_max: forward soft limit used before calibration data 164 exists. See fallback_min for details. 165 motor: (optional) rev.SparkMax for convenience callback generation 166 encoder: (optional) rev.RelativeEncoder (defaults to motor.getEncoder()) 167 **kwargs: callback overrides (see _VALID_CALLBACKS for valid keys) 168 """ 169 # Assign this instance to a cached subclass with ntproperty 170 # descriptors keyed to this mechanism's name. ntproperty is a 171 # class-level descriptor, so each mechanism name needs its own 172 # class to avoid key collisions. The cache ensures the 173 # ntproperty is created only once per name so defaults don't 174 # overwrite previously persisted values. 175 if name not in _calibration_classes: 176 prefix = f"/{name}/calibration" 177 attrs = { 178 "_nt_hard_limit_min": ntproperty( 179 f"{prefix}/hard_limit_min", float("nan"), 180 writeDefault=False, persistent=True), 181 "_nt_hard_limit_max": ntproperty( 182 f"{prefix}/hard_limit_max", float("nan"), 183 writeDefault=False, persistent=True), 184 "_nt_soft_limit_margin": ntproperty( 185 f"{prefix}/soft_limit_margin", 0.05, 186 writeDefault=False, persistent=True), 187 } 188 # Non-persistent booleans for callback status 189 for cb_name in sorted(_VALID_CALLBACKS): 190 attrs[f"_nt_cb_{cb_name}"] = ntproperty( 191 f"{prefix}/callbacks/{cb_name}", False, 192 writeDefault=True, persistent=False) 193 _calibration_classes[name] = type( 194 f"PositionCalibration_{name}", 195 (PositionCalibration,), 196 attrs, 197 ) 198 self.__class__ = _calibration_classes[name] 199 200 self._name = name 201 202 # Soft limits — initialized to defaults, overwritten by 203 # _load_from_nt() if persisted calibration data exists 204 self._min_soft_limit = fallback_min 205 self._max_soft_limit = fallback_max 206 207 # Calibration state 208 self._hard_limit_min = None 209 self._hard_limit_max = None 210 self._is_calibrated = False 211 self._is_calibrating = False 212 self._calibration_phase = 0 213 self._soft_limit_margin = 0.05 214 self._position_offset = 0.0 215 216 # Homing state 217 self._is_homing = False 218 self._saved_config = None 219 220 # -- Build callbacks -- 221 # Validate kwargs keys 222 unknown = set(kwargs.keys()) - _VALID_CALLBACKS 223 if unknown: 224 raise ValueError( 225 f"Unknown callback(s): {', '.join(sorted(unknown))}" 226 ) 227 228 # If motor provided, generate SparkMax defaults 229 defaults = {} 230 if motor is not None: 231 from utils.spark_max_callbacks import SparkMaxCallbacks 232 defaults = SparkMaxCallbacks(motor, encoder).as_dict() 233 234 # Merge: explicit kwargs override SparkMax defaults 235 merged = {**defaults, **kwargs} 236 237 # Store each callback as self._cb_{name} (None if not provided) 238 for cb_name in _VALID_CALLBACKS: 239 setattr(self, f'_cb_{cb_name}', merged.get(cb_name)) 240 241 # Publish callback status to NT 242 self._publish_callback_status() 243 244 # Attempt to load persisted calibration from NT 245 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
249 @property 250 def is_busy(self) -> bool: 251 """True when homing or calibrating (blocks motor commands).""" 252 return self._is_homing or self._is_calibrating
True when homing or calibrating (blocks motor commands).
is_homing: bool
254 @property 255 def is_homing(self) -> bool: 256 """Whether a homing routine is active.""" 257 return self._is_homing
Whether a homing routine is active.
is_calibrating: bool
259 @property 260 def is_calibrating(self) -> bool: 261 """Whether a calibration routine is active.""" 262 return self._is_calibrating
Whether a calibration routine is active.
is_calibrated: bool
264 @property 265 def is_calibrated(self) -> bool: 266 """Whether the full mechanical range has been discovered.""" 267 return self._is_calibrated
Whether the full mechanical range has been discovered.
is_zeroed: bool
269 @property 270 def is_zeroed(self) -> bool: 271 """Whether the encoder has been aligned to a reference point.""" 272 return self._hard_limit_min is not None
Whether the encoder has been aligned to a reference point.
min_limit
274 @property 275 def min_limit(self): 276 """Hard limit min, or None if uncalibrated.""" 277 return self._hard_limit_min
Hard limit min, or None if uncalibrated.
max_limit
279 @property 280 def max_limit(self): 281 """Hard limit max, or None if uncalibrated.""" 282 return self._hard_limit_max
Hard limit max, or None if uncalibrated.
min_soft_limit: float
284 @property 285 def min_soft_limit(self) -> float: 286 """Current minimum soft limit.""" 287 return self._min_soft_limit
Current minimum soft limit.
max_soft_limit: float
289 @property 290 def max_soft_limit(self) -> float: 291 """Current maximum soft limit.""" 292 return self._max_soft_limit
Current maximum soft limit.
soft_limit_margin: float
294 @property 295 def soft_limit_margin(self) -> float: 296 """Current safety margin as a fraction of the full range.""" 297 return self._soft_limit_margin
Current safety margin as a fraction of the full range.
def
get_callbacks(self) -> dict:
301 def get_callbacks(self) -> dict: 302 """ 303 Return a dict of all callback names and their current values. 304 305 Each key is a callback name from _VALID_CALLBACKS. The value is 306 the callable if set, or None if not set. Useful for inspecting 307 which callbacks are configured:: 308 309 cbs = cal.get_callbacks() 310 for name, func in cbs.items(): 311 print(f"{name}: {'set' if func else 'not set'}") 312 313 Returns: 314 dict mapping callback name -> callable or None 315 """ 316 return { 317 name: getattr(self, f'_cb_{name}', None) 318 for name in sorted(_VALID_CALLBACKS) 319 }
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:
321 def set_callbacks(self, **kwargs) -> None: 322 """ 323 Set, override, or clear one or more callbacks by name. 324 325 Pass a callable to set a callback, or None to clear it:: 326 327 # Set a callback 328 cal.set_callbacks(get_velocity=encoder.getVelocity) 329 330 # Clear callbacks that don't apply to this mechanism 331 cal.set_callbacks( 332 get_forward_limit_switch=None, 333 get_reverse_limit_switch=None) 334 335 Args: 336 **kwargs: callback name/value pairs (see _VALID_CALLBACKS). 337 Pass None to clear a callback. 338 """ 339 for key, value in kwargs.items(): 340 if key not in _VALID_CALLBACKS: 341 raise ValueError(f"Unknown callback: {key}") 342 setattr(self, f'_cb_{key}', value) 343 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, max_travel: float | None = None, min_trigger_travel: float = 0.0) -> None:
345 def homing_init( 346 self, 347 max_current: float, 348 max_power_pct: float, 349 max_homing_time: float, 350 homing_forward: bool, 351 min_velocity: float = None, 352 home_position: float = 0.0, 353 max_travel: Optional[float] = None, 354 min_trigger_travel: float = 0.0, 355 ) -> None: 356 """ 357 Initialize the sensorless or sensor homing routine. 358 359 Saves current motor settings (if save_config callback is set), 360 applies homing-specific configuration, and begins driving toward 361 the specified hard stop. 362 363 Args: 364 max_current: current limit in amps during homing 365 max_power_pct: motor duty cycle during homing (0.0-1.0) 366 max_homing_time: maximum time in seconds before timeout 367 homing_forward: True to home toward forward limit, False for reverse 368 min_velocity: stall detection threshold in user units/second. 369 Defaults to 5% of soft limit range over 2 seconds. 370 home_position: encoder value to set when the hard stop is 371 found (default 0.0) 372 max_travel: abort if encoder travels more than this many user 373 units from the start position. Catches a stuck-open limit 374 switch that never triggers. None disables the check — 375 useful when the mechanical range is unknown. Requires 376 ``get_position`` callback to take effect. 377 min_trigger_travel: minimum encoder travel (user units) 378 required before a limit switch trigger is trusted. If 379 the switch fires before this much movement, abort with 380 a stuck-closed-switch alert. 0.0 disables (default). 381 Requires ``get_position`` callback to take effect. 382 """ 383 # Validate callbacks before starting 384 self._validate_homing(homing_forward) 385 386 if min_velocity is None: 387 full_range = self._max_soft_limit - self._min_soft_limit 388 min_velocity = full_range * 0.05 / 2.0 389 390 # Save current motor settings for restoration (if callback exists) 391 if self._cb_save_config is not None: 392 self._saved_config = self._cb_save_config() 393 else: 394 self._saved_config = None 395 396 # Apply homing current limit (if callback exists) 397 if self._cb_set_current_limit is not None: 398 self._cb_set_current_limit(max_current) 399 400 # Disable soft limit in homing direction (if callback exists) 401 if self._cb_disable_soft_limits is not None: 402 if homing_forward: 403 self._cb_disable_soft_limits(True, False) 404 else: 405 self._cb_disable_soft_limits(False, True) 406 407 # Store homing parameters 408 self._homing_forward = homing_forward 409 self._max_power_pct = max_power_pct 410 self._min_velocity = min_velocity 411 self._max_homing_time = max_homing_time 412 self._home_position = home_position 413 self._max_travel = max_travel 414 self._min_trigger_travel = min_trigger_travel 415 416 # Capture start position for travel tracking. Only meaningful if 417 # get_position is wired; otherwise both travel-based guards are 418 # silently no-ops. 419 if self._cb_get_position is not None: 420 self._homing_start_pos = self._cb_get_position() 421 else: 422 self._homing_start_pos = None 423 424 # Set homing state 425 self._is_homing = True 426 self._stall_detected = False 427 428 # Start timeout timer 429 self._homing_timer = wpilib.Timer() 430 self._homing_timer.start() 431 432 # Stall detection timer 433 self._stall_timer = wpilib.Timer() 434 435 # Alerts 436 self._homing_status_alert = wpilib.Alert( 437 f"{self._name}: homing started", 438 wpilib.Alert.AlertType.kInfo 439 ) 440 self._homing_status_alert.set(True) 441 self._homing_error_alert = wpilib.Alert( 442 f"{self._name}: homing failed", 443 wpilib.Alert.AlertType.kError 444 ) 445 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)
max_travel: abort if encoder travels more than this many user
units from the start position. Catches a stuck-open limit
switch that never triggers. None disables the check —
useful when the mechanical range is unknown. Requires
get_position callback to take effect.
min_trigger_travel: minimum encoder travel (user units)
required before a limit switch trigger is trusted. If
the switch fires before this much movement, abort with
a stuck-closed-switch alert. 0.0 disables (default).
Requires get_position callback to take effect.
def
calibration_init( self, max_current: float, max_power_pct: float, max_homing_time: float, min_velocity: float = None, known_range: float = None, max_travel: float | None = None, min_trigger_travel: float = 0.0) -> None:
447 def calibration_init( 448 self, 449 max_current: float, 450 max_power_pct: float, 451 max_homing_time: float, 452 min_velocity: float = None, 453 known_range: float = None, 454 max_travel: Optional[float] = None, 455 min_trigger_travel: float = 0.0, 456 ) -> None: 457 """ 458 Start a calibration routine that discovers the mechanical range. 459 460 Phase 1: Home negative to find the reverse hard limit (set as zero). 461 Phase 2: Home positive to find the forward hard limit (measured). 462 463 If known_range is provided, only phase 1 runs and the forward hard 464 limit is computed as known_range from the zero point. 465 466 Args: 467 max_current: current limit in amps during calibration 468 max_power_pct: motor duty cycle (0.0-1.0) 469 max_homing_time: maximum time per phase before timeout 470 min_velocity: stall detection threshold in user units/second 471 known_range: if provided, skip phase 2 and use this as the 472 full mechanical range from the zero point 473 max_travel: abort a homing phase if encoder travels more 474 than this from its phase start. Defaults to 110% of the 475 current soft-limit range (a sane bound for calibration 476 because the expected range is known). Pass an explicit 477 None via the kwarg to disable the check entirely. 478 min_trigger_travel: minimum travel required before a limit 479 switch trigger is trusted per phase. Guards against a 480 stuck-closed switch firing immediately. 0.0 disables. 481 """ 482 # Validate callbacks before starting 483 self._validate_calibration() 484 485 # Save motor settings for restoration after calibration 486 if self._cb_save_config is not None: 487 self._cal_saved_config = self._cb_save_config() 488 else: 489 self._cal_saved_config = None 490 491 # Disable both soft limits for free travel 492 if self._cb_disable_soft_limits is not None: 493 self._cb_disable_soft_limits(True, True) 494 495 # Default max_travel to 110% of the expected range. Each phase 496 # homes one direction, so the full range is the per-phase ceiling. 497 if max_travel is None: 498 expected_range = abs(self._max_soft_limit - self._min_soft_limit) 499 if expected_range > 0: 500 max_travel = expected_range * 1.1 501 502 # Store calibration parameters for phase transitions 503 self._cal_max_current = max_current 504 self._cal_max_power_pct = max_power_pct 505 self._cal_max_homing_time = max_homing_time 506 self._cal_min_velocity = min_velocity 507 self._cal_known_range = known_range 508 self._cal_max_travel = max_travel 509 self._cal_min_trigger_travel = min_trigger_travel 510 511 self._is_calibrating = True 512 self._calibration_phase = 1 513 514 # Start phase 1: home negative 515 self.homing_init( 516 max_current, max_power_pct, max_homing_time, 517 homing_forward=False, min_velocity=min_velocity, 518 max_travel=max_travel, 519 min_trigger_travel=min_trigger_travel, 520 ) 521 522 self._cal_status_alert = wpilib.Alert( 523 f"{self._name}: calibration phase 1 - homing negative", 524 wpilib.Alert.AlertType.kInfo 525 ) 526 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 max_travel: abort a homing phase if encoder travels more than this from its phase start. Defaults to 110% of the current soft-limit range (a sane bound for calibration because the expected range is known). Pass an explicit None via the kwarg to disable the check entirely. min_trigger_travel: minimum travel required before a limit switch trigger is trusted per phase. Guards against a stuck-closed switch firing immediately. 0.0 disables.
def
periodic(self) -> None:
528 def periodic(self) -> None: 529 """ 530 Run the active homing or calibration state machine. 531 532 Call this every cycle when is_busy is True. 533 """ 534 if self._is_calibrating: 535 self._calibration_periodic() 536 elif self._is_homing: 537 self._homing_periodic()
Run the active homing or calibration state machine.
Call this every cycle when is_busy is True.
def
abort(self) -> None:
539 def abort(self) -> None: 540 """Abort any active homing or calibration routine.""" 541 if self._is_calibrating: 542 self._calibration_end(abort=True) 543 elif self._is_homing: 544 self._homing_end(abort=True)
Abort any active homing or calibration routine.
def
set_soft_limit_margin(self, margin_pct: float) -> None:
546 def set_soft_limit_margin(self, margin_pct: float) -> None: 547 """ 548 Set the soft limit safety margin and apply to the motor controller. 549 550 Args: 551 margin_pct: safety margin as a fraction (e.g. 0.05 for 5%) 552 """ 553 if not self._is_calibrated: 554 return 555 self._soft_limit_margin = margin_pct 556 full_range = self._hard_limit_max - self._hard_limit_min 557 margin = full_range * margin_pct 558 self._min_soft_limit = self._hard_limit_min + margin 559 self._max_soft_limit = self._hard_limit_max - margin 560 561 # Apply to hardware only if callback exists 562 if self._cb_set_soft_limits is not None: 563 self._cb_set_soft_limits( 564 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
compute_conversion_factor( self, current_factor: float, desired_min: float, desired_max: float) -> float:
566 def compute_conversion_factor( 567 self, 568 current_factor: float, 569 desired_min: float, 570 desired_max: float, 571 ) -> float: 572 """ 573 Compute a corrected encoder conversion factor after calibration. 574 575 After a full calibration discovers the raw mechanical range, this 576 method computes the conversion factor needed so the measured 577 range maps to ``[desired_min, desired_max]``. 578 579 Example: if calibration measured 500 user-units of travel with 580 the initial conversion factor and you want -90..90 (180°), the 581 corrected factor scales the encoder so full travel reads 180°. 582 583 Args: 584 current_factor: the position conversion factor currently set 585 on the encoder 586 desired_min: target minimum position in user units 587 desired_max: target maximum position in user units 588 589 Returns: 590 The corrected conversion factor to apply to the encoder. 591 592 Raises: 593 RuntimeError: if calibration has not completed, or the 594 measured range is zero. 595 """ 596 if not self._is_calibrated: 597 raise RuntimeError( 598 f"{self._name}: cannot compute conversion factor " 599 "before calibration is complete" 600 ) 601 measured_range = self._hard_limit_max - self._hard_limit_min 602 if measured_range == 0: 603 raise RuntimeError( 604 f"{self._name}: measured range is zero, " 605 "calibration data invalid" 606 ) 607 desired_range = desired_max - desired_min 608 return current_factor * (desired_range / measured_range)
Compute a corrected encoder conversion factor after calibration.
After a full calibration discovers the raw mechanical range, this
method computes the conversion factor needed so the measured
range maps to [desired_min, desired_max].
Example: if calibration measured 500 user-units of travel with the initial conversion factor and you want -90..90 (180°), the corrected factor scales the encoder so full travel reads 180°.
Args: current_factor: the position conversion factor currently set on the encoder desired_min: target minimum position in user units desired_max: target maximum position in user units
Returns: The corrected conversion factor to apply to the encoder.
Raises: RuntimeError: if calibration has not completed, or the measured range is zero.
def
apply_conversion_factor( self, current_factor: float, desired_min: float, desired_max: float) -> float:
610 def apply_conversion_factor( 611 self, 612 current_factor: float, 613 desired_min: float, 614 desired_max: float, 615 ) -> float: 616 """ 617 Compute and apply a corrected encoder conversion factor. 618 619 Calls :meth:`compute_conversion_factor` for the new value, then 620 applies it via the ``set_conversion_factor`` callback, resets 621 the encoder to ``desired_min`` (the new zero reference), and 622 rewrites the internal hard/soft limits to match the desired 623 range. 624 625 This is intended to be called immediately after a full 626 calibration completes (encoder at the reverse hard stop, 627 reading 0). A guard rejects the call if the encoder reports 628 more than 5% of the current soft-limit range away from zero — 629 the mechanism has moved since calibration, so the post-rescale 630 encoder zero would not correspond to a known physical position. 631 632 Args: 633 current_factor: the position conversion factor currently set 634 on the encoder 635 desired_min: target minimum position in user units (becomes 636 the new zero reference) 637 desired_max: target maximum position in user units 638 639 Returns: 640 The corrected conversion factor that was applied. 641 642 Raises: 643 ValueError: if the ``set_conversion_factor`` callback is 644 not configured. 645 RuntimeError: if calibration has not produced a zero 646 reference, or the mechanism is not at that zero point. 647 """ 648 if self._cb_set_conversion_factor is None: 649 raise ValueError( 650 "set_conversion_factor callback not set" 651 ) 652 653 if not self.is_zeroed: 654 raise RuntimeError( 655 f"{self._name}: cannot apply conversion factor " 656 "before homing to zero position" 657 ) 658 659 # Verify the mechanism is still at the reverse hard stop 660 # (encoder reads ~0) before rescaling. Without this, the post- 661 # rescale encoder zero corresponds to no known physical point. 662 if self._cb_get_position is not None: 663 pos = abs(self._cb_get_position()) 664 full_range = self._max_soft_limit - self._min_soft_limit 665 tolerance = abs(full_range) * 0.05 666 if pos > tolerance: 667 raise RuntimeError( 668 f"{self._name}: mechanism is not at the zero " 669 f"position (current: {pos:.1f}, tolerance: " 670 f"{tolerance:.1f}). Home the mechanism before " 671 "applying a new conversion factor" 672 ) 673 674 new_factor = self.compute_conversion_factor( 675 current_factor, desired_min, desired_max 676 ) 677 self._cb_set_conversion_factor(new_factor) 678 679 # The encoder unit-scale just changed, so its raw reading no 680 # longer maps to the same physical position. Reseed to 681 # desired_min (we calibrated from the reverse hard stop) and 682 # rewrite limits to match the desired range. 683 self._cb_set_position(desired_min) 684 self._hard_limit_min = desired_min 685 self._hard_limit_max = desired_max 686 self.set_soft_limit_margin(self._soft_limit_margin) 687 self._save_to_nt() 688 689 return new_factor
Compute and apply a corrected encoder conversion factor.
Calls compute_conversion_factor() for the new value, then
applies it via the set_conversion_factor callback, resets
the encoder to desired_min (the new zero reference), and
rewrites the internal hard/soft limits to match the desired
range.
This is intended to be called immediately after a full calibration completes (encoder at the reverse hard stop, reading 0). A guard rejects the call if the encoder reports more than 5% of the current soft-limit range away from zero — the mechanism has moved since calibration, so the post-rescale encoder zero would not correspond to a known physical position.
Args: current_factor: the position conversion factor currently set on the encoder desired_min: target minimum position in user units (becomes the new zero reference) desired_max: target maximum position in user units
Returns: The corrected conversion factor that was applied.
Raises:
ValueError: if the set_conversion_factor callback is
not configured.
RuntimeError: if calibration has not produced a zero
reference, or the mechanism is not at that zero point.
def
update_telemetry(self, prefix: str) -> None:
691 def update_telemetry(self, prefix: str) -> None: 692 """ 693 Publish calibration data to SmartDashboard. 694 695 Args: 696 prefix: SmartDashboard key prefix (e.g. "Turret/") 697 """ 698 sd = wpilib.SmartDashboard 699 sd.putBoolean(prefix + "isCalibrated", self._is_calibrated) 700 sd.putBoolean(prefix + "isCalibrating", self._is_calibrating) 701 sd.putBoolean(prefix + "isHoming", self._is_homing) 702 sd.putNumber( 703 prefix + "hardLimitMin", 704 self._hard_limit_min if self._hard_limit_min is not None 705 else 0.0 706 ) 707 sd.putNumber( 708 prefix + "hardLimitMax", 709 self._hard_limit_max if self._hard_limit_max is not None 710 else 0.0 711 ) 712 sd.putNumber(prefix + "positionOffset", self._position_offset) 713 sd.putNumber(prefix + "softLimitMargin", self._soft_limit_margin)
Publish calibration data to SmartDashboard.
Args: prefix: SmartDashboard key prefix (e.g. "Turret/")