From d4b2e6f1164b4a50ea569ec81c19657a025ac21f Mon Sep 17 00:00:00 2001 From: Alexander-Mironenko Date: Sun, 25 Jan 2026 17:11:20 +1100 Subject: [PATCH 01/12] Add initial hood interface --- components/shooter.py | 12 ++++++++++++ ids.py | 2 ++ 2 files changed, 14 insertions(+) diff --git a/components/shooter.py b/components/shooter.py index af08507..8197ad6 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -6,6 +6,11 @@ from phoenix6.signals import MotorAlignmentValue from ids import TalonId +from rev import SparkMax, SparkMaxConfig + +from ids import SparkId +from utilities.rev import configure_spark_reset_and_persist +import math class ShooterComponent: @@ -15,6 +20,12 @@ class ShooterComponent: target_feeder_percentage = will_reset_to(0) desired_feeder_percentage = tunable(1) + def sah_slew_absolute(self): + pass + self.slew_absolute = math.radians(90) + + + def __init__(self) -> None: self.flywheel_motor_left = TalonFX( device_id=TalonId.FLYWHEEL_LEFT @@ -24,6 +35,7 @@ def __init__(self) -> None: ) # Defined from behind shooter self.feeder_motor = TalonSRX(TalonId.FEEDER) self.feeder_motor.setInverted(False) + self.hood_motor = SparkMax(SparkId.HOOD_ADJUSTER, SparkMax.MotorType.kBrushless) gains_cfg = ( configs.Slot0Configs() diff --git a/ids.py b/ids.py index 76c2764..4bc2b92 100644 --- a/ids.py +++ b/ids.py @@ -45,6 +45,8 @@ class SparkId(enum.IntEnum): INTAKE = 11 + HOOD_ADJUSTER = 888 + @enum.unique class DioChannel(enum.IntEnum): From 3e5b034eef4db280442f5913794c4dbd0c7e8fd4 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Mon, 26 Jan 2026 13:12:59 +1100 Subject: [PATCH 02/12] add hood motor and encoder config --- components/shooter.py | 58 ++++++++++++++++++++++++++++++++----------- ids.py | 4 ++- 2 files changed, 47 insertions(+), 15 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 8197ad6..9c991b6 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -1,16 +1,19 @@ -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, SparkMax, SparkMaxConfig +from wpilib import DutyCycleEncoder -from ids import TalonId -from rev import SparkMax, SparkMaxConfig - -from ids import SparkId -from utilities.rev import configure_spark_reset_and_persist -import math +from ids import DioChannel, SparkId, TalonId +from utilities.rev import ( + configure_spark_reset_and_persist, + configure_through_bore_encoder, +) class ShooterComponent: @@ -20,11 +23,12 @@ class ShooterComponent: target_feeder_percentage = will_reset_to(0) desired_feeder_percentage = tunable(1) - def sah_slew_absolute(self): - pass - self.slew_absolute = math.radians(90) - + desired_hood_angle = tunable(60) + MAX_HOOD_ANGLE = 70 # TODO Tune this value + MIN_HOOD_ANGLE = 10 # TODO Tune this value + ENCODER_ROTS_PER_DEGREE = 54 / 26 / 360 + ENCODER_ZERO_OFFSET = 0 # TODO Tune this value def __init__(self) -> None: self.flywheel_motor_left = TalonFX( @@ -33,11 +37,18 @@ 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) - self.hood_motor = SparkMax(SparkId.HOOD_ADJUSTER, SparkMax.MotorType.kBrushless) - gains_cfg = ( + self.hood_motor = SparkMax(SparkId.HOOD, SparkMax.MotorType.kBrushless) + self.hood_motor_controller = self.hood_motor.getClosedLoopController() + + self.hood_encoder = DutyCycleEncoder(DioChannel.HOOD_ENCODER, math.tau, 0) + configure_through_bore_encoder(self.hood_encoder) + self.hood_encoder.setInverted(False) # TODO Tune this value + + flywheel_gains_cfg = ( configs.Slot0Configs() .with_k_p(0.036653) .with_k_i(0) @@ -48,9 +59,28 @@ def __init__(self) -> None: ) self.flywheel_motor_left.configurator.apply( - configs.TalonFXConfiguration().with_slot0(gains_cfg) + configs.TalonFXConfiguration().with_slot0(flywheel_gains_cfg) ) + hood_motor_cfg = SparkMaxConfig() + + hood_motor_cfg.setIdleMode(SparkMaxConfig.IdleMode.kBrake) + hood_motor_cfg.closedLoop.pid( + 0.01, 0, 0, ClosedLoopSlot.kSlot1 + ) # TODO Tune these values + + configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) + + @feedback + def raw_encoder_value(self): + return self.hood_encoder.get() + + def articluate_by_angle(self, angle): + pass + + def articluate_to_angle(self, angle): + pass + def shoot(self) -> None: self.target_shooter_rps = self.desired_shooter_rps self.target_feeder_percentage = self.desired_feeder_percentage diff --git a/ids.py b/ids.py index 4bc2b92..eddfd0a 100644 --- a/ids.py +++ b/ids.py @@ -45,7 +45,7 @@ class SparkId(enum.IntEnum): INTAKE = 11 - HOOD_ADJUSTER = 888 + HOOD = 67 # TODO Tune this value @enum.unique @@ -54,6 +54,8 @@ class DioChannel(enum.IntEnum): PORT_VISION_ENCODER = 0 + HOOD_ENCODER = 76 # TODO Tune this value + @enum.unique class PwmChannel(enum.IntEnum): From 9758cf90b06cbdff637627151b0ce94885a2dc83 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Mon, 26 Jan 2026 15:12:20 +1100 Subject: [PATCH 03/12] renamed hood articulate functions --- components/shooter.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 9c991b6..2258f74 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -27,7 +27,7 @@ class ShooterComponent: MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value - ENCODER_ROTS_PER_DEGREE = 54 / 26 / 360 + ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 ENCODER_ZERO_OFFSET = 0 # TODO Tune this value def __init__(self) -> None: @@ -75,10 +75,16 @@ def __init__(self) -> None: def raw_encoder_value(self): return self.hood_encoder.get() - def articluate_by_angle(self, angle): + @feedback + def raw_turret_angle(self): + return ( + self.raw_encoder_value() - self.ENCODER_ZERO_OFFSET + ) * self.ENCODER_ROTS_PER_HOOD_DEGREE + + def articluate_relative(self, angle): pass - def articluate_to_angle(self, angle): + def articluate_absolute(self, angle): pass def shoot(self) -> None: From 5d7e36857e8757684da4e7ff6801e32fe0577ac0 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Mon, 26 Jan 2026 16:59:01 +1100 Subject: [PATCH 04/12] correctly configure the hood encoder to be directly wired into the hood motor controller --- components/shooter.py | 55 ++++++++++++++++++++++++++----------------- ids.py | 4 ++-- 2 files changed, 36 insertions(+), 23 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 2258f74..fb334b9 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -1,15 +1,19 @@ -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, SparkMax, SparkMaxConfig -from wpilib import DutyCycleEncoder +from rev import ( + AbsoluteEncoder, + AbsoluteEncoderConfig, + ClosedLoopSlot, + FeedbackSensor, + SparkMax, + SparkMaxConfig, +) -from ids import DioChannel, SparkId, TalonId +from ids import SparkId, TalonId from utilities.rev import ( configure_spark_reset_and_persist, configure_through_bore_encoder, @@ -17,6 +21,8 @@ class ShooterComponent: + hood_move_speed = tunable(0.01) + target_shooter_rps = will_reset_to(0.0) desired_shooter_rps = tunable(30) @@ -27,7 +33,9 @@ class ShooterComponent: MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value + MOTOR_GEAR_RATIO = 10 ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 + MOTOR_ROTS_PER_HOOD_DEGREE = MOTOR_GEAR_RATIO * ENCODER_ROTS_PER_HOOD_DEGREE ENCODER_ZERO_OFFSET = 0 # TODO Tune this value def __init__(self) -> None: @@ -38,16 +46,6 @@ def __init__(self) -> None: device_id=TalonId.FLYWHEEL_RIGHT ) # Defined from behind shooter - self.feeder_motor = TalonSRX(TalonId.FEEDER) - self.feeder_motor.setInverted(False) - - self.hood_motor = SparkMax(SparkId.HOOD, SparkMax.MotorType.kBrushless) - self.hood_motor_controller = self.hood_motor.getClosedLoopController() - - self.hood_encoder = DutyCycleEncoder(DioChannel.HOOD_ENCODER, math.tau, 0) - configure_through_bore_encoder(self.hood_encoder) - self.hood_encoder.setInverted(False) # TODO Tune this value - flywheel_gains_cfg = ( configs.Slot0Configs() .with_k_p(0.036653) @@ -62,24 +60,36 @@ def __init__(self) -> None: configs.TalonFXConfiguration().with_slot0(flywheel_gains_cfg) ) - hood_motor_cfg = SparkMaxConfig() + self.feeder_motor = TalonSRX(TalonId.FEEDER) + self.feeder_motor.setInverted(False) + + self.hood_encoder = AbsoluteEncoder() + self.hood_encoder_cfg = AbsoluteEncoderConfig() + self.hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET) + self.hood_encoder_cfg.positionConversionFactor(self.MOTOR_GEAR_RATIO) + + # TODO apply encoder config + + self.hood_motor = SparkMax(SparkId.HOOD, SparkMax.MotorType.kBrushless) + 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.01, 0, 0, ClosedLoopSlot.kSlot1 ) # TODO Tune these values + hood_motor_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kAbsoluteEncoder) + configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) @feedback def raw_encoder_value(self): - return self.hood_encoder.get() + return self.hood_encoder.getPosition() @feedback - def raw_turret_angle(self): - return ( - self.raw_encoder_value() - self.ENCODER_ZERO_OFFSET - ) * self.ENCODER_ROTS_PER_HOOD_DEGREE + def raw_hood_angle(self): + return self.raw_encoder_value() * self.ENCODER_ROTS_PER_HOOD_DEGREE def articluate_relative(self, angle): pass @@ -102,3 +112,6 @@ def execute(self) -> None: ) self.feeder_motor.set(ControlMode.PercentOutput, self.target_feeder_percentage) + + self.hood_motor.set(self.hood_move_speed) + # self.hood_motor_controller.setSetpoint diff --git a/ids.py b/ids.py index eddfd0a..a49fc9a 100644 --- a/ids.py +++ b/ids.py @@ -45,7 +45,7 @@ class SparkId(enum.IntEnum): INTAKE = 11 - HOOD = 67 # TODO Tune this value + HOOD = 10 @enum.unique @@ -54,7 +54,7 @@ class DioChannel(enum.IntEnum): PORT_VISION_ENCODER = 0 - HOOD_ENCODER = 76 # TODO Tune this value + HOOD_ENCODER = 1 @enum.unique From 1fd01a7b95ef567be979a7257d63d8f98ba6edaa Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Tue, 27 Jan 2026 23:12:28 +1100 Subject: [PATCH 05/12] configure encoder and write functions to operate the hood --- components/shooter.py | 58 ++++++++++++++++++++++--------------------- 1 file changed, 30 insertions(+), 28 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index fb334b9..b1965c0 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -4,20 +4,10 @@ from phoenix6.controls import Follower from phoenix6.hardware import TalonFX from phoenix6.signals import MotorAlignmentValue -from rev import ( - AbsoluteEncoder, - AbsoluteEncoderConfig, - ClosedLoopSlot, - FeedbackSensor, - SparkMax, - SparkMaxConfig, -) +from rev import ClosedLoopSlot, FeedbackSensor, SparkMax, SparkMaxConfig from ids import SparkId, TalonId -from utilities.rev import ( - configure_spark_reset_and_persist, - configure_through_bore_encoder, -) +from utilities.rev import configure_spark_reset_and_persist class ShooterComponent: @@ -30,6 +20,7 @@ class ShooterComponent: desired_feeder_percentage = tunable(1) desired_hood_angle = tunable(60) + hood_error_tolerance = tunable(5) MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value @@ -63,13 +54,6 @@ def __init__(self) -> None: self.feeder_motor = TalonSRX(TalonId.FEEDER) self.feeder_motor.setInverted(False) - self.hood_encoder = AbsoluteEncoder() - self.hood_encoder_cfg = AbsoluteEncoderConfig() - self.hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET) - self.hood_encoder_cfg.positionConversionFactor(self.MOTOR_GEAR_RATIO) - - # TODO apply encoder config - self.hood_motor = SparkMax(SparkId.HOOD, SparkMax.MotorType.kBrushless) self.hood_motor_controller = self.hood_motor.getClosedLoopController() @@ -78,24 +62,36 @@ def __init__(self) -> None: hood_motor_cfg.closedLoop.pid( 0.01, 0, 0, ClosedLoopSlot.kSlot1 ) # TODO Tune these values - + hood_motor_cfg.closedLoop.allowedClosedLoopError(5) hood_motor_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kAbsoluteEncoder) configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) + self.hood_encoder = self.hood_motor.getAbsoluteEncoder() + hood_encoder_cfg = hood_motor_cfg.absoluteEncoder # TODO Re-check this line + hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET) + hood_encoder_cfg.positionConversionFactor( + self.MOTOR_GEAR_RATIO + * self.ENCODER_ROTS_PER_HOOD_DEGREE # TODO Re-check this line + ) + @feedback - def raw_encoder_value(self): + def raw_encoder_angle(self): return self.hood_encoder.getPosition() @feedback - def raw_hood_angle(self): - return self.raw_encoder_value() * self.ENCODER_ROTS_PER_HOOD_DEGREE + def get_hood_angle(self): + return self.hood_encoder.getPosition() / self.MOTOR_GEAR_RATIO - def articluate_relative(self, angle): - pass + @feedback + def is_hood_at_setpoint(self): + return self.get_hood_angle() + + def change_pitch_relative(self, angle): + self.desired_hood_angle = self.get_hood_angle() - angle - def articluate_absolute(self, angle): - pass + def change_pitch_absolute(self, angle): + self.desired_hood_angle = 90 - angle def shoot(self) -> None: self.target_shooter_rps = self.desired_shooter_rps @@ -114,4 +110,10 @@ def execute(self) -> None: self.feeder_motor.set(ControlMode.PercentOutput, self.target_feeder_percentage) self.hood_motor.set(self.hood_move_speed) - # self.hood_motor_controller.setSetpoint + + # TODO This code is commented until basic hood function has been tested + """ + self.hood_motor_controller.setSetpoint( + self.desired_hood_angle, SparkMax.ControlType.kPosition + ) + """ From e8aca102ae4fbc2186483d26295933ecf2ac2c46 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Wed, 28 Jan 2026 23:42:02 +1100 Subject: [PATCH 06/12] fix encoder position conversion factor and complete some unfinished code --- components/shooter.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index b1965c0..9800992 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -1,3 +1,5 @@ +import math + from magicbot import feedback, tunable, will_reset_to from phoenix5 import ControlMode, TalonSRX from phoenix6 import configs, controls @@ -20,7 +22,7 @@ class ShooterComponent: desired_feeder_percentage = tunable(1) desired_hood_angle = tunable(60) - hood_error_tolerance = tunable(5) + hood_error_tolerance = tunable(5.0) MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value @@ -62,7 +64,7 @@ def __init__(self) -> None: hood_motor_cfg.closedLoop.pid( 0.01, 0, 0, ClosedLoopSlot.kSlot1 ) # TODO Tune these values - hood_motor_cfg.closedLoop.allowedClosedLoopError(5) + hood_motor_cfg.closedLoop.allowedClosedLoopError(self.hood_error_tolerance) hood_motor_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kAbsoluteEncoder) configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) @@ -70,10 +72,7 @@ def __init__(self) -> None: self.hood_encoder = self.hood_motor.getAbsoluteEncoder() hood_encoder_cfg = hood_motor_cfg.absoluteEncoder # TODO Re-check this line hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET) - hood_encoder_cfg.positionConversionFactor( - self.MOTOR_GEAR_RATIO - * self.ENCODER_ROTS_PER_HOOD_DEGREE # TODO Re-check this line - ) + hood_encoder_cfg.positionConversionFactor(self.ENCODER_ROTS_PER_HOOD_DEGREE) @feedback def raw_encoder_angle(self): @@ -81,11 +80,15 @@ def raw_encoder_angle(self): @feedback def get_hood_angle(self): - return self.hood_encoder.getPosition() / self.MOTOR_GEAR_RATIO + return self.hood_encoder.getPosition() @feedback - def is_hood_at_setpoint(self): - return self.get_hood_angle() + 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 = self.get_hood_angle() - angle @@ -114,6 +117,7 @@ def execute(self) -> None: # TODO This code is commented until basic hood function has been tested """ self.hood_motor_controller.setSetpoint( - self.desired_hood_angle, SparkMax.ControlType.kPosition + self.desired_hood_angle * self.MOTOR_GEAR_RATIO, + SparkMax.ControlType.kPosition, ) """ From 714f3839aad506c3be96ec6d48affa6813f42520 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Wed, 28 Jan 2026 23:52:56 +1100 Subject: [PATCH 07/12] fix tunable injection error --- components/shooter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/shooter.py b/components/shooter.py index 9800992..5f5b02d 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -22,7 +22,7 @@ class ShooterComponent: desired_feeder_percentage = tunable(1) desired_hood_angle = tunable(60) - hood_error_tolerance = tunable(5.0) + hood_error_tolerance = 5.0 MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value From 4cdf38ace5d70bd610f2f6a9fe7a376a216e1ad3 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 29 Jan 2026 14:35:12 +1100 Subject: [PATCH 08/12] update gear ratios --- components/shooter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/shooter.py b/components/shooter.py index 5f5b02d..27fa79f 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -26,7 +26,7 @@ class ShooterComponent: MAX_HOOD_ANGLE = 70 # TODO Tune this value MIN_HOOD_ANGLE = 10 # TODO Tune this value - MOTOR_GEAR_RATIO = 10 + MOTOR_GEAR_RATIO = 24 / 22 ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 MOTOR_ROTS_PER_HOOD_DEGREE = MOTOR_GEAR_RATIO * ENCODER_ROTS_PER_HOOD_DEGREE ENCODER_ZERO_OFFSET = 0 # TODO Tune this value From 087ef84a51ee743293ecbcfd40a0fa4f58bf2ff4 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 29 Jan 2026 14:55:59 +1100 Subject: [PATCH 09/12] fixed some logic and config errors (kindly spotted by greptile) --- components/shooter.py | 11 ++++++----- ids.py | 2 -- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 27fa79f..75cde64 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -9,6 +9,7 @@ from rev import ClosedLoopSlot, FeedbackSensor, SparkMax, SparkMaxConfig from ids import SparkId, TalonId +from utilities.functions import clamp from utilities.rev import configure_spark_reset_and_persist @@ -28,7 +29,6 @@ class ShooterComponent: MOTOR_GEAR_RATIO = 24 / 22 ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 - MOTOR_ROTS_PER_HOOD_DEGREE = MOTOR_GEAR_RATIO * ENCODER_ROTS_PER_HOOD_DEGREE ENCODER_ZERO_OFFSET = 0 # TODO Tune this value def __init__(self) -> None: @@ -67,13 +67,13 @@ def __init__(self) -> None: hood_motor_cfg.closedLoop.allowedClosedLoopError(self.hood_error_tolerance) hood_motor_cfg.closedLoop.setFeedbackSensor(FeedbackSensor.kAbsoluteEncoder) - configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) - self.hood_encoder = self.hood_motor.getAbsoluteEncoder() hood_encoder_cfg = hood_motor_cfg.absoluteEncoder # TODO Re-check this line hood_encoder_cfg.zeroOffset(self.ENCODER_ZERO_OFFSET) hood_encoder_cfg.positionConversionFactor(self.ENCODER_ROTS_PER_HOOD_DEGREE) + configure_spark_reset_and_persist(self.hood_motor, hood_motor_cfg) + @feedback def raw_encoder_angle(self): return self.hood_encoder.getPosition() @@ -92,9 +92,11 @@ def hood_is_at_setpoint(self): def change_pitch_relative(self, angle): self.desired_hood_angle = self.get_hood_angle() - angle + clamp(self.desired_hood_angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE) def change_pitch_absolute(self, angle): self.desired_hood_angle = 90 - angle + clamp(self.desired_hood_angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE) def shoot(self) -> None: self.target_shooter_rps = self.desired_shooter_rps @@ -117,7 +119,6 @@ def execute(self) -> None: # TODO This code is commented until basic hood function has been tested """ self.hood_motor_controller.setSetpoint( - self.desired_hood_angle * self.MOTOR_GEAR_RATIO, - SparkMax.ControlType.kPosition, + self.desired_hood_angle, SparkMax.ControlType.kPosition ) """ diff --git a/ids.py b/ids.py index a49fc9a..afca8a6 100644 --- a/ids.py +++ b/ids.py @@ -54,8 +54,6 @@ class DioChannel(enum.IntEnum): PORT_VISION_ENCODER = 0 - HOOD_ENCODER = 1 - @enum.unique class PwmChannel(enum.IntEnum): From 6d73afbe452aee5e100a35137c44f57ab7260d54 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 29 Jan 2026 15:26:10 +1100 Subject: [PATCH 10/12] fix another error --- components/shooter.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 75cde64..e95e3cf 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -91,12 +91,14 @@ def hood_is_at_setpoint(self): ) def change_pitch_relative(self, angle): - self.desired_hood_angle = self.get_hood_angle() - angle - clamp(self.desired_hood_angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_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 = 90 - angle - clamp(self.desired_hood_angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE) + self.desired_hood_angle = clamp( + 90 - angle, self.MIN_HOOD_ANGLE, self.MAX_HOOD_ANGLE + ) def shoot(self) -> None: self.target_shooter_rps = self.desired_shooter_rps From edd15b86fc60f5fa4b5d22be0e0c613f1ea60ec8 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Thu, 29 Jan 2026 21:05:18 +1100 Subject: [PATCH 11/12] tune encoder offsets --- components/shooter.py | 27 ++++++++++----------------- 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index e95e3cf..81d4c5c 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -14,22 +14,19 @@ class ShooterComponent: - hood_move_speed = tunable(0.01) - target_shooter_rps = will_reset_to(0.0) desired_shooter_rps = tunable(30) target_feeder_percentage = will_reset_to(0) desired_feeder_percentage = tunable(1) - desired_hood_angle = tunable(60) - hood_error_tolerance = 5.0 - MAX_HOOD_ANGLE = 70 # TODO Tune this value - MIN_HOOD_ANGLE = 10 # TODO Tune this value + desired_hood_angle = tunable(36.0) + hood_error_tolerance = 3.0 + MAX_HOOD_ANGLE = 59 + MIN_HOOD_ANGLE = 18 - MOTOR_GEAR_RATIO = 24 / 22 - ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 - ENCODER_ZERO_OFFSET = 0 # TODO Tune this value + ENCODER_ROTS_PER_HOOD_DEGREE = 1 / (54 / 26 / 360) + ENCODER_ZERO_OFFSET = 0.7775692 def __init__(self) -> None: self.flywheel_motor_left = TalonFX( @@ -57,20 +54,21 @@ def __init__(self) -> None: 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.01, 0, 0, ClosedLoopSlot.kSlot1 + 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.zeroOffset(self.ENCODER_ZERO_OFFSET) 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) @@ -80,7 +78,7 @@ def raw_encoder_angle(self): @feedback def get_hood_angle(self): - return self.hood_encoder.getPosition() + return self.hood_encoder.getPosition() - 90 @feedback def hood_is_at_setpoint(self): @@ -116,11 +114,6 @@ def execute(self) -> None: self.feeder_motor.set(ControlMode.PercentOutput, self.target_feeder_percentage) - self.hood_motor.set(self.hood_move_speed) - - # TODO This code is commented until basic hood function has been tested - """ self.hood_motor_controller.setSetpoint( self.desired_hood_angle, SparkMax.ControlType.kPosition ) - """ From 614f557275e76f51ff442fe5f4a87f753abbabb3 Mon Sep 17 00:00:00 2001 From: DannyPomeranian Date: Fri, 30 Jan 2026 12:44:33 +1100 Subject: [PATCH 12/12] fix closed loop setup errors and hood at setpoint check --- components/shooter.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/components/shooter.py b/components/shooter.py index 81d4c5c..13228eb 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -25,7 +25,7 @@ class ShooterComponent: MAX_HOOD_ANGLE = 59 MIN_HOOD_ANGLE = 18 - ENCODER_ROTS_PER_HOOD_DEGREE = 1 / (54 / 26 / 360) + ENCODER_ROTS_PER_HOOD_DEGREE = 54 / 26 / 360 ENCODER_ZERO_OFFSET = 0.7775692 def __init__(self) -> None: @@ -60,14 +60,16 @@ def __init__(self) -> None: hood_motor_cfg = SparkMaxConfig() hood_motor_cfg.setIdleMode(SparkMaxConfig.IdleMode.kBrake) hood_motor_cfg.closedLoop.pid( - 0.5, 0, 0, ClosedLoopSlot.kSlot1 + 0.05, 0, 0, ClosedLoopSlot.kSlot1 ) # TODO Tune these values - hood_motor_cfg.closedLoop.allowedClosedLoopError(self.hood_error_tolerance) + hood_motor_cfg.closedLoop.allowedClosedLoopError( + self.hood_error_tolerance, ClosedLoopSlot.kSlot1 + ) 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.positionConversionFactor(1 / 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) @@ -85,7 +87,7 @@ def hood_is_at_setpoint(self): return math.isclose( self.get_hood_angle(), self.desired_hood_angle, - rel_tol=self.hood_error_tolerance, + abs_tol=self.hood_error_tolerance, ) def change_pitch_relative(self, angle): @@ -115,5 +117,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 + self.desired_hood_angle, + SparkMax.ControlType.kPosition, + ClosedLoopSlot.kSlot1, )