Add hood control

components/shooter.py

@@ -1,11 +1,16 @@
-from magicbot import tunable, will_reset_to
+import math
+
+from magicbot import feedback, tunable, will_reset_to
 from phoenix5 import ControlMode, TalonSRX
 from phoenix6 import configs, controls
 from phoenix6.controls import Follower
 from phoenix6.hardware import TalonFX
 from phoenix6.signals import MotorAlignmentValue
+from rev import ClosedLoopSlot, FeedbackSensor, SparkMax, SparkMaxConfig
 
-from ids import TalonId
+from ids import SparkId, TalonId
+from utilities.functions import clamp
+from utilities.rev import configure_spark_reset_and_persist
 
 
 class ShooterComponent:
@@ -15,17 +20,23 @@ class ShooterComponent:
     target_feeder_percentage = will_reset_to(0)
     desired_feeder_percentage = tunable(1)
 
+    desired_hood_angle = tunable(36.0)
+    hood_error_tolerance = 3.0
+    MAX_HOOD_ANGLE = 59
+    MIN_HOOD_ANGLE = 18
+
+    ENCODER_ROTS_PER_HOOD_DEGREE = 1 / (54 / 26 / 360)
+    ENCODER_ZERO_OFFSET = 0.7775692
+
     def __init__(self) -> None:
         self.flywheel_motor_left = TalonFX(
             device_id=TalonId.FLYWHEEL_LEFT
         )  # Defined from behind shooter
         self.flywheel_motor_right = TalonFX(
             device_id=TalonId.FLYWHEEL_RIGHT
         )  # Defined from behind shooter
-        self.feeder_motor = TalonSRX(TalonId.FEEDER)
-        self.feeder_motor.setInverted(False)
 
-        gains_cfg = (
+        flywheel_gains_cfg = (
             configs.Slot0Configs()
             .with_k_p(0.036653)
             .with_k_i(0)
@@ -36,7 +47,55 @@ def __init__(self) -> None:
         )
 
         self.flywheel_motor_left.configurator.apply(
-            configs.TalonFXConfiguration().with_slot0(gains_cfg)
+            configs.TalonFXConfiguration().with_slot0(flywheel_gains_cfg)
+        )
+
+        self.feeder_motor = TalonSRX(TalonId.FEEDER)
+        self.feeder_motor.setInverted(False)
+
+        self.hood_motor = SparkMax(SparkId.HOOD, SparkMax.MotorType.kBrushless)
+        self.hood_motor.setInverted(True)
+        self.hood_motor_controller = self.hood_motor.getClosedLoopController()
+
+        hood_motor_cfg = SparkMaxConfig()
+        hood_motor_cfg.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
+        hood_motor_cfg.closedLoop.pid(
+            0.5, 0, 0, ClosedLoopSlot.kSlot1
+        )  # TODO Tune these values
+        hood_motor_cfg.closedLoop.allowedClosedLoopError(self.hood_error_tolerance)
+        hood_motor_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+
+        self.hood_encoder = self.hood_motor.getAbsoluteEncoder()
+        hood_encoder_cfg = hood_motor_cfg.absoluteEncoder  # TODO Re-check this line
+        hood_encoder_cfg.positionConversionFactor(self.ENCODER_ROTS_PER_HOOD_DEGREE)
+        hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET)
+
+        configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg)
+
+    @feedback
+    def raw_encoder_angle(self):
+        return self.hood_encoder.getPosition()
+
+    @feedback
+    def get_hood_angle(self):
+        return self.hood_encoder.getPosition() - 90
+
+    @feedback
+    def hood_is_at_setpoint(self):
+        return math.isclose(
+            self.get_hood_angle(),
+            self.desired_hood_angle,
+            rel_tol=self.hood_error_tolerance,
+        )
+
+    def change_pitch_relative(self, angle):
+        self.desired_hood_angle = clamp(
+            self.get_hood_angle() - angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE
+        )
+
+    def change_pitch_absolute(self, angle):
+        self.desired_hood_angle = clamp(
+            90 - angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE
         )
 
     def shoot(self) -> None:
@@ -54,3 +113,7 @@ def execute(self) -> None:
         )
 
         self.feeder_motor.set(ControlMode.PercentOutput, self.target_feeder_percentage)
+
+        self.hood_motor_controller.setSetpoint(
+            self.desired_hood_angle, SparkMax.ControlType.kPosition
+        )

ids.py

@@ -45,6 +45,8 @@ class SparkId(enum.IntEnum):
 
     INTAKE = 11
 
+    HOOD = 10
+
 
 @enum.unique
 class DioChannel(enum.IntEnum):