subsystem.mechanisms.shooter.hood

  1# Native imports
  2import math
  3
  4# Third-party imports
  5import rev
  6import wpilib
  7from typing import Callable
  8from commands2 import Command, Subsystem
  9import commands2.cmd
 10from commands2.sysid import SysIdRoutine
 11from ntcore.util import ntproperty
 12from wpilib.sysid import SysIdRoutineLog
 13from wpimath.controller import ArmFeedforward, PIDController
 14
 15# Internal imports
 16from utils.spark_utils import GetSparkSignalsPositionControlConfig
 17
 18
 19class Hood(Subsystem):
 20    """
 21    Subsystem for a hood angle adjustment mechanism.
 22
 23    Uses a single motor with position control to adjust the shooting angle.
 24    Position is measured in degrees via the internal relative encoder with
 25    a configurable conversion factor. Assumes 0 degrees on startup.
 26
 27    The feedforward uses ArmFeedforward to compensate for gravity —
 28    0 degrees is approximately horizontal, positive degrees lift
 29    toward vertical.
 30
 31    Safety interlock: when enabled (default on code load), the hood
 32    automatically stows if the injected shooter's RPM setpoint falls
 33    below a configurable threshold. Safety is always enabled on startup
 34    and must be explicitly disabled at runtime.
 35    """
 36
 37    # Telemetry via ntproperty (published to NT, not persisted)
 38    nt_position = ntproperty('/Hood/position', 0.0, writeDefault=True)
 39    nt_velocity = ntproperty('/Hood/velocity', 0.0, writeDefault=True)
 40    nt_target = ntproperty('/Hood/targetPosition', 0.0, writeDefault=True)
 41    nt_normalized_position = ntproperty(
 42        '/Hood/normalizedPosition', 0.0, writeDefault=True)
 43    nt_normalized_target = ntproperty(
 44        '/Hood/normalizedTarget', 0.0, writeDefault=True)
 45    nt_at_setpoint = ntproperty(
 46        '/Hood/atSetpoint', False, writeDefault=True)
 47    nt_applied_output = ntproperty(
 48        '/Hood/appliedOutput', 0.0, writeDefault=True)
 49    nt_current = ntproperty('/Hood/current', 0.0, writeDefault=True)
 50    nt_bus_voltage = ntproperty(
 51        '/Hood/busVoltage', 0.0, writeDefault=True)
 52    nt_min_soft_limit = ntproperty(
 53        '/Hood/minSoftLimit', 0.0, writeDefault=True)
 54    nt_max_soft_limit = ntproperty(
 55        '/Hood/maxSoftLimit', 0.0, writeDefault=True)
 56
 57    # Safety: persistent configuration
 58    nt_safety_enabled = ntproperty(
 59        '/Hood/safetyEnabled', True, writeDefault=True)
 60    nt_stowed_angle_degrees = ntproperty(
 61        '/Hood/stowedAngleDegrees', 0.0,
 62        writeDefault=False, persistent=True)
 63    nt_safety_rpm_threshold = ntproperty(
 64        '/Hood/safetyRpmThreshold', 5.0,
 65        writeDefault=False, persistent=True)
 66
 67    # Dashboard tuning: set ntControlEnabled=True on dashboard,
 68    # then adjust ntControlAngle to drive hood to a specific angle.
 69    nt_control_enabled = ntproperty(
 70        '/Hood/ntControlEnabled', False, writeDefault=True)
 71    nt_control_angle = ntproperty(
 72        '/Hood/ntControlAngle', 0.0, writeDefault=True)
 73
 74    def __init__(
 75        self,
 76        motor: rev.SparkMax,
 77        position_conversion_factor: float,
 78        max_angle_degrees: float,
 79        pid: tuple,
 80        feedforward: tuple,
 81        inverted: bool = False,
 82        min_angle_degrees: float = 0.0,
 83        horizontal_offset_degrees: float = 0.0,
 84    ) -> None:
 85        """
 86        Creates a new Hood subsystem.
 87
 88        Args:
 89            motor: the SparkMax motor controller driving the hood
 90            position_conversion_factor: encoder conversion factor that
 91                converts raw encoder rotations to degrees
 92            max_angle_degrees: the maximum hood angle in degrees
 93                (forward soft limit)
 94            pid: (P, I, D) gains for the WPILib PIDController
 95            feedforward: (kS, kG, kV, kA) gains for ArmFeedforward
 96            inverted: whether the motor direction is inverted
 97            min_angle_degrees: the minimum hood angle in degrees
 98                (reverse soft limit)
 99            horizontal_offset_degrees: offset from hood 0-position to
100                true horizontal for feedforward calculation
101        """
102        super().__init__()
103        self.motor = motor
104        self.encoder = self.motor.getEncoder()
105
106        self.max_angle_degrees = max_angle_degrees
107        self.min_angle_degrees = min_angle_degrees
108        self._horizontal_offset_degrees = horizontal_offset_degrees
109
110        # Control
111        self.controller = PIDController(*pid)
112        wpilib.SmartDashboard.putData(
113            self.getName() + "/pid", self.controller)
114        self.feedforward = ArmFeedforward(*feedforward)
115
116        # Voltage output limits
117        self._min_output_volts = -12.0
118        self._max_output_volts = 12.0
119
120        # Position tracking
121        self._target_degrees = 0.0
122        self._enabled = True
123
124        # Configure motor
125        config = rev.SparkMaxConfig()
126        (
127            config
128            .inverted(inverted)
129            .setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
130            .voltageCompensation(12.0)
131            .smartCurrentLimit(20)
132        )
133
134        # Encoder conversion factors
135        # position: rotations -> degrees (deg/rotation)
136        # velocity: RPM -> deg/s (divide by 60)
137        velocity_conversion_factor_dps = position_conversion_factor / 60.0
138        (
139            config.encoder
140            .positionConversionFactor(position_conversion_factor)
141            .velocityConversionFactor(velocity_conversion_factor_dps)
142        )
143
144        # Soft limits in degrees
145        (
146            config.softLimit
147            .forwardSoftLimit(max_angle_degrees)
148            .forwardSoftLimitEnabled(True)
149            .reverseSoftLimit(min_angle_degrees)
150            .reverseSoftLimitEnabled(True)
151        )
152
153        # Telemetry signals at 20ms
154        GetSparkSignalsPositionControlConfig(config.signals, 20)
155
156        # Apply configuration
157        self.motor.configure(
158            config,
159            rev.ResetMode.kResetSafeParameters,
160            rev.PersistMode.kNoPersistParameters
161        )
162
163        # Assume 0 position on startup
164        self.encoder.setPosition(0)
165
166        # Shooter dependency (optional, injected via setShooter)
167        self._shooter = None
168
169        # Mechanism2d visualization
170        self._configureMechanism2d()
171
172    # -- Setpoint methods --
173
174    def setAngleDegrees(self, degrees: float) -> None:
175        """Set hood target angle in degrees, clamped to valid range."""
176        if not self._enabled:
177            self.enable()
178        self._target_degrees = max(
179            self.min_angle_degrees,
180            min(self.max_angle_degrees, degrees)
181        )
182
183    def setAngleNormalized(self, value: float) -> None:
184        """Set hood target from 0..1 range (0 = min, 1 = max angle)."""
185        value = max(0.0, min(1.0, value))
186        self._target_degrees = value * self.max_angle_degrees
187
188    def setAngleRadians(self, radians: float) -> None:
189        """Set hood target angle in radians, converted to degrees."""
190        self.setAngleDegrees(math.degrees(radians))
191
192    # -- Shooter injection --
193
194    def setShooter(self, shooter) -> None:
195        """Inject a shooter subsystem for safety interlock.
196
197        The shooter must have an `RPM` attribute representing
198        the current setpoint in RPM.
199        """
200        self._shooter = shooter
201
202    def getShooter(self):
203        """Return the injected shooter, or None."""
204        return self._shooter
205
206    # -- Safety --
207
208    def setSafetyEnabled(self, enabled: bool) -> None:
209        """Enable or disable the shooter-based safety interlock."""
210        self.nt_safety_enabled = enabled
211
212    def getSafetyEnabled(self) -> bool:
213        """Return whether the safety interlock is enabled."""
214        return self.nt_safety_enabled
215
216    # -- Getter methods --
217
218    def getAngleDegrees(self) -> float:
219        """Get current hood angle in degrees from encoder."""
220        return self.encoder.getPosition()
221
222    def getAngleNormalized(self) -> float:
223        """Get current hood angle as 0..1 normalized value."""
224        return self.encoder.getPosition() / self.max_angle_degrees
225
226    def getAngleRadians(self) -> float:
227        """Get current hood angle in radians."""
228        return math.radians(self.encoder.getPosition())
229
230    def getTargetDegrees(self) -> float:
231        """Get the current target angle in degrees."""
232        return self._target_degrees
233
234    def atSetpoint(self) -> bool:
235        """Return whether the hood is at its target setpoint."""
236        return self.controller.atSetpoint()
237
238    # -- Lifecycle --
239
240    def onDisabledInit(self) -> None:
241        """Stop the motor when robot is disabled."""
242        self.motor.stopMotor()
243
244    def enable(self) -> None:
245        """Re-enable periodic PID control."""
246        self.controller.reset()
247        self._target_degrees = self.getAngleDegrees()
248        self._enabled = True
249
250    def disable(self) -> None:
251        """Stop motor output and suspend periodic PID control."""
252        self._enabled = False
253        self._target_degrees = self.getAngleDegrees()
254        self.controller.reset()
255        self.motor.stopMotor()
256
257    # -- Periodic --
258
259    def periodic(self) -> None:
260        """
261        Drive PID + feedforward to target, update viz and telemetry.
262        """
263        if not self._enabled:
264            self._updateTelemetry()
265            return
266
267        # Dashboard tuning override
268        if self.nt_control_enabled:
269            self._target_degrees = max(
270                self.min_angle_degrees,
271                min(self.max_angle_degrees, self.nt_control_angle)
272            )
273
274        # Safety interlock: stow hood when shooter setpoint is low
275        if (self.nt_safety_enabled
276                and self._shooter is not None
277                and self._shooter.RPM < self.nt_safety_rpm_threshold):
278            self._target_degrees = self.nt_stowed_angle_degrees
279
280        position_deg = self.encoder.getPosition()
281
282        # PID output (volts)
283        pid_volts = self.controller.calculate(
284            position_deg, self._target_degrees)
285
286        # ArmFeedforward expects angle in radians from horizontal
287        ff_angle_rad = math.radians(
288            position_deg + self._horizontal_offset_degrees)
289        ff_volts = self.feedforward.calculate(ff_angle_rad, 0)
290
291        # Combine and clamp voltage
292        total_volts = pid_volts + ff_volts
293        total_volts = max(self._min_output_volts,
294                          min(self._max_output_volts, total_volts))
295
296        if self.controller.atSetpoint():
297            # Still apply feedforward to hold against gravity
298            self.motor.setVoltage(ff_volts)
299        else:
300            self.motor.setVoltage(total_volts)
301
302        self._updateTelemetry()
303
304    # -- Mechanism2d --
305
306    def _configureMechanism2d(self) -> None:
307        """
308        Create and publish a Mechanism2d widget for hood visualization.
309
310        Sets up a 2D canvas with:
311        - Red arm: current encoder position
312        - Green arm: target setpoint position
313        - Gray arms: static soft limit indicators
314        """
315        self.mech2d = wpilib.Mechanism2d(200, 200)
316        pivot = self.mech2d.getRoot("hood_pivot", 100, 100)
317        self.mech_current_arm = pivot.appendLigament(
318            "current_position", 80, 0, 6,
319            wpilib.Color8Bit(wpilib.Color.kRed)
320        )
321        self.mech_target_arm = pivot.appendLigament(
322            "target_position", 80, 0, 4,
323            wpilib.Color8Bit(wpilib.Color.kGreen)
324        )
325        pivot.appendLigament(
326            "min_limit", 80, self.min_angle_degrees, 2,
327            wpilib.Color8Bit(100, 100, 100)
328        )
329        pivot.appendLigament(
330            "max_limit", 80, self.max_angle_degrees, 2,
331            wpilib.Color8Bit(100, 100, 100)
332        )
333        wpilib.SmartDashboard.putData(
334            self.getName() + "/mechanism", self.mech2d)
335
336    # -- Telemetry --
337
338    def _updateTelemetry(self) -> None:
339        """Publish telemetry via ntproperty and update Mechanism2d."""
340        position_deg = self.encoder.getPosition()
341        velocity_dps = self.encoder.getVelocity()
342
343        self.nt_position = position_deg
344        self.nt_velocity = velocity_dps
345        self.nt_target = self._target_degrees
346        self.nt_normalized_position = position_deg / self.max_angle_degrees
347        self.nt_normalized_target = (
348            self._target_degrees / self.max_angle_degrees)
349        self.nt_at_setpoint = self.atSetpoint()
350        self.nt_applied_output = self.motor.getAppliedOutput()
351        self.nt_current = self.motor.getOutputCurrent()
352        self.nt_bus_voltage = self.motor.getBusVoltage()
353
354        # Soft limits
355        sl = self.motor.configAccessor.softLimit
356        self.nt_min_soft_limit = sl.getReverseSoftLimit()
357        self.nt_max_soft_limit = sl.getForwardSoftLimit()
358
359        # Update mechanism2d arms
360        self.mech_current_arm.setAngle(position_deg)
361        self.mech_target_arm.setAngle(self._target_degrees)
362
363    # -- SysId --
364
365    def _setMotorVoltage(self, volts: float) -> None:
366        """Set motor voltage directly. Used by SysId routines."""
367        self._commanded_volts = volts
368        self.motor.setVoltage(volts)
369
370    def _sysIdLog(self, sys_id_routine: SysIdRoutineLog) -> None:
371        """
372        Log a frame of data for SysId characterization.
373
374        Logs radians (SI units for SysId), plus degrees and 0..1
375        normalized range as custom fields for analysis.
376        """
377        motor_log = sys_id_routine.motor("hood")
378
379        position_deg = self.encoder.getPosition()
380        velocity_dps = self.encoder.getVelocity()
381        position_rad = math.radians(position_deg)
382        velocity_rps = math.radians(velocity_dps)
383
384        (
385            motor_log
386            .angularPosition(position_rad)
387            .angularVelocity(velocity_rps)
388            .current(self.motor.getOutputCurrent())
389            .voltage(getattr(self, '_commanded_volts', 0.0))
390            .value("positionDegrees", position_deg, "deg")
391            .value("velocityDegPerSec", velocity_dps, "deg/s")
392            .value("positionNormalized",
393                   position_deg / self.max_angle_degrees, "")
394        )
395
396    def _sysIdQuasistaticCommand(
397        self,
398        direction: SysIdRoutine.Direction,
399        sysIdRoutine: SysIdRoutine
400    ) -> Command:
401        """Create a quasistatic SysId command for the hood."""
402        return sysIdRoutine.quasistatic(direction)
403
404    def _sysIdDynamicCommand(
405        self,
406        direction: SysIdRoutine.Direction,
407        sysIdRoutine: SysIdRoutine
408    ) -> Command:
409        """Create a dynamic SysId command for the hood."""
410        return sysIdRoutine.dynamic(direction)
411
412    # -- Command generators --
413
414    def autoAngleCommand(self) -> Command:
415        """Command that sets hood angle from shooter's distance lookup."""
416        def _action():
417            if self._shooter is not None:
418                self.setAngleDegrees(
419                    self._shooter.getHoodAngleForDistance(
420                        self._shooter.targetDistance))
421
422        return commands2.cmd.run(_action, self)
423
424    def manualTestCommand(
425        self, analog_input: Callable[[], float]
426    ) -> Command:
427        """Test command: analog input controls hood angle directly.
428
429        Disables safety when input > 0.02, re-enables when released.
430        """
431        def _action():
432            trigger = analog_input()
433            self.setSafetyEnabled(trigger <= 0.02)
434            self.setAngleNormalized(trigger)
435
436        return commands2.cmd.run(_action, self)
437
438
439def createHood(constants, config) -> Hood:
440    """
441    Factory helper to create a Hood subsystem from config objects.
442
443    Args:
444        constants: HoodConstants (motorId, positionConversionFactor,
445            maxAngleDegrees, inverted)
446        config: HoodConfig (hoodPID, hoodFeedforward,
447            horizontalOffsetDegrees)
448
449    Returns:
450        A fully configured Hood subsystem
451    """
452    motor = rev.SparkMax(
453        constants.motorId,
454        rev.SparkLowLevel.MotorType.kBrushless
455    )
456    return Hood(
457        motor=motor,
458        position_conversion_factor=constants.positionConversionFactor,
459        max_angle_degrees=constants.maxAngleDegrees,
460        pid=config.hoodPID,
461        feedforward=config.hoodFeedforward,
462        inverted=constants.inverted,
463        min_angle_degrees=constants.minAngleDegrees,
464        horizontal_offset_degrees=config.horizontalOffsetDegrees,
465    )