utils.passive_range_finder
Passive range finder command for discovering mechanism range of motion.
Sets coast mode so a mechanism can be moved by hand. Tracks min/max encoder positions, detects hard limit switches, and applies the discovered range as soft limits when stopped.
1""" 2Passive range finder command for discovering mechanism range of motion. 3 4Sets coast mode so a mechanism can be moved by hand. Tracks min/max 5encoder positions, detects hard limit switches, and applies the 6discovered range as soft limits when stopped. 7""" 8 9import ntcore 10import rev 11import wpilib 12from commands2 import Command, Subsystem 13 14 15class PassiveRangeFinderCommand(Command): 16 """ 17 Command that lets you passively move a motor through its range of 18 motion to discover min/max positions. 19 20 On start: saves config, sets coast mode so mechanism moves freely. 21 While running: tracks min/max encoder positions, checks limit switches. 22 On stop: applies discovered range as soft limits, restores idle mode. 23 24 Bind to a button with toggleOnTrue for start/stop behavior. 25 """ 26 27 def __init__( 28 self, 29 motor: rev.SparkMax, 30 name: str, 31 subsystem: Subsystem, 32 zero_on_end: bool = False, 33 full_range: float = None, 34 ) -> None: 35 super().__init__() 36 self.motor = motor 37 self.encoder = motor.getEncoder() 38 self._name = name 39 self._subsystem = subsystem 40 self._zero_on_end = zero_on_end 41 self._full_range = full_range 42 self.addRequirements(subsystem) 43 44 # State 45 self._min_raw = 0.0 46 self._max_raw = 0.0 47 self._start_time = 0.0 48 self._original_conversion_factor = 1.0 49 self._original_idle_mode = rev.SparkBaseConfig.IdleMode.kBrake 50 self._hit_forward_limit = False 51 self._hit_reverse_limit = False 52 self._forward_limit_position = 0.0 53 self._reverse_limit_position = 0.0 54 55 # NT table 56 self._table = ntcore.NetworkTableInstance.getDefault().getTable( 57 f"PassiveRangeFinder/{name}") 58 59 # Alerts 60 self._fwd_alert = wpilib.Alert( 61 f"PassiveRangeFinder/{name}: Forward hard limit hit", 62 wpilib.Alert.AlertType.kWarning) 63 self._rev_alert = wpilib.Alert( 64 f"PassiveRangeFinder/{name}: Reverse hard limit hit", 65 wpilib.Alert.AlertType.kWarning) 66 self._result_alert = wpilib.Alert( 67 f"PassiveRangeFinder/{name}: Results pending", 68 wpilib.Alert.AlertType.kInfo) 69 70 def initialize(self) -> None: 71 """Save config, set coast mode, start tracking.""" 72 ca = self.motor.configAccessor 73 74 # Save original config 75 self._original_idle_mode = ca.getIdleMode() 76 self._original_conversion_factor = ( 77 ca.encoder.getPositionConversionFactor()) 78 79 # Set conversion factor to 1.0 for raw encoder units, coast mode 80 # position: 1.0 = raw rotations 81 # velocity: 1/60 = rotations/sec (from RPM) 82 cfg = rev.SparkMaxConfig() 83 cfg.setIdleMode(rev.SparkBaseConfig.IdleMode.kCoast) 84 ( 85 cfg.encoder 86 .positionConversionFactor(1.0) 87 .velocityConversionFactor(1.0 / 60.0) 88 ) 89 self.motor.configure( 90 cfg, 91 rev.ResetMode.kNoResetSafeParameters, 92 rev.PersistMode.kNoPersistParameters 93 ) 94 95 # Zero encoder and reset tracking 96 self.encoder.setPosition(0) 97 pos = 0.0 98 self._min_raw = pos 99 self._max_raw = pos 100 self._hit_forward_limit = False 101 self._hit_reverse_limit = False 102 self._forward_limit_position = 0.0 103 self._reverse_limit_position = 0.0 104 self._start_time = wpilib.Timer.getFPGATimestamp() 105 106 # Clear alerts 107 self._fwd_alert.set(False) 108 self._rev_alert.set(False) 109 self._result_alert.set(False) 110 111 # Initialize NT entries 112 self._publish_nt(pos) 113 114 print(f"[PassiveRangeFinder/{self._name}] Started. " 115 f"Move mechanism through its range, then press button again.") 116 117 def execute(self) -> None: 118 """Track min/max position and check limit switches.""" 119 pos = self.encoder.getPosition() 120 121 # Update min/max 122 if pos < self._min_raw: 123 self._min_raw = pos 124 if pos > self._max_raw: 125 self._max_raw = pos 126 127 # Check forward limit switch 128 if (not self._hit_forward_limit 129 and self.motor.getForwardLimitSwitch().get()): 130 self._hit_forward_limit = True 131 self._forward_limit_position = pos 132 self._fwd_alert.setText( 133 f"PassiveRangeFinder/{self._name}: Forward hard limit " 134 f"at raw={pos:.4f} " 135 f"normalized={pos * self._original_conversion_factor:.2f}") 136 self._fwd_alert.set(True) 137 138 # Check reverse limit switch 139 if (not self._hit_reverse_limit 140 and self.motor.getReverseLimitSwitch().get()): 141 self._hit_reverse_limit = True 142 self._reverse_limit_position = pos 143 self._rev_alert.setText( 144 f"PassiveRangeFinder/{self._name}: Reverse hard limit " 145 f"at raw={pos:.4f} " 146 f"normalized={pos * self._original_conversion_factor:.2f}") 147 self._rev_alert.set(True) 148 149 self._publish_nt(pos) 150 151 def end(self, interrupted: bool) -> None: 152 """Apply discovered range as soft limits and restore idle mode.""" 153 self.motor.stopMotor() 154 155 cfg = rev.SparkMaxConfig() 156 cfg.setIdleMode(self._original_idle_mode) 157 158 # Determine best limits in raw encoder units 159 fwd_raw = (self._forward_limit_position 160 if self._hit_forward_limit else self._max_raw) 161 rev_raw = (self._reverse_limit_position 162 if self._hit_reverse_limit else self._min_raw) 163 164 # Offset relative to current position when zeroing 165 current_raw = self.encoder.getPosition() 166 if self._zero_on_end: 167 fwd_raw -= current_raw 168 rev_raw -= current_raw 169 170 range_raw = self._max_raw - self._min_raw 171 172 # Compute new conversion factor from measured range. 173 # full_range given (e.g. 20 degrees): encoder reads 0..20 174 # full_range not given: encoder reads 0..1 175 if range_raw > 0: 176 if self._full_range is not None: 177 cf = self._full_range / range_raw 178 else: 179 cf = 1.0 / range_raw 180 else: 181 cf = self._original_conversion_factor 182 183 # position: cf units/rotation (deg or normalized) 184 # velocity: cf/60 = units/sec (from RPM) 185 ( 186 cfg.encoder 187 .positionConversionFactor(cf) 188 .velocityConversionFactor(cf / 60.0) 189 ) 190 191 # Apply soft limits in the new units 192 fwd_scaled = fwd_raw * cf 193 rev_scaled = rev_raw * cf 194 195 ( 196 cfg.softLimit 197 .forwardSoftLimit(fwd_scaled) 198 .forwardSoftLimitEnabled(True) 199 .reverseSoftLimit(rev_scaled) 200 .reverseSoftLimitEnabled(True) 201 ) 202 203 elapsed = wpilib.Timer.getFPGATimestamp() - self._start_time 204 unit = ("deg" if self._full_range is not None 205 else "normalized") 206 207 summary = ( 208 f"[PassiveRangeFinder/{self._name}] Complete.\n" 209 f" Raw rotations: min={self._min_raw:.4f} " 210 f"max={self._max_raw:.4f} range={range_raw:.4f}\n" 211 f" Conversion factor: {cf:.4f} {unit}/rotation\n" 212 f" Soft limits: [{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit}" 213 ) 214 if self._zero_on_end: 215 summary += ( 216 f"\n Zeroed at raw={current_raw:.4f}") 217 summary += ( 218 f"\n Forward limit hit: {self._hit_forward_limit}\n" 219 f" Reverse limit hit: {self._hit_reverse_limit}\n" 220 f" Elapsed: {elapsed:.1f}s" 221 ) 222 print(summary) 223 224 limits_str = ( 225 'fwd+rev' if self._hit_forward_limit 226 and self._hit_reverse_limit 227 else 'fwd' if self._hit_forward_limit 228 else 'rev' if self._hit_reverse_limit 229 else 'none') 230 self._result_alert.setText( 231 f"PassiveRangeFinder/{self._name}: " 232 f"range=[{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit} " 233 f"hardLimits={limits_str}") 234 self._result_alert.set(True) 235 236 self.motor.configure( 237 cfg, 238 rev.ResetMode.kNoResetSafeParameters, 239 rev.PersistMode.kNoPersistParameters 240 ) 241 242 # Zero the encoder after config is applied so soft limits 243 # are relative to the new zero position. 244 if self._zero_on_end: 245 self.encoder.setPosition(0) 246 247 # Tell the subsystem to hold at current position so its 248 # periodic PID doesn't drive back to the old target. 249 if hasattr(self._subsystem, 'disable') and callable( 250 self._subsystem.disable): 251 self._subsystem.disable() 252 253 # Final NT update 254 self._table.putBoolean("active", False) 255 256 def isFinished(self) -> bool: 257 return False 258 259 def _publish_nt(self, position: float) -> None: 260 """Publish all telemetry to NetworkTables.""" 261 t = self._table 262 elapsed = wpilib.Timer.getFPGATimestamp() - self._start_time 263 range_raw = self._max_raw - self._min_raw 264 265 # Live normalized: 0..1 based on range seen so far 266 if range_raw > 0: 267 norm = (position - self._min_raw) / range_raw 268 else: 269 norm = 0.0 270 271 t.putBoolean("active", True) 272 t.putNumber("currentRaw", position) 273 t.putNumber("currentNormalized", norm) 274 t.putNumber("minRaw", self._min_raw) 275 t.putNumber("maxRaw", self._max_raw) 276 t.putNumber("rangeRaw", range_raw) 277 t.putNumber("elapsedTime", elapsed) 278 t.putBoolean("passedForwardHardLimit", self._hit_forward_limit) 279 t.putBoolean("passedReverseHardLimit", self._hit_reverse_limit) 280 t.putNumber("forwardLimitRaw", self._forward_limit_position) 281 t.putNumber("reverseLimitRaw", self._reverse_limit_position)
class
PassiveRangeFinderCommand(commands2.command.Command):
16class PassiveRangeFinderCommand(Command): 17 """ 18 Command that lets you passively move a motor through its range of 19 motion to discover min/max positions. 20 21 On start: saves config, sets coast mode so mechanism moves freely. 22 While running: tracks min/max encoder positions, checks limit switches. 23 On stop: applies discovered range as soft limits, restores idle mode. 24 25 Bind to a button with toggleOnTrue for start/stop behavior. 26 """ 27 28 def __init__( 29 self, 30 motor: rev.SparkMax, 31 name: str, 32 subsystem: Subsystem, 33 zero_on_end: bool = False, 34 full_range: float = None, 35 ) -> None: 36 super().__init__() 37 self.motor = motor 38 self.encoder = motor.getEncoder() 39 self._name = name 40 self._subsystem = subsystem 41 self._zero_on_end = zero_on_end 42 self._full_range = full_range 43 self.addRequirements(subsystem) 44 45 # State 46 self._min_raw = 0.0 47 self._max_raw = 0.0 48 self._start_time = 0.0 49 self._original_conversion_factor = 1.0 50 self._original_idle_mode = rev.SparkBaseConfig.IdleMode.kBrake 51 self._hit_forward_limit = False 52 self._hit_reverse_limit = False 53 self._forward_limit_position = 0.0 54 self._reverse_limit_position = 0.0 55 56 # NT table 57 self._table = ntcore.NetworkTableInstance.getDefault().getTable( 58 f"PassiveRangeFinder/{name}") 59 60 # Alerts 61 self._fwd_alert = wpilib.Alert( 62 f"PassiveRangeFinder/{name}: Forward hard limit hit", 63 wpilib.Alert.AlertType.kWarning) 64 self._rev_alert = wpilib.Alert( 65 f"PassiveRangeFinder/{name}: Reverse hard limit hit", 66 wpilib.Alert.AlertType.kWarning) 67 self._result_alert = wpilib.Alert( 68 f"PassiveRangeFinder/{name}: Results pending", 69 wpilib.Alert.AlertType.kInfo) 70 71 def initialize(self) -> None: 72 """Save config, set coast mode, start tracking.""" 73 ca = self.motor.configAccessor 74 75 # Save original config 76 self._original_idle_mode = ca.getIdleMode() 77 self._original_conversion_factor = ( 78 ca.encoder.getPositionConversionFactor()) 79 80 # Set conversion factor to 1.0 for raw encoder units, coast mode 81 # position: 1.0 = raw rotations 82 # velocity: 1/60 = rotations/sec (from RPM) 83 cfg = rev.SparkMaxConfig() 84 cfg.setIdleMode(rev.SparkBaseConfig.IdleMode.kCoast) 85 ( 86 cfg.encoder 87 .positionConversionFactor(1.0) 88 .velocityConversionFactor(1.0 / 60.0) 89 ) 90 self.motor.configure( 91 cfg, 92 rev.ResetMode.kNoResetSafeParameters, 93 rev.PersistMode.kNoPersistParameters 94 ) 95 96 # Zero encoder and reset tracking 97 self.encoder.setPosition(0) 98 pos = 0.0 99 self._min_raw = pos 100 self._max_raw = pos 101 self._hit_forward_limit = False 102 self._hit_reverse_limit = False 103 self._forward_limit_position = 0.0 104 self._reverse_limit_position = 0.0 105 self._start_time = wpilib.Timer.getFPGATimestamp() 106 107 # Clear alerts 108 self._fwd_alert.set(False) 109 self._rev_alert.set(False) 110 self._result_alert.set(False) 111 112 # Initialize NT entries 113 self._publish_nt(pos) 114 115 print(f"[PassiveRangeFinder/{self._name}] Started. " 116 f"Move mechanism through its range, then press button again.") 117 118 def execute(self) -> None: 119 """Track min/max position and check limit switches.""" 120 pos = self.encoder.getPosition() 121 122 # Update min/max 123 if pos < self._min_raw: 124 self._min_raw = pos 125 if pos > self._max_raw: 126 self._max_raw = pos 127 128 # Check forward limit switch 129 if (not self._hit_forward_limit 130 and self.motor.getForwardLimitSwitch().get()): 131 self._hit_forward_limit = True 132 self._forward_limit_position = pos 133 self._fwd_alert.setText( 134 f"PassiveRangeFinder/{self._name}: Forward hard limit " 135 f"at raw={pos:.4f} " 136 f"normalized={pos * self._original_conversion_factor:.2f}") 137 self._fwd_alert.set(True) 138 139 # Check reverse limit switch 140 if (not self._hit_reverse_limit 141 and self.motor.getReverseLimitSwitch().get()): 142 self._hit_reverse_limit = True 143 self._reverse_limit_position = pos 144 self._rev_alert.setText( 145 f"PassiveRangeFinder/{self._name}: Reverse hard limit " 146 f"at raw={pos:.4f} " 147 f"normalized={pos * self._original_conversion_factor:.2f}") 148 self._rev_alert.set(True) 149 150 self._publish_nt(pos) 151 152 def end(self, interrupted: bool) -> None: 153 """Apply discovered range as soft limits and restore idle mode.""" 154 self.motor.stopMotor() 155 156 cfg = rev.SparkMaxConfig() 157 cfg.setIdleMode(self._original_idle_mode) 158 159 # Determine best limits in raw encoder units 160 fwd_raw = (self._forward_limit_position 161 if self._hit_forward_limit else self._max_raw) 162 rev_raw = (self._reverse_limit_position 163 if self._hit_reverse_limit else self._min_raw) 164 165 # Offset relative to current position when zeroing 166 current_raw = self.encoder.getPosition() 167 if self._zero_on_end: 168 fwd_raw -= current_raw 169 rev_raw -= current_raw 170 171 range_raw = self._max_raw - self._min_raw 172 173 # Compute new conversion factor from measured range. 174 # full_range given (e.g. 20 degrees): encoder reads 0..20 175 # full_range not given: encoder reads 0..1 176 if range_raw > 0: 177 if self._full_range is not None: 178 cf = self._full_range / range_raw 179 else: 180 cf = 1.0 / range_raw 181 else: 182 cf = self._original_conversion_factor 183 184 # position: cf units/rotation (deg or normalized) 185 # velocity: cf/60 = units/sec (from RPM) 186 ( 187 cfg.encoder 188 .positionConversionFactor(cf) 189 .velocityConversionFactor(cf / 60.0) 190 ) 191 192 # Apply soft limits in the new units 193 fwd_scaled = fwd_raw * cf 194 rev_scaled = rev_raw * cf 195 196 ( 197 cfg.softLimit 198 .forwardSoftLimit(fwd_scaled) 199 .forwardSoftLimitEnabled(True) 200 .reverseSoftLimit(rev_scaled) 201 .reverseSoftLimitEnabled(True) 202 ) 203 204 elapsed = wpilib.Timer.getFPGATimestamp() - self._start_time 205 unit = ("deg" if self._full_range is not None 206 else "normalized") 207 208 summary = ( 209 f"[PassiveRangeFinder/{self._name}] Complete.\n" 210 f" Raw rotations: min={self._min_raw:.4f} " 211 f"max={self._max_raw:.4f} range={range_raw:.4f}\n" 212 f" Conversion factor: {cf:.4f} {unit}/rotation\n" 213 f" Soft limits: [{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit}" 214 ) 215 if self._zero_on_end: 216 summary += ( 217 f"\n Zeroed at raw={current_raw:.4f}") 218 summary += ( 219 f"\n Forward limit hit: {self._hit_forward_limit}\n" 220 f" Reverse limit hit: {self._hit_reverse_limit}\n" 221 f" Elapsed: {elapsed:.1f}s" 222 ) 223 print(summary) 224 225 limits_str = ( 226 'fwd+rev' if self._hit_forward_limit 227 and self._hit_reverse_limit 228 else 'fwd' if self._hit_forward_limit 229 else 'rev' if self._hit_reverse_limit 230 else 'none') 231 self._result_alert.setText( 232 f"PassiveRangeFinder/{self._name}: " 233 f"range=[{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit} " 234 f"hardLimits={limits_str}") 235 self._result_alert.set(True) 236 237 self.motor.configure( 238 cfg, 239 rev.ResetMode.kNoResetSafeParameters, 240 rev.PersistMode.kNoPersistParameters 241 ) 242 243 # Zero the encoder after config is applied so soft limits 244 # are relative to the new zero position. 245 if self._zero_on_end: 246 self.encoder.setPosition(0) 247 248 # Tell the subsystem to hold at current position so its 249 # periodic PID doesn't drive back to the old target. 250 if hasattr(self._subsystem, 'disable') and callable( 251 self._subsystem.disable): 252 self._subsystem.disable() 253 254 # Final NT update 255 self._table.putBoolean("active", False) 256 257 def isFinished(self) -> bool: 258 return False 259 260 def _publish_nt(self, position: float) -> None: 261 """Publish all telemetry to NetworkTables.""" 262 t = self._table 263 elapsed = wpilib.Timer.getFPGATimestamp() - self._start_time 264 range_raw = self._max_raw - self._min_raw 265 266 # Live normalized: 0..1 based on range seen so far 267 if range_raw > 0: 268 norm = (position - self._min_raw) / range_raw 269 else: 270 norm = 0.0 271 272 t.putBoolean("active", True) 273 t.putNumber("currentRaw", position) 274 t.putNumber("currentNormalized", norm) 275 t.putNumber("minRaw", self._min_raw) 276 t.putNumber("maxRaw", self._max_raw) 277 t.putNumber("rangeRaw", range_raw) 278 t.putNumber("elapsedTime", elapsed) 279 t.putBoolean("passedForwardHardLimit", self._hit_forward_limit) 280 t.putBoolean("passedReverseHardLimit", self._hit_reverse_limit) 281 t.putNumber("forwardLimitRaw", self._forward_limit_position) 282 t.putNumber("reverseLimitRaw", self._reverse_limit_position)
Command that lets you passively move a motor through its range of motion to discover min/max positions.
On start: saves config, sets coast mode so mechanism moves freely. While running: tracks min/max encoder positions, checks limit switches. On stop: applies discovered range as soft limits, restores idle mode.
Bind to a button with toggleOnTrue for start/stop behavior.
PassiveRangeFinderCommand( motor: rev._rev.SparkMax, name: str, subsystem: commands2.subsystem.Subsystem, zero_on_end: bool = False, full_range: float = None)
28 def __init__( 29 self, 30 motor: rev.SparkMax, 31 name: str, 32 subsystem: Subsystem, 33 zero_on_end: bool = False, 34 full_range: float = None, 35 ) -> None: 36 super().__init__() 37 self.motor = motor 38 self.encoder = motor.getEncoder() 39 self._name = name 40 self._subsystem = subsystem 41 self._zero_on_end = zero_on_end 42 self._full_range = full_range 43 self.addRequirements(subsystem) 44 45 # State 46 self._min_raw = 0.0 47 self._max_raw = 0.0 48 self._start_time = 0.0 49 self._original_conversion_factor = 1.0 50 self._original_idle_mode = rev.SparkBaseConfig.IdleMode.kBrake 51 self._hit_forward_limit = False 52 self._hit_reverse_limit = False 53 self._forward_limit_position = 0.0 54 self._reverse_limit_position = 0.0 55 56 # NT table 57 self._table = ntcore.NetworkTableInstance.getDefault().getTable( 58 f"PassiveRangeFinder/{name}") 59 60 # Alerts 61 self._fwd_alert = wpilib.Alert( 62 f"PassiveRangeFinder/{name}: Forward hard limit hit", 63 wpilib.Alert.AlertType.kWarning) 64 self._rev_alert = wpilib.Alert( 65 f"PassiveRangeFinder/{name}: Reverse hard limit hit", 66 wpilib.Alert.AlertType.kWarning) 67 self._result_alert = wpilib.Alert( 68 f"PassiveRangeFinder/{name}: Results pending", 69 wpilib.Alert.AlertType.kInfo)
__init__(self: wpiutil._wpiutil.Sendable) -> None
def
initialize(self) -> None:
71 def initialize(self) -> None: 72 """Save config, set coast mode, start tracking.""" 73 ca = self.motor.configAccessor 74 75 # Save original config 76 self._original_idle_mode = ca.getIdleMode() 77 self._original_conversion_factor = ( 78 ca.encoder.getPositionConversionFactor()) 79 80 # Set conversion factor to 1.0 for raw encoder units, coast mode 81 # position: 1.0 = raw rotations 82 # velocity: 1/60 = rotations/sec (from RPM) 83 cfg = rev.SparkMaxConfig() 84 cfg.setIdleMode(rev.SparkBaseConfig.IdleMode.kCoast) 85 ( 86 cfg.encoder 87 .positionConversionFactor(1.0) 88 .velocityConversionFactor(1.0 / 60.0) 89 ) 90 self.motor.configure( 91 cfg, 92 rev.ResetMode.kNoResetSafeParameters, 93 rev.PersistMode.kNoPersistParameters 94 ) 95 96 # Zero encoder and reset tracking 97 self.encoder.setPosition(0) 98 pos = 0.0 99 self._min_raw = pos 100 self._max_raw = pos 101 self._hit_forward_limit = False 102 self._hit_reverse_limit = False 103 self._forward_limit_position = 0.0 104 self._reverse_limit_position = 0.0 105 self._start_time = wpilib.Timer.getFPGATimestamp() 106 107 # Clear alerts 108 self._fwd_alert.set(False) 109 self._rev_alert.set(False) 110 self._result_alert.set(False) 111 112 # Initialize NT entries 113 self._publish_nt(pos) 114 115 print(f"[PassiveRangeFinder/{self._name}] Started. " 116 f"Move mechanism through its range, then press button again.")
Save config, set coast mode, start tracking.
def
execute(self) -> None:
118 def execute(self) -> None: 119 """Track min/max position and check limit switches.""" 120 pos = self.encoder.getPosition() 121 122 # Update min/max 123 if pos < self._min_raw: 124 self._min_raw = pos 125 if pos > self._max_raw: 126 self._max_raw = pos 127 128 # Check forward limit switch 129 if (not self._hit_forward_limit 130 and self.motor.getForwardLimitSwitch().get()): 131 self._hit_forward_limit = True 132 self._forward_limit_position = pos 133 self._fwd_alert.setText( 134 f"PassiveRangeFinder/{self._name}: Forward hard limit " 135 f"at raw={pos:.4f} " 136 f"normalized={pos * self._original_conversion_factor:.2f}") 137 self._fwd_alert.set(True) 138 139 # Check reverse limit switch 140 if (not self._hit_reverse_limit 141 and self.motor.getReverseLimitSwitch().get()): 142 self._hit_reverse_limit = True 143 self._reverse_limit_position = pos 144 self._rev_alert.setText( 145 f"PassiveRangeFinder/{self._name}: Reverse hard limit " 146 f"at raw={pos:.4f} " 147 f"normalized={pos * self._original_conversion_factor:.2f}") 148 self._rev_alert.set(True) 149 150 self._publish_nt(pos)
Track min/max position and check limit switches.
def
end(self, interrupted: bool) -> None:
152 def end(self, interrupted: bool) -> None: 153 """Apply discovered range as soft limits and restore idle mode.""" 154 self.motor.stopMotor() 155 156 cfg = rev.SparkMaxConfig() 157 cfg.setIdleMode(self._original_idle_mode) 158 159 # Determine best limits in raw encoder units 160 fwd_raw = (self._forward_limit_position 161 if self._hit_forward_limit else self._max_raw) 162 rev_raw = (self._reverse_limit_position 163 if self._hit_reverse_limit else self._min_raw) 164 165 # Offset relative to current position when zeroing 166 current_raw = self.encoder.getPosition() 167 if self._zero_on_end: 168 fwd_raw -= current_raw 169 rev_raw -= current_raw 170 171 range_raw = self._max_raw - self._min_raw 172 173 # Compute new conversion factor from measured range. 174 # full_range given (e.g. 20 degrees): encoder reads 0..20 175 # full_range not given: encoder reads 0..1 176 if range_raw > 0: 177 if self._full_range is not None: 178 cf = self._full_range / range_raw 179 else: 180 cf = 1.0 / range_raw 181 else: 182 cf = self._original_conversion_factor 183 184 # position: cf units/rotation (deg or normalized) 185 # velocity: cf/60 = units/sec (from RPM) 186 ( 187 cfg.encoder 188 .positionConversionFactor(cf) 189 .velocityConversionFactor(cf / 60.0) 190 ) 191 192 # Apply soft limits in the new units 193 fwd_scaled = fwd_raw * cf 194 rev_scaled = rev_raw * cf 195 196 ( 197 cfg.softLimit 198 .forwardSoftLimit(fwd_scaled) 199 .forwardSoftLimitEnabled(True) 200 .reverseSoftLimit(rev_scaled) 201 .reverseSoftLimitEnabled(True) 202 ) 203 204 elapsed = wpilib.Timer.getFPGATimestamp() - self._start_time 205 unit = ("deg" if self._full_range is not None 206 else "normalized") 207 208 summary = ( 209 f"[PassiveRangeFinder/{self._name}] Complete.\n" 210 f" Raw rotations: min={self._min_raw:.4f} " 211 f"max={self._max_raw:.4f} range={range_raw:.4f}\n" 212 f" Conversion factor: {cf:.4f} {unit}/rotation\n" 213 f" Soft limits: [{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit}" 214 ) 215 if self._zero_on_end: 216 summary += ( 217 f"\n Zeroed at raw={current_raw:.4f}") 218 summary += ( 219 f"\n Forward limit hit: {self._hit_forward_limit}\n" 220 f" Reverse limit hit: {self._hit_reverse_limit}\n" 221 f" Elapsed: {elapsed:.1f}s" 222 ) 223 print(summary) 224 225 limits_str = ( 226 'fwd+rev' if self._hit_forward_limit 227 and self._hit_reverse_limit 228 else 'fwd' if self._hit_forward_limit 229 else 'rev' if self._hit_reverse_limit 230 else 'none') 231 self._result_alert.setText( 232 f"PassiveRangeFinder/{self._name}: " 233 f"range=[{rev_scaled:.2f}, {fwd_scaled:.2f}] {unit} " 234 f"hardLimits={limits_str}") 235 self._result_alert.set(True) 236 237 self.motor.configure( 238 cfg, 239 rev.ResetMode.kNoResetSafeParameters, 240 rev.PersistMode.kNoPersistParameters 241 ) 242 243 # Zero the encoder after config is applied so soft limits 244 # are relative to the new zero position. 245 if self._zero_on_end: 246 self.encoder.setPosition(0) 247 248 # Tell the subsystem to hold at current position so its 249 # periodic PID doesn't drive back to the old target. 250 if hasattr(self._subsystem, 'disable') and callable( 251 self._subsystem.disable): 252 self._subsystem.disable() 253 254 # Final NT update 255 self._table.putBoolean("active", False)
Apply discovered range as soft limits and restore idle mode.