diff --git a/components/shooter.py b/components/shooter.py index af08507..81d4c5c 100644 --- a/components/shooter.py +++ b/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,6 +20,14 @@ 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 @@ -22,10 +35,8 @@ def __init__(self) -> None: 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 + ) diff --git a/ids.py b/ids.py index 76c2764..afca8a6 100644 --- a/ids.py +++ b/ids.py @@ -45,6 +45,8 @@ class SparkId(enum.IntEnum): INTAKE = 11 + HOOD = 10 + @enum.unique class DioChannel(enum.IntEnum):