subsystem.mechanisms.turret
1# Native imports 2import math 3 4# Third-party imports 5import rev 6import wpilib 7from commands2 import Command, Subsystem 8from commands2.sysid import SysIdRoutine 9from wpilib.sysid import SysIdRoutineLog 10from wpimath.controller import PIDController 11 12# Internal imports 13from utils.position_calibration import PositionCalibration 14from utils.spark_max_callbacks import SparkMaxCallbacks 15from utils.spark_utils import GetSparkSignalsPositionControlConfig 16 17 18class Turret(Subsystem): 19 """ 20 Subsystem for a single-motor turret with position control and soft limits. 21 22 The turret uses a SparkMax motor controller to rotate within defined 23 angular boundaries. Position is measured in degrees via the internal 24 relative encoder with a configurable conversion factor. 25 26 Calibration and homing are delegated to self.calibration 27 (PositionCalibration), a reusable controller that discovers mechanical 28 limits and persists them across reboots. 29 """ 30 31 def __init__( 32 self, 33 motor: rev.SparkMax, 34 position_conversion_factor: float, 35 min_soft_limit: float, 36 max_soft_limit: float 37 ) -> None: 38 """ 39 Creates a new Turret subsystem. 40 41 Args: 42 motor: the SparkMax motor controller driving the turret 43 position_conversion_factor: encoder conversion factor that converts 44 raw encoder rotations to degrees 45 min_soft_limit: the minimum turret position in degrees (reverse limit) 46 max_soft_limit: the maximum turret position in degrees (forward limit) 47 48 Returns: 49 None: class initialization executed upon construction 50 """ 51 super().__init__() 52 self.motor = motor 53 self.encoder = self.motor.getEncoder() 54 self.controller = PIDController(0, 0, 0) 55 wpilib.SmartDashboard.putData( 56 self.getName() + "/pid", self.controller) 57 58 self.min_soft_limit = min_soft_limit 59 self.max_soft_limit = max_soft_limit 60 61 # Voltage output limits 62 self._min_output_voltage = -12.0 63 self._max_output_voltage = 12.0 64 65 # Position tracking state 66 self._target_position = None 67 68 # Calibration controller (callbacks set up in setupCalibration) 69 self.calibration = self.setupCalibration( 70 min_soft_limit, max_soft_limit) 71 72 self.configureMechanism2d() 73 74 config = rev.SparkMaxConfig() 75 76 # General motor settings 77 ( 78 config 79 .setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake) 80 .voltageCompensation(12.0) 81 .smartCurrentLimit(40) 82 ) 83 84 # Encoder conversion factors 85 # Position: rotations -> degrees 86 # Velocity: RPM -> degrees/second (divide by 60) 87 velocity_conversion_factor = position_conversion_factor / 60.0 88 ( 89 config.encoder 90 .positionConversionFactor(position_conversion_factor) 91 .velocityConversionFactor(velocity_conversion_factor) 92 ) 93 94 # Soft limits in degrees 95 ( 96 config.softLimit 97 .forwardSoftLimit(max_soft_limit) 98 .forwardSoftLimitEnabled(True) 99 .reverseSoftLimit(min_soft_limit) 100 .reverseSoftLimitEnabled(True) 101 ) 102 103 # Telemetry signals at 20ms 104 GetSparkSignalsPositionControlConfig(config.signals, 20) 105 106 # Apply configuration 107 self.motor.configure( 108 config, 109 rev.ResetMode.kResetSafeParameters, 110 rev.PersistMode.kNoPersistParameters 111 ) 112 113 def setupCalibration( 114 self, 115 min_soft_limit: float, 116 max_soft_limit: float, 117 ) -> PositionCalibration: 118 """ 119 Create and configure the calibration controller for this turret. 120 121 Creates a PositionCalibration with no callbacks, then sets them 122 up using set_callbacks(). This keeps __init__ clean and puts all 123 the calibration wiring in one place. 124 125 Each callback is a small function that tells PositionCalibration 126 how to talk to the motor and encoder. If your mechanism has no 127 limit switches, just leave those callbacks out and homing will 128 use stall detection instead. 129 130 Args: 131 min_soft_limit: the minimum turret position in degrees 132 max_soft_limit: the maximum turret position in degrees 133 134 Returns: 135 The configured PositionCalibration controller 136 """ 137 # Step 1: Create calibration with no callbacks 138 cal = PositionCalibration( 139 name=self.getName(), 140 fallback_min=min_soft_limit, 141 fallback_max=max_soft_limit, 142 ) 143 144 # Step 2: Build callbacks from the SparkMax motor and encoder. 145 # SparkMaxCallbacks generates all the callbacks for you. 146 # You can also write your own lambdas instead, for example: 147 # set_motor_output=lambda pct: self.motor.set(pct), 148 # stop_motor=lambda: self.motor.stopMotor(), 149 spark_cbs = SparkMaxCallbacks(self.motor, self.encoder).as_dict() 150 151 # Step 3: Set callbacks on the calibration controller. 152 # Each callback tells PositionCalibration how to interact 153 # with the hardware. See PositionCalibration docs for the 154 # full list of available callbacks. 155 cal.set_callbacks( 156 # --- Core required (must have all three) --- 157 # Drive the motor at a duty cycle (-1.0 to 1.0) 158 set_motor_output=spark_cbs['set_motor_output'], 159 # Stop the motor 160 stop_motor=spark_cbs['stop_motor'], 161 # Reset the encoder position to a value 162 set_position=spark_cbs['set_position'], 163 164 # --- Detection (need at least one per homing direction) --- 165 # Read encoder velocity for stall detection 166 get_velocity=spark_cbs['get_velocity'], 167 # Read limit switches (leave out if not installed) 168 get_forward_limit_switch=( 169 spark_cbs['get_forward_limit_switch']), 170 get_reverse_limit_switch=( 171 spark_cbs['get_reverse_limit_switch']), 172 173 # --- Optional (make homing safer but not required) --- 174 # Read encoder position (needed for calibration phase 2) 175 get_position=spark_cbs['get_position'], 176 # Lower current limit during homing to protect the motor 177 set_current_limit=spark_cbs['set_current_limit'], 178 # Apply soft limits to the motor controller 179 set_soft_limits=spark_cbs['set_soft_limits'], 180 # Disable soft limits so the motor can travel freely 181 disable_soft_limits=spark_cbs['disable_soft_limits'], 182 # Save motor config before homing, restore it after 183 save_config=spark_cbs['save_config'], 184 restore_config=spark_cbs['restore_config'], 185 ) 186 187 # ---- Shortcut: motor= does the same thing in one line ---- 188 # If you don't need to customize anything, you can replace 189 # all of the above with: 190 # 191 # cal = PositionCalibration( 192 # name=self.getName(), 193 # fallback_min=min_soft_limit, 194 # fallback_max=max_soft_limit, 195 # motor=self.motor, 196 # encoder=self.encoder, 197 # ) 198 199 return cal 200 201 def setMotorVoltage(self, output: float) -> None: 202 """ 203 Set the motor voltage directly. Used by SysId routines to drive 204 the turret at controlled voltages for characterization. 205 Blocked during homing/calibration for safety. 206 207 Args: 208 output: voltage to apply to the motor 209 210 Returns: 211 None 212 """ 213 if self.calibration.is_busy: 214 return 215 self.motor.setVoltage(output) 216 217 def setPosition(self, position_degrees: float) -> None: 218 """ 219 Set the target turret position. The periodic method drives the PID 220 controller to this position each cycle. Blocked during homing and 221 calibration for safety. 222 223 Args: 224 position_degrees: the target turret position in degrees 225 226 Returns: 227 None 228 """ 229 if self.calibration.is_busy: 230 return 231 self._target_position = max( 232 self.calibration.min_soft_limit, 233 min(self.calibration.max_soft_limit, position_degrees) 234 ) 235 236 def getPosition(self) -> float: 237 """ 238 Get the current turret position in degrees. 239 240 Returns: 241 The current position of the turret in degrees 242 """ 243 return self.encoder.getPosition() 244 245 def getVelocity(self) -> float: 246 """ 247 Get the current turret angular velocity in degrees per second. 248 249 Returns: 250 The current angular velocity of the turret in degrees per second 251 """ 252 return self.encoder.getVelocity() 253 254 @property 255 def is_homing(self) -> bool: 256 """Whether the turret is currently in a homing routine.""" 257 return self.calibration.is_homing 258 259 def turretDisable(self) -> None: 260 """ 261 Disable turret motor output and clear the target position. 262 The turret will idle until a new position is set. 263 264 Returns: 265 None 266 """ 267 self._target_position = None 268 self.motor.stopMotor() 269 270 def configureMechanism2d(self) -> None: 271 """ 272 Create and publish a Mechanism2d widget for turret visualization. 273 274 Sets up a 2D canvas with: 275 - Red arm: current encoder position 276 - Green arm: target setpoint position 277 - Gray arms: static soft limit indicators 278 279 Returns: 280 None 281 """ 282 self.mech2d = wpilib.Mechanism2d(200, 200) 283 pivot = self.mech2d.getRoot("turret_pivot", 100, 100) 284 self.mech_current_arm = pivot.appendLigament( 285 "current_position", 80, 0, 6, 286 wpilib.Color8Bit(wpilib.Color.kRed) 287 ) 288 self.mech_target_arm = pivot.appendLigament( 289 "target_position", 80, 0, 4, 290 wpilib.Color8Bit(wpilib.Color.kGreen) 291 ) 292 pivot.appendLigament( 293 "min_limit", 80, self.min_soft_limit, 2, 294 wpilib.Color8Bit(100, 100, 100) 295 ) 296 pivot.appendLigament( 297 "max_limit", 80, self.max_soft_limit, 2, 298 wpilib.Color8Bit(100, 100, 100) 299 ) 300 wpilib.SmartDashboard.putData( 301 self.getName() + "/mechanism", self.mech2d) 302 303 def periodic(self) -> None: 304 """ 305 Subsystem periodic method called every cycle by the command scheduler. 306 307 Manages turret state: runs calibration/homing routine if active, 308 drives PID to target position if set, or idles if disabled. 309 Publishes telemetry to SmartDashboard under the subsystem name prefix. 310 311 Returns: 312 None 313 """ 314 if self.calibration.is_busy: 315 self.calibration.periodic() 316 elif self._target_position is not None: 317 position = self.encoder.getPosition() 318 pidOutput = self.controller.calculate( 319 position, self._target_position) 320 pidOutput = max(self._min_output_voltage, 321 min(self._max_output_voltage, pidOutput)) 322 if self.controller.atSetpoint(): 323 self.motor.setVoltage(0) 324 else: 325 self.motor.setVoltage(pidOutput) 326 327 self.updateTelemetry() 328 329 def updateTelemetry(self) -> None: 330 """ 331 Publish telemetry to SmartDashboard and read back tunable 332 parameters (PID gains and voltage limits) for live tuning. 333 """ 334 prefix = self.getName() + "/" 335 sd = wpilib.SmartDashboard 336 sd.putNumber(prefix + "position", self.encoder.getPosition()) 337 sd.putNumber(prefix + "velocity", self.encoder.getVelocity()) 338 sd.putNumber( 339 prefix + "appliedOutput", self.motor.getAppliedOutput() 340 ) 341 sd.putNumber(prefix + "current", self.motor.getOutputCurrent()) 342 sd.putNumber( 343 prefix + "busVoltage", self.motor.getBusVoltage() 344 ) 345 sd.putNumber( 346 prefix + "temperature", self.motor.getMotorTemperature() 347 ) 348 target = self._target_position if self._target_position is not None else 0.0 349 sd.putNumber(prefix + "targetPosition", target) 350 sd.putBoolean( 351 prefix + "atTargetPosition", 352 self._target_position is not None 353 ) 354 # Soft limits 355 sl = self.motor.configAccessor.softLimit 356 sd.putNumber(prefix + "minSoftLimit", sl.getReverseSoftLimit()) 357 sd.putNumber(prefix + "maxSoftLimit", sl.getForwardSoftLimit()) 358 # Limit switches 359 sd.putBoolean( 360 prefix + "forwardLimitHit", 361 self.motor.getForwardLimitSwitch().get() 362 ) 363 sd.putBoolean( 364 prefix + "reverseLimitHit", 365 self.motor.getReverseLimitSwitch().get() 366 ) 367 # Voltage output limits (read back from dashboard) 368 self._min_output_voltage = sd.getNumber( 369 prefix + "pid/minOutputVoltage", self._min_output_voltage) 370 self._max_output_voltage = sd.getNumber( 371 prefix + "pid/maxOutputVoltage", self._max_output_voltage) 372 # Calibration telemetry (delegated) 373 self.calibration.update_telemetry(prefix) 374 # Mechanism2d visualization 375 self.mech_current_arm.setAngle(self.encoder.getPosition()) 376 self.mech_target_arm.setAngle( 377 self._target_position if self._target_position is not None 378 else 0.0 379 ) 380 381 def sysIdLog(self, sys_id_routine: SysIdRoutineLog) -> None: 382 """ 383 Log a frame of data for the turret motor for SysId characterization. 384 385 Records angular position, angular velocity, applied voltage, current, 386 motor temperature, and bus voltage. Converts from degrees to radians 387 for SysId which expects SI units. 388 389 Args: 390 sys_id_routine: the SysIdRoutineLog to record data into 391 392 Returns: 393 None 394 """ 395 motor_log = sys_id_routine.motor("turret") 396 397 # Encoder reports in degrees due to conversion factor; 398 # SysId expects radians 399 angular_position = math.radians(self.encoder.getPosition()) 400 angular_velocity = math.radians(self.encoder.getVelocity()) 401 402 current = self.motor.getOutputCurrent() 403 battery_voltage = self.motor.getBusVoltage() 404 motor_temp = self.motor.getMotorTemperature() 405 applied_voltage = self.motor.getAppliedOutput() * battery_voltage 406 407 motor_log.angularPosition(angular_position) 408 motor_log.angularVelocity(angular_velocity) 409 motor_log.current(current) 410 motor_log.voltage(applied_voltage) 411 motor_log.value("temperature", motor_temp, "C") 412 motor_log.value("busVoltage", battery_voltage, "V") 413 414 def sysIdQuasistaticCommand( 415 self, 416 direction: SysIdRoutine.Direction, 417 sysIdRoutine: SysIdRoutine 418 ) -> Command: 419 """ 420 Create a quasistatic SysId command for the turret. 421 422 Args: 423 direction: the direction to run the quasistatic test 424 sysIdRoutine: the SysIdRoutine instance to use 425 426 Returns: 427 A Command that runs the quasistatic test 428 """ 429 return sysIdRoutine.quasistatic(direction) 430 431 def sysIdDynamicCommand( 432 self, 433 direction: SysIdRoutine.Direction, 434 sysIdRoutine: SysIdRoutine 435 ) -> Command: 436 """ 437 Create a dynamic SysId command for the turret. 438 439 Args: 440 direction: the direction to run the dynamic test 441 sysIdRoutine: the SysIdRoutine instance to use 442 443 Returns: 444 A Command that runs the dynamic test 445 """ 446 return sysIdRoutine.dynamic(direction)
class
Turret(commands2.subsystem.Subsystem):
19class Turret(Subsystem): 20 """ 21 Subsystem for a single-motor turret with position control and soft limits. 22 23 The turret uses a SparkMax motor controller to rotate within defined 24 angular boundaries. Position is measured in degrees via the internal 25 relative encoder with a configurable conversion factor. 26 27 Calibration and homing are delegated to self.calibration 28 (PositionCalibration), a reusable controller that discovers mechanical 29 limits and persists them across reboots. 30 """ 31 32 def __init__( 33 self, 34 motor: rev.SparkMax, 35 position_conversion_factor: float, 36 min_soft_limit: float, 37 max_soft_limit: float 38 ) -> None: 39 """ 40 Creates a new Turret subsystem. 41 42 Args: 43 motor: the SparkMax motor controller driving the turret 44 position_conversion_factor: encoder conversion factor that converts 45 raw encoder rotations to degrees 46 min_soft_limit: the minimum turret position in degrees (reverse limit) 47 max_soft_limit: the maximum turret position in degrees (forward limit) 48 49 Returns: 50 None: class initialization executed upon construction 51 """ 52 super().__init__() 53 self.motor = motor 54 self.encoder = self.motor.getEncoder() 55 self.controller = PIDController(0, 0, 0) 56 wpilib.SmartDashboard.putData( 57 self.getName() + "/pid", self.controller) 58 59 self.min_soft_limit = min_soft_limit 60 self.max_soft_limit = max_soft_limit 61 62 # Voltage output limits 63 self._min_output_voltage = -12.0 64 self._max_output_voltage = 12.0 65 66 # Position tracking state 67 self._target_position = None 68 69 # Calibration controller (callbacks set up in setupCalibration) 70 self.calibration = self.setupCalibration( 71 min_soft_limit, max_soft_limit) 72 73 self.configureMechanism2d() 74 75 config = rev.SparkMaxConfig() 76 77 # General motor settings 78 ( 79 config 80 .setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake) 81 .voltageCompensation(12.0) 82 .smartCurrentLimit(40) 83 ) 84 85 # Encoder conversion factors 86 # Position: rotations -> degrees 87 # Velocity: RPM -> degrees/second (divide by 60) 88 velocity_conversion_factor = position_conversion_factor / 60.0 89 ( 90 config.encoder 91 .positionConversionFactor(position_conversion_factor) 92 .velocityConversionFactor(velocity_conversion_factor) 93 ) 94 95 # Soft limits in degrees 96 ( 97 config.softLimit 98 .forwardSoftLimit(max_soft_limit) 99 .forwardSoftLimitEnabled(True) 100 .reverseSoftLimit(min_soft_limit) 101 .reverseSoftLimitEnabled(True) 102 ) 103 104 # Telemetry signals at 20ms 105 GetSparkSignalsPositionControlConfig(config.signals, 20) 106 107 # Apply configuration 108 self.motor.configure( 109 config, 110 rev.ResetMode.kResetSafeParameters, 111 rev.PersistMode.kNoPersistParameters 112 ) 113 114 def setupCalibration( 115 self, 116 min_soft_limit: float, 117 max_soft_limit: float, 118 ) -> PositionCalibration: 119 """ 120 Create and configure the calibration controller for this turret. 121 122 Creates a PositionCalibration with no callbacks, then sets them 123 up using set_callbacks(). This keeps __init__ clean and puts all 124 the calibration wiring in one place. 125 126 Each callback is a small function that tells PositionCalibration 127 how to talk to the motor and encoder. If your mechanism has no 128 limit switches, just leave those callbacks out and homing will 129 use stall detection instead. 130 131 Args: 132 min_soft_limit: the minimum turret position in degrees 133 max_soft_limit: the maximum turret position in degrees 134 135 Returns: 136 The configured PositionCalibration controller 137 """ 138 # Step 1: Create calibration with no callbacks 139 cal = PositionCalibration( 140 name=self.getName(), 141 fallback_min=min_soft_limit, 142 fallback_max=max_soft_limit, 143 ) 144 145 # Step 2: Build callbacks from the SparkMax motor and encoder. 146 # SparkMaxCallbacks generates all the callbacks for you. 147 # You can also write your own lambdas instead, for example: 148 # set_motor_output=lambda pct: self.motor.set(pct), 149 # stop_motor=lambda: self.motor.stopMotor(), 150 spark_cbs = SparkMaxCallbacks(self.motor, self.encoder).as_dict() 151 152 # Step 3: Set callbacks on the calibration controller. 153 # Each callback tells PositionCalibration how to interact 154 # with the hardware. See PositionCalibration docs for the 155 # full list of available callbacks. 156 cal.set_callbacks( 157 # --- Core required (must have all three) --- 158 # Drive the motor at a duty cycle (-1.0 to 1.0) 159 set_motor_output=spark_cbs['set_motor_output'], 160 # Stop the motor 161 stop_motor=spark_cbs['stop_motor'], 162 # Reset the encoder position to a value 163 set_position=spark_cbs['set_position'], 164 165 # --- Detection (need at least one per homing direction) --- 166 # Read encoder velocity for stall detection 167 get_velocity=spark_cbs['get_velocity'], 168 # Read limit switches (leave out if not installed) 169 get_forward_limit_switch=( 170 spark_cbs['get_forward_limit_switch']), 171 get_reverse_limit_switch=( 172 spark_cbs['get_reverse_limit_switch']), 173 174 # --- Optional (make homing safer but not required) --- 175 # Read encoder position (needed for calibration phase 2) 176 get_position=spark_cbs['get_position'], 177 # Lower current limit during homing to protect the motor 178 set_current_limit=spark_cbs['set_current_limit'], 179 # Apply soft limits to the motor controller 180 set_soft_limits=spark_cbs['set_soft_limits'], 181 # Disable soft limits so the motor can travel freely 182 disable_soft_limits=spark_cbs['disable_soft_limits'], 183 # Save motor config before homing, restore it after 184 save_config=spark_cbs['save_config'], 185 restore_config=spark_cbs['restore_config'], 186 ) 187 188 # ---- Shortcut: motor= does the same thing in one line ---- 189 # If you don't need to customize anything, you can replace 190 # all of the above with: 191 # 192 # cal = PositionCalibration( 193 # name=self.getName(), 194 # fallback_min=min_soft_limit, 195 # fallback_max=max_soft_limit, 196 # motor=self.motor, 197 # encoder=self.encoder, 198 # ) 199 200 return cal 201 202 def setMotorVoltage(self, output: float) -> None: 203 """ 204 Set the motor voltage directly. Used by SysId routines to drive 205 the turret at controlled voltages for characterization. 206 Blocked during homing/calibration for safety. 207 208 Args: 209 output: voltage to apply to the motor 210 211 Returns: 212 None 213 """ 214 if self.calibration.is_busy: 215 return 216 self.motor.setVoltage(output) 217 218 def setPosition(self, position_degrees: float) -> None: 219 """ 220 Set the target turret position. The periodic method drives the PID 221 controller to this position each cycle. Blocked during homing and 222 calibration for safety. 223 224 Args: 225 position_degrees: the target turret position in degrees 226 227 Returns: 228 None 229 """ 230 if self.calibration.is_busy: 231 return 232 self._target_position = max( 233 self.calibration.min_soft_limit, 234 min(self.calibration.max_soft_limit, position_degrees) 235 ) 236 237 def getPosition(self) -> float: 238 """ 239 Get the current turret position in degrees. 240 241 Returns: 242 The current position of the turret in degrees 243 """ 244 return self.encoder.getPosition() 245 246 def getVelocity(self) -> float: 247 """ 248 Get the current turret angular velocity in degrees per second. 249 250 Returns: 251 The current angular velocity of the turret in degrees per second 252 """ 253 return self.encoder.getVelocity() 254 255 @property 256 def is_homing(self) -> bool: 257 """Whether the turret is currently in a homing routine.""" 258 return self.calibration.is_homing 259 260 def turretDisable(self) -> None: 261 """ 262 Disable turret motor output and clear the target position. 263 The turret will idle until a new position is set. 264 265 Returns: 266 None 267 """ 268 self._target_position = None 269 self.motor.stopMotor() 270 271 def configureMechanism2d(self) -> None: 272 """ 273 Create and publish a Mechanism2d widget for turret visualization. 274 275 Sets up a 2D canvas with: 276 - Red arm: current encoder position 277 - Green arm: target setpoint position 278 - Gray arms: static soft limit indicators 279 280 Returns: 281 None 282 """ 283 self.mech2d = wpilib.Mechanism2d(200, 200) 284 pivot = self.mech2d.getRoot("turret_pivot", 100, 100) 285 self.mech_current_arm = pivot.appendLigament( 286 "current_position", 80, 0, 6, 287 wpilib.Color8Bit(wpilib.Color.kRed) 288 ) 289 self.mech_target_arm = pivot.appendLigament( 290 "target_position", 80, 0, 4, 291 wpilib.Color8Bit(wpilib.Color.kGreen) 292 ) 293 pivot.appendLigament( 294 "min_limit", 80, self.min_soft_limit, 2, 295 wpilib.Color8Bit(100, 100, 100) 296 ) 297 pivot.appendLigament( 298 "max_limit", 80, self.max_soft_limit, 2, 299 wpilib.Color8Bit(100, 100, 100) 300 ) 301 wpilib.SmartDashboard.putData( 302 self.getName() + "/mechanism", self.mech2d) 303 304 def periodic(self) -> None: 305 """ 306 Subsystem periodic method called every cycle by the command scheduler. 307 308 Manages turret state: runs calibration/homing routine if active, 309 drives PID to target position if set, or idles if disabled. 310 Publishes telemetry to SmartDashboard under the subsystem name prefix. 311 312 Returns: 313 None 314 """ 315 if self.calibration.is_busy: 316 self.calibration.periodic() 317 elif self._target_position is not None: 318 position = self.encoder.getPosition() 319 pidOutput = self.controller.calculate( 320 position, self._target_position) 321 pidOutput = max(self._min_output_voltage, 322 min(self._max_output_voltage, pidOutput)) 323 if self.controller.atSetpoint(): 324 self.motor.setVoltage(0) 325 else: 326 self.motor.setVoltage(pidOutput) 327 328 self.updateTelemetry() 329 330 def updateTelemetry(self) -> None: 331 """ 332 Publish telemetry to SmartDashboard and read back tunable 333 parameters (PID gains and voltage limits) for live tuning. 334 """ 335 prefix = self.getName() + "/" 336 sd = wpilib.SmartDashboard 337 sd.putNumber(prefix + "position", self.encoder.getPosition()) 338 sd.putNumber(prefix + "velocity", self.encoder.getVelocity()) 339 sd.putNumber( 340 prefix + "appliedOutput", self.motor.getAppliedOutput() 341 ) 342 sd.putNumber(prefix + "current", self.motor.getOutputCurrent()) 343 sd.putNumber( 344 prefix + "busVoltage", self.motor.getBusVoltage() 345 ) 346 sd.putNumber( 347 prefix + "temperature", self.motor.getMotorTemperature() 348 ) 349 target = self._target_position if self._target_position is not None else 0.0 350 sd.putNumber(prefix + "targetPosition", target) 351 sd.putBoolean( 352 prefix + "atTargetPosition", 353 self._target_position is not None 354 ) 355 # Soft limits 356 sl = self.motor.configAccessor.softLimit 357 sd.putNumber(prefix + "minSoftLimit", sl.getReverseSoftLimit()) 358 sd.putNumber(prefix + "maxSoftLimit", sl.getForwardSoftLimit()) 359 # Limit switches 360 sd.putBoolean( 361 prefix + "forwardLimitHit", 362 self.motor.getForwardLimitSwitch().get() 363 ) 364 sd.putBoolean( 365 prefix + "reverseLimitHit", 366 self.motor.getReverseLimitSwitch().get() 367 ) 368 # Voltage output limits (read back from dashboard) 369 self._min_output_voltage = sd.getNumber( 370 prefix + "pid/minOutputVoltage", self._min_output_voltage) 371 self._max_output_voltage = sd.getNumber( 372 prefix + "pid/maxOutputVoltage", self._max_output_voltage) 373 # Calibration telemetry (delegated) 374 self.calibration.update_telemetry(prefix) 375 # Mechanism2d visualization 376 self.mech_current_arm.setAngle(self.encoder.getPosition()) 377 self.mech_target_arm.setAngle( 378 self._target_position if self._target_position is not None 379 else 0.0 380 ) 381 382 def sysIdLog(self, sys_id_routine: SysIdRoutineLog) -> None: 383 """ 384 Log a frame of data for the turret motor for SysId characterization. 385 386 Records angular position, angular velocity, applied voltage, current, 387 motor temperature, and bus voltage. Converts from degrees to radians 388 for SysId which expects SI units. 389 390 Args: 391 sys_id_routine: the SysIdRoutineLog to record data into 392 393 Returns: 394 None 395 """ 396 motor_log = sys_id_routine.motor("turret") 397 398 # Encoder reports in degrees due to conversion factor; 399 # SysId expects radians 400 angular_position = math.radians(self.encoder.getPosition()) 401 angular_velocity = math.radians(self.encoder.getVelocity()) 402 403 current = self.motor.getOutputCurrent() 404 battery_voltage = self.motor.getBusVoltage() 405 motor_temp = self.motor.getMotorTemperature() 406 applied_voltage = self.motor.getAppliedOutput() * battery_voltage 407 408 motor_log.angularPosition(angular_position) 409 motor_log.angularVelocity(angular_velocity) 410 motor_log.current(current) 411 motor_log.voltage(applied_voltage) 412 motor_log.value("temperature", motor_temp, "C") 413 motor_log.value("busVoltage", battery_voltage, "V") 414 415 def sysIdQuasistaticCommand( 416 self, 417 direction: SysIdRoutine.Direction, 418 sysIdRoutine: SysIdRoutine 419 ) -> Command: 420 """ 421 Create a quasistatic SysId command for the turret. 422 423 Args: 424 direction: the direction to run the quasistatic test 425 sysIdRoutine: the SysIdRoutine instance to use 426 427 Returns: 428 A Command that runs the quasistatic test 429 """ 430 return sysIdRoutine.quasistatic(direction) 431 432 def sysIdDynamicCommand( 433 self, 434 direction: SysIdRoutine.Direction, 435 sysIdRoutine: SysIdRoutine 436 ) -> Command: 437 """ 438 Create a dynamic SysId command for the turret. 439 440 Args: 441 direction: the direction to run the dynamic test 442 sysIdRoutine: the SysIdRoutine instance to use 443 444 Returns: 445 A Command that runs the dynamic test 446 """ 447 return sysIdRoutine.dynamic(direction)
Subsystem for a single-motor turret with position control and soft limits.
The turret uses a SparkMax motor controller to rotate within defined angular boundaries. Position is measured in degrees via the internal relative encoder with a configurable conversion factor.
Calibration and homing are delegated to self.calibration (PositionCalibration), a reusable controller that discovers mechanical limits and persists them across reboots.
Turret( motor: rev._rev.SparkMax, position_conversion_factor: float, min_soft_limit: float, max_soft_limit: float)
32 def __init__( 33 self, 34 motor: rev.SparkMax, 35 position_conversion_factor: float, 36 min_soft_limit: float, 37 max_soft_limit: float 38 ) -> None: 39 """ 40 Creates a new Turret subsystem. 41 42 Args: 43 motor: the SparkMax motor controller driving the turret 44 position_conversion_factor: encoder conversion factor that converts 45 raw encoder rotations to degrees 46 min_soft_limit: the minimum turret position in degrees (reverse limit) 47 max_soft_limit: the maximum turret position in degrees (forward limit) 48 49 Returns: 50 None: class initialization executed upon construction 51 """ 52 super().__init__() 53 self.motor = motor 54 self.encoder = self.motor.getEncoder() 55 self.controller = PIDController(0, 0, 0) 56 wpilib.SmartDashboard.putData( 57 self.getName() + "/pid", self.controller) 58 59 self.min_soft_limit = min_soft_limit 60 self.max_soft_limit = max_soft_limit 61 62 # Voltage output limits 63 self._min_output_voltage = -12.0 64 self._max_output_voltage = 12.0 65 66 # Position tracking state 67 self._target_position = None 68 69 # Calibration controller (callbacks set up in setupCalibration) 70 self.calibration = self.setupCalibration( 71 min_soft_limit, max_soft_limit) 72 73 self.configureMechanism2d() 74 75 config = rev.SparkMaxConfig() 76 77 # General motor settings 78 ( 79 config 80 .setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake) 81 .voltageCompensation(12.0) 82 .smartCurrentLimit(40) 83 ) 84 85 # Encoder conversion factors 86 # Position: rotations -> degrees 87 # Velocity: RPM -> degrees/second (divide by 60) 88 velocity_conversion_factor = position_conversion_factor / 60.0 89 ( 90 config.encoder 91 .positionConversionFactor(position_conversion_factor) 92 .velocityConversionFactor(velocity_conversion_factor) 93 ) 94 95 # Soft limits in degrees 96 ( 97 config.softLimit 98 .forwardSoftLimit(max_soft_limit) 99 .forwardSoftLimitEnabled(True) 100 .reverseSoftLimit(min_soft_limit) 101 .reverseSoftLimitEnabled(True) 102 ) 103 104 # Telemetry signals at 20ms 105 GetSparkSignalsPositionControlConfig(config.signals, 20) 106 107 # Apply configuration 108 self.motor.configure( 109 config, 110 rev.ResetMode.kResetSafeParameters, 111 rev.PersistMode.kNoPersistParameters 112 )
Creates a new Turret subsystem.
Args: motor: the SparkMax motor controller driving the turret position_conversion_factor: encoder conversion factor that converts raw encoder rotations to degrees min_soft_limit: the minimum turret position in degrees (reverse limit) max_soft_limit: the maximum turret position in degrees (forward limit)
Returns: None: class initialization executed upon construction
def
setupCalibration( self, min_soft_limit: float, max_soft_limit: float) -> utils.position_calibration.PositionCalibration:
114 def setupCalibration( 115 self, 116 min_soft_limit: float, 117 max_soft_limit: float, 118 ) -> PositionCalibration: 119 """ 120 Create and configure the calibration controller for this turret. 121 122 Creates a PositionCalibration with no callbacks, then sets them 123 up using set_callbacks(). This keeps __init__ clean and puts all 124 the calibration wiring in one place. 125 126 Each callback is a small function that tells PositionCalibration 127 how to talk to the motor and encoder. If your mechanism has no 128 limit switches, just leave those callbacks out and homing will 129 use stall detection instead. 130 131 Args: 132 min_soft_limit: the minimum turret position in degrees 133 max_soft_limit: the maximum turret position in degrees 134 135 Returns: 136 The configured PositionCalibration controller 137 """ 138 # Step 1: Create calibration with no callbacks 139 cal = PositionCalibration( 140 name=self.getName(), 141 fallback_min=min_soft_limit, 142 fallback_max=max_soft_limit, 143 ) 144 145 # Step 2: Build callbacks from the SparkMax motor and encoder. 146 # SparkMaxCallbacks generates all the callbacks for you. 147 # You can also write your own lambdas instead, for example: 148 # set_motor_output=lambda pct: self.motor.set(pct), 149 # stop_motor=lambda: self.motor.stopMotor(), 150 spark_cbs = SparkMaxCallbacks(self.motor, self.encoder).as_dict() 151 152 # Step 3: Set callbacks on the calibration controller. 153 # Each callback tells PositionCalibration how to interact 154 # with the hardware. See PositionCalibration docs for the 155 # full list of available callbacks. 156 cal.set_callbacks( 157 # --- Core required (must have all three) --- 158 # Drive the motor at a duty cycle (-1.0 to 1.0) 159 set_motor_output=spark_cbs['set_motor_output'], 160 # Stop the motor 161 stop_motor=spark_cbs['stop_motor'], 162 # Reset the encoder position to a value 163 set_position=spark_cbs['set_position'], 164 165 # --- Detection (need at least one per homing direction) --- 166 # Read encoder velocity for stall detection 167 get_velocity=spark_cbs['get_velocity'], 168 # Read limit switches (leave out if not installed) 169 get_forward_limit_switch=( 170 spark_cbs['get_forward_limit_switch']), 171 get_reverse_limit_switch=( 172 spark_cbs['get_reverse_limit_switch']), 173 174 # --- Optional (make homing safer but not required) --- 175 # Read encoder position (needed for calibration phase 2) 176 get_position=spark_cbs['get_position'], 177 # Lower current limit during homing to protect the motor 178 set_current_limit=spark_cbs['set_current_limit'], 179 # Apply soft limits to the motor controller 180 set_soft_limits=spark_cbs['set_soft_limits'], 181 # Disable soft limits so the motor can travel freely 182 disable_soft_limits=spark_cbs['disable_soft_limits'], 183 # Save motor config before homing, restore it after 184 save_config=spark_cbs['save_config'], 185 restore_config=spark_cbs['restore_config'], 186 ) 187 188 # ---- Shortcut: motor= does the same thing in one line ---- 189 # If you don't need to customize anything, you can replace 190 # all of the above with: 191 # 192 # cal = PositionCalibration( 193 # name=self.getName(), 194 # fallback_min=min_soft_limit, 195 # fallback_max=max_soft_limit, 196 # motor=self.motor, 197 # encoder=self.encoder, 198 # ) 199 200 return cal
Create and configure the calibration controller for this turret.
Creates a PositionCalibration with no callbacks, then sets them up using set_callbacks(). This keeps __init__ clean and puts all the calibration wiring in one place.
Each callback is a small function that tells PositionCalibration how to talk to the motor and encoder. If your mechanism has no limit switches, just leave those callbacks out and homing will use stall detection instead.
Args: min_soft_limit: the minimum turret position in degrees max_soft_limit: the maximum turret position in degrees
Returns: The configured PositionCalibration controller
def
setMotorVoltage(self, output: float) -> None:
202 def setMotorVoltage(self, output: float) -> None: 203 """ 204 Set the motor voltage directly. Used by SysId routines to drive 205 the turret at controlled voltages for characterization. 206 Blocked during homing/calibration for safety. 207 208 Args: 209 output: voltage to apply to the motor 210 211 Returns: 212 None 213 """ 214 if self.calibration.is_busy: 215 return 216 self.motor.setVoltage(output)
Set the motor voltage directly. Used by SysId routines to drive the turret at controlled voltages for characterization. Blocked during homing/calibration for safety.
Args: output: voltage to apply to the motor
Returns: None
def
setPosition(self, position_degrees: float) -> None:
218 def setPosition(self, position_degrees: float) -> None: 219 """ 220 Set the target turret position. The periodic method drives the PID 221 controller to this position each cycle. Blocked during homing and 222 calibration for safety. 223 224 Args: 225 position_degrees: the target turret position in degrees 226 227 Returns: 228 None 229 """ 230 if self.calibration.is_busy: 231 return 232 self._target_position = max( 233 self.calibration.min_soft_limit, 234 min(self.calibration.max_soft_limit, position_degrees) 235 )
Set the target turret position. The periodic method drives the PID controller to this position each cycle. Blocked during homing and calibration for safety.
Args: position_degrees: the target turret position in degrees
Returns: None
def
getPosition(self) -> float:
237 def getPosition(self) -> float: 238 """ 239 Get the current turret position in degrees. 240 241 Returns: 242 The current position of the turret in degrees 243 """ 244 return self.encoder.getPosition()
Get the current turret position in degrees.
Returns: The current position of the turret in degrees
def
getVelocity(self) -> float:
246 def getVelocity(self) -> float: 247 """ 248 Get the current turret angular velocity in degrees per second. 249 250 Returns: 251 The current angular velocity of the turret in degrees per second 252 """ 253 return self.encoder.getVelocity()
Get the current turret angular velocity in degrees per second.
Returns: The current angular velocity of the turret in degrees per second
is_homing: bool
255 @property 256 def is_homing(self) -> bool: 257 """Whether the turret is currently in a homing routine.""" 258 return self.calibration.is_homing
Whether the turret is currently in a homing routine.
def
turretDisable(self) -> None:
260 def turretDisable(self) -> None: 261 """ 262 Disable turret motor output and clear the target position. 263 The turret will idle until a new position is set. 264 265 Returns: 266 None 267 """ 268 self._target_position = None 269 self.motor.stopMotor()
Disable turret motor output and clear the target position. The turret will idle until a new position is set.
Returns: None
def
configureMechanism2d(self) -> None:
271 def configureMechanism2d(self) -> None: 272 """ 273 Create and publish a Mechanism2d widget for turret visualization. 274 275 Sets up a 2D canvas with: 276 - Red arm: current encoder position 277 - Green arm: target setpoint position 278 - Gray arms: static soft limit indicators 279 280 Returns: 281 None 282 """ 283 self.mech2d = wpilib.Mechanism2d(200, 200) 284 pivot = self.mech2d.getRoot("turret_pivot", 100, 100) 285 self.mech_current_arm = pivot.appendLigament( 286 "current_position", 80, 0, 6, 287 wpilib.Color8Bit(wpilib.Color.kRed) 288 ) 289 self.mech_target_arm = pivot.appendLigament( 290 "target_position", 80, 0, 4, 291 wpilib.Color8Bit(wpilib.Color.kGreen) 292 ) 293 pivot.appendLigament( 294 "min_limit", 80, self.min_soft_limit, 2, 295 wpilib.Color8Bit(100, 100, 100) 296 ) 297 pivot.appendLigament( 298 "max_limit", 80, self.max_soft_limit, 2, 299 wpilib.Color8Bit(100, 100, 100) 300 ) 301 wpilib.SmartDashboard.putData( 302 self.getName() + "/mechanism", self.mech2d)
Create and publish a Mechanism2d widget for turret visualization.
Sets up a 2D canvas with:
- Red arm: current encoder position
- Green arm: target setpoint position
- Gray arms: static soft limit indicators
Returns: None
def
periodic(self) -> None:
304 def periodic(self) -> None: 305 """ 306 Subsystem periodic method called every cycle by the command scheduler. 307 308 Manages turret state: runs calibration/homing routine if active, 309 drives PID to target position if set, or idles if disabled. 310 Publishes telemetry to SmartDashboard under the subsystem name prefix. 311 312 Returns: 313 None 314 """ 315 if self.calibration.is_busy: 316 self.calibration.periodic() 317 elif self._target_position is not None: 318 position = self.encoder.getPosition() 319 pidOutput = self.controller.calculate( 320 position, self._target_position) 321 pidOutput = max(self._min_output_voltage, 322 min(self._max_output_voltage, pidOutput)) 323 if self.controller.atSetpoint(): 324 self.motor.setVoltage(0) 325 else: 326 self.motor.setVoltage(pidOutput) 327 328 self.updateTelemetry()
Subsystem periodic method called every cycle by the command scheduler.
Manages turret state: runs calibration/homing routine if active, drives PID to target position if set, or idles if disabled. Publishes telemetry to SmartDashboard under the subsystem name prefix.
Returns: None
def
updateTelemetry(self) -> None:
330 def updateTelemetry(self) -> None: 331 """ 332 Publish telemetry to SmartDashboard and read back tunable 333 parameters (PID gains and voltage limits) for live tuning. 334 """ 335 prefix = self.getName() + "/" 336 sd = wpilib.SmartDashboard 337 sd.putNumber(prefix + "position", self.encoder.getPosition()) 338 sd.putNumber(prefix + "velocity", self.encoder.getVelocity()) 339 sd.putNumber( 340 prefix + "appliedOutput", self.motor.getAppliedOutput() 341 ) 342 sd.putNumber(prefix + "current", self.motor.getOutputCurrent()) 343 sd.putNumber( 344 prefix + "busVoltage", self.motor.getBusVoltage() 345 ) 346 sd.putNumber( 347 prefix + "temperature", self.motor.getMotorTemperature() 348 ) 349 target = self._target_position if self._target_position is not None else 0.0 350 sd.putNumber(prefix + "targetPosition", target) 351 sd.putBoolean( 352 prefix + "atTargetPosition", 353 self._target_position is not None 354 ) 355 # Soft limits 356 sl = self.motor.configAccessor.softLimit 357 sd.putNumber(prefix + "minSoftLimit", sl.getReverseSoftLimit()) 358 sd.putNumber(prefix + "maxSoftLimit", sl.getForwardSoftLimit()) 359 # Limit switches 360 sd.putBoolean( 361 prefix + "forwardLimitHit", 362 self.motor.getForwardLimitSwitch().get() 363 ) 364 sd.putBoolean( 365 prefix + "reverseLimitHit", 366 self.motor.getReverseLimitSwitch().get() 367 ) 368 # Voltage output limits (read back from dashboard) 369 self._min_output_voltage = sd.getNumber( 370 prefix + "pid/minOutputVoltage", self._min_output_voltage) 371 self._max_output_voltage = sd.getNumber( 372 prefix + "pid/maxOutputVoltage", self._max_output_voltage) 373 # Calibration telemetry (delegated) 374 self.calibration.update_telemetry(prefix) 375 # Mechanism2d visualization 376 self.mech_current_arm.setAngle(self.encoder.getPosition()) 377 self.mech_target_arm.setAngle( 378 self._target_position if self._target_position is not None 379 else 0.0 380 )
Publish telemetry to SmartDashboard and read back tunable parameters (PID gains and voltage limits) for live tuning.
def
sysIdLog(self, sys_id_routine: wpilib._wpilib.sysid.SysIdRoutineLog) -> None:
382 def sysIdLog(self, sys_id_routine: SysIdRoutineLog) -> None: 383 """ 384 Log a frame of data for the turret motor for SysId characterization. 385 386 Records angular position, angular velocity, applied voltage, current, 387 motor temperature, and bus voltage. Converts from degrees to radians 388 for SysId which expects SI units. 389 390 Args: 391 sys_id_routine: the SysIdRoutineLog to record data into 392 393 Returns: 394 None 395 """ 396 motor_log = sys_id_routine.motor("turret") 397 398 # Encoder reports in degrees due to conversion factor; 399 # SysId expects radians 400 angular_position = math.radians(self.encoder.getPosition()) 401 angular_velocity = math.radians(self.encoder.getVelocity()) 402 403 current = self.motor.getOutputCurrent() 404 battery_voltage = self.motor.getBusVoltage() 405 motor_temp = self.motor.getMotorTemperature() 406 applied_voltage = self.motor.getAppliedOutput() * battery_voltage 407 408 motor_log.angularPosition(angular_position) 409 motor_log.angularVelocity(angular_velocity) 410 motor_log.current(current) 411 motor_log.voltage(applied_voltage) 412 motor_log.value("temperature", motor_temp, "C") 413 motor_log.value("busVoltage", battery_voltage, "V")
Log a frame of data for the turret motor for SysId characterization.
Records angular position, angular velocity, applied voltage, current, motor temperature, and bus voltage. Converts from degrees to radians for SysId which expects SI units.
Args: sys_id_routine: the SysIdRoutineLog to record data into
Returns: None
def
sysIdQuasistaticCommand( self, direction: commands2.sysid.sysidroutine.SysIdRoutine.Direction, sysIdRoutine: commands2.sysid.sysidroutine.SysIdRoutine) -> commands2.command.Command:
415 def sysIdQuasistaticCommand( 416 self, 417 direction: SysIdRoutine.Direction, 418 sysIdRoutine: SysIdRoutine 419 ) -> Command: 420 """ 421 Create a quasistatic SysId command for the turret. 422 423 Args: 424 direction: the direction to run the quasistatic test 425 sysIdRoutine: the SysIdRoutine instance to use 426 427 Returns: 428 A Command that runs the quasistatic test 429 """ 430 return sysIdRoutine.quasistatic(direction)
Create a quasistatic SysId command for the turret.
Args: direction: the direction to run the quasistatic test sysIdRoutine: the SysIdRoutine instance to use
Returns: A Command that runs the quasistatic test
def
sysIdDynamicCommand( self, direction: commands2.sysid.sysidroutine.SysIdRoutine.Direction, sysIdRoutine: commands2.sysid.sysidroutine.SysIdRoutine) -> commands2.command.Command:
432 def sysIdDynamicCommand( 433 self, 434 direction: SysIdRoutine.Direction, 435 sysIdRoutine: SysIdRoutine 436 ) -> Command: 437 """ 438 Create a dynamic SysId command for the turret. 439 440 Args: 441 direction: the direction to run the dynamic test 442 sysIdRoutine: the SysIdRoutine instance to use 443 444 Returns: 445 A Command that runs the dynamic test 446 """ 447 return sysIdRoutine.dynamic(direction)
Create a dynamic SysId command for the turret.
Args: direction: the direction to run the dynamic test sysIdRoutine: the SysIdRoutine instance to use
Returns: A Command that runs the dynamic test