From 1b3caeea0640b140536eec561af69cdbc8f08a92 Mon Sep 17 00:00:00 2001 From: e-bauman Date: Wed, 11 Feb 2026 14:56:45 -0500 Subject: [PATCH 01/19] add indexing trigger --- robotcontainer.py | 4 ++++ subsystems/drivetrain/command.py | 7 +++++-- subsystems/drivetrain/command_swerve_drivetrain.py | 2 ++ subsystems/drivetrain/constants.py | 3 ++- subsystems/shooter/command.py | 1 + subsystems/shooter/subsystem.py | 5 +---- 6 files changed, 15 insertions(+), 7 deletions(-) diff --git a/robotcontainer.py b/robotcontainer.py index 0c50279..0c0f980 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -107,6 +107,10 @@ def configureButtonBindings(self) -> None: ) ) + Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue( + RunIndexer(self.indexer) + ) + # force the indexer to spin self.operator_controller.a().or_(self.driver_controller.a()).whileTrue( RunIndexer(self.indexer) diff --git a/subsystems/drivetrain/command.py b/subsystems/drivetrain/command.py index 5b69a6f..b24b7d1 100644 --- a/subsystems/drivetrain/command.py +++ b/subsystems/drivetrain/command.py @@ -49,16 +49,19 @@ def execute(self): self.v_y = math_utils.curve(-self.controller.getLeftX(), deadband, curve) * max_speed self.cmd_speed = math.hypot(self.v_x, self.v_y) + self.is_facing_angle = self.drivetrain.is_facing_angle(self.target_angle.radians()) - if self.drivetrain.is_facing_angle(self.target_angle.radians()) and self.cmd_speed == 0: + if self.is_facing_angle and self.cmd_speed == 0: self.drivetrain.set_control(self._brake) - elif self.cmd_speed > 0 or not self.drivetrain.is_facing_angle(self.target_angle.radians()): + else: self.drivetrain.set_control( self._aim_at.with_target_direction(self.target_angle) .with_velocity_x(self.v_x) .with_velocity_y(self.v_y) ) + + self.drivetrain.ready_to_shoot = self.cmd_speed < drivetrain_shooting_velocity_tolerance and self.is_facing_angle def isFinished(self) -> bool: return False diff --git a/subsystems/drivetrain/command_swerve_drivetrain.py b/subsystems/drivetrain/command_swerve_drivetrain.py index 7780c45..a018dcb 100644 --- a/subsystems/drivetrain/command_swerve_drivetrain.py +++ b/subsystems/drivetrain/command_swerve_drivetrain.py @@ -249,6 +249,8 @@ def __init__( self ) + self.ready_to_shoot = False + def get_pose(self) -> Pose2d: return self.get_state().pose diff --git a/subsystems/drivetrain/constants.py b/subsystems/drivetrain/constants.py index 7017b2c..6e140b8 100644 --- a/subsystems/drivetrain/constants.py +++ b/subsystems/drivetrain/constants.py @@ -18,4 +18,5 @@ aiming_pid_i = 0 aiming_pid_d = 0 -drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees \ No newline at end of file +drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees +drivetrain_shooting_velocity_tolerance = 0.1 # PLACEHOLDER, percentage (0 to 1) \ No newline at end of file diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 4f80f09..c0f4414 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -39,6 +39,7 @@ def isFinished(self): def end(self, interrupted): pass + class SetShooterAuto(commands2.Command): """ uses target_stationary function to set left and right flywheels to specified velocity and set hood to specified angle. diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 01477d2..69e31d4 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -153,10 +153,7 @@ def ready_to_shoot(self): Returns: boolean: whether or not the system is ready to shoot (true it is and false it isn't) """ - if self.left_is_at_velocity(self.left_target_velocity) == True and self.right_is_at_velocity(self.right_target_velocity) and self.hood_is_at_angle(self.hood_target_angle) == True: - return True - else: - return False + return self.left_is_at_velocity(self.left_target_velocity) == True and self.right_is_at_velocity(self.right_target_velocity) and self.hood_is_at_angle(self.hood_target_angle) == True def target_stationary(self, robot_pose: Pose2d, passing: bool): """ From e8351e8677a9fc4eb5a9d90fd851a738bb297841 Mon Sep 17 00:00:00 2001 From: lhonney26 Date: Wed, 11 Feb 2026 15:27:15 -0500 Subject: [PATCH 02/19] set shooter command --- subsystems/shooter/command.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index c0f4414..a7e88ce 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -90,5 +90,37 @@ def execute(self): def isFinished(self): return False + def end(self, interrupted): + pass + +class SetShooter(commands2.Command): + """ + sets left and right flywheels to specified velocity and set hood to specified angle + never ends + + Args: + velocity (rotations per second): desired left and right flywheel velocity + angle (radians): desired hood angle + """ + + def __init__(self, subsytem: Shooter, velocity: float, angle: float): + super().__init__() + + self.subsystem = subsytem + self.addRequirements(self.subsystem) + self.velocity = velocity + self.angle = angle + + def initialize(self): + self.subsystem.set_left_target_velocity(self.velocity) + self.subsystem.set_right_target_velocity(self.velocity) + self.subsystem.set_hood_angle(self.angle) + + def execute(self): + pass + + def isFinished(self): + return False + def end(self, interrupted): pass \ No newline at end of file From fc3651998ccd53db45b42d1168590dac944f8cb6 Mon Sep 17 00:00:00 2001 From: e-bauman Date: Wed, 11 Feb 2026 15:59:33 -0500 Subject: [PATCH 03/19] fix units --- subsystems/shooter/command.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index a7e88ce..9693348 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -4,6 +4,7 @@ from subsystems.drivetrain.command_swerve_drivetrain import CommandSwerveDrivetrain from utils import alliance_flip_util, field_constants from .constants import * +from phoenix6 import units class AimShooter(commands2.Command): """ @@ -103,7 +104,7 @@ class SetShooter(commands2.Command): angle (radians): desired hood angle """ - def __init__(self, subsytem: Shooter, velocity: float, angle: float): + def __init__(self, subsytem: Shooter, velocity: units.rotations_per_second, angle: units.radian): super().__init__() self.subsystem = subsytem From e0fc40f1a9d9f961b31bf772cfe29379ab1161b2 Mon Sep 17 00:00:00 2001 From: lhonney26 Date: Wed, 11 Feb 2026 16:00:07 -0500 Subject: [PATCH 04/19] drive at angle command --- subsystems/drivetrain/command.py | 53 ++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/subsystems/drivetrain/command.py b/subsystems/drivetrain/command.py index b24b7d1..17d85f9 100644 --- a/subsystems/drivetrain/command.py +++ b/subsystems/drivetrain/command.py @@ -5,6 +5,7 @@ from .command_swerve_drivetrain import CommandSwerveDrivetrain from phoenix6 import swerve from utils import alliance_flip_util, field_constants, math_utils, shooter_utils +from wpimath.geometry import Rotation2d class AimDrivetrain(commands2.Command): def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.button.CommandXboxController): @@ -66,5 +67,57 @@ def execute(self): def isFinished(self) -> bool: return False + def end(self, interrupted: bool) -> None: + pass + +class DriveAtAngle(commands2.Command): + def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.button.CommandXboxController, target_angle: Rotation2d): + super().__init__() + + self.drivetrain = subsystem + self.controller = controller + self._brake = swerve.requests.SwerveDriveBrake() + self._aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( + aiming_pid_p, + aiming_pid_i, + aiming_pid_d + ) + + self.addRequirements(self.drivetrain) + self.target_angle = target_angle + + def initialize(self): + self.target_angle = alliance_flip_util.get_alliance(self.target_angle) + + def execute(self): + """ + 1. Calculate v_x and v_y based on controller inputs + 2. If speed is too high, drive at angle + 3. Check if drivetrain is facing angle within tolerance + 4. If facing angle and speed is 0, apply brake + 5. Else: drive at angle + """ + + self.v_x = math_utils.curve(-self.controller.getLeftY(), deadband) * max_speed + self.v_y = math_utils.curve(-self.controller.getLeftX(), deadband, curve) * max_speed + + self.cmd_speed = math.hypot(self.v_x, self.v_y) + self.is_facing_angle = self.drivetrain.is_facing_angle(self.target_angle.radians()) + + if self.is_facing_angle and self.cmd_speed == 0: + self.drivetrain.set_control(self._brake) + + else: + self.drivetrain.set_control( + self._aim_at.with_target_direction(self.target_angle) + .with_velocity_x(self.v_x) + .with_velocity_y(self.v_y) + ) + + self.drivetrain.ready_to_shoot = self.cmd_speed < drivetrain_shooting_velocity_tolerance and self.is_facing_angle + + def isFinished(self) -> bool: + return False + def end(self, interrupted: bool) -> None: pass \ No newline at end of file From e4112bef423f55429d58f5b83cc3f2996c69f46b Mon Sep 17 00:00:00 2001 From: lhonney26 Date: Thu, 12 Feb 2026 16:25:06 -0500 Subject: [PATCH 05/19] fixed drivetrain and shooter aiming --- robot_constants.py | 10 +++++++++- robotcontainer.py | 23 +++++++++++++++++++---- subsystems/drivetrain/__init__.py | 2 +- subsystems/shooter/__init__.py | 2 +- 4 files changed, 30 insertions(+), 7 deletions(-) diff --git a/robot_constants.py b/robot_constants.py index 1b8c39a..b4c333c 100644 --- a/robot_constants.py +++ b/robot_constants.py @@ -1 +1,9 @@ -LOGGING = True \ No newline at end of file +LOGGING = True + +tower_drivetrain_angle = 0 # PLACEHOLDER +tower_flywheel_velocity = 0 # PLACEHOLDER +tower_hood_angle = 0 # PLACEHOLDER + +hub_drivetrain_angle = 0 # PLACEHOLDER +hub_flywheel_velocity = 0 # PLACEHOLDER +hub_hood_angle = 0 # PLACEHOLDER \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py index 0c0f980..f9f1a87 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -3,13 +3,14 @@ # Open Source Software; you can modify and/or share it under the terms of # the WPILib BSD license file in the root directory of this project. # -from commands2 import ParallelCommandGroup +from commands2 import ParallelCommandGroup, ConditionalCommand from commands2.button import CommandXboxController, Trigger from commands2.sysid import SysIdRoutine from generated.tuner_constants import TunerConstants from telemetry import Telemetry from subsystems import * +from robot_constants import * from phoenix6 import swerve from wpilib import DriverStation, SendableChooser, SmartDashboard @@ -101,9 +102,23 @@ def configureButtonBindings(self) -> None: # Aim drivetrain and shooter self.driver_controller.rightTrigger().whileTrue( - ParallelCommandGroup( - AimDrivetrain(self.drivetrain, self.driver_controller), - AimShooter(self.shooter, self.drivetrain) + ConditionalCommand( + ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), + SetShooter(self.shooter, self.driver_controller, tower_flywheel_velocity, tower_hood_angle) + ), + ConditionalCommand( + ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), + SetShooter(self.shooter, self.driver_controller, hub_flywheel_velocity, hub_hood_angle) + ), + ParallelCommandGroup( + AimDrivetrain(self.drivetrain, self.driver_controller), + AimShooter(self.shooter, self.drivetrain) + ), + lambda: self.operator_controller.getHID().getPOV() == 0 + ), + lambda: self.operator_controller.getHID().getPOV() == 180 ) ) diff --git a/subsystems/drivetrain/__init__.py b/subsystems/drivetrain/__init__.py index 5dc5843..b1a5b9f 100644 --- a/subsystems/drivetrain/__init__.py +++ b/subsystems/drivetrain/__init__.py @@ -1,3 +1,3 @@ from .command_swerve_drivetrain import CommandSwerveDrivetrain -from .command import AimDrivetrain +from .command import AimDrivetrain, DriveAtAngle from .constants import max_angular_rate, max_speed, deadband, curve \ No newline at end of file diff --git a/subsystems/shooter/__init__.py b/subsystems/shooter/__init__.py index 57a6798..9ef3835 100644 --- a/subsystems/shooter/__init__.py +++ b/subsystems/shooter/__init__.py @@ -1,2 +1,2 @@ from .subsystem import Shooter -from .command import AimShooter, SetShooterAuto, SetShooterIdle \ No newline at end of file +from .command import AimShooter, SetShooterAuto, SetShooterIdle, SetShooter \ No newline at end of file From 7195d3c487a26e24aa699aa6c4cb72e0be8ffaa0 Mon Sep 17 00:00:00 2001 From: alexl1239 Date: Thu, 12 Feb 2026 16:45:54 -0500 Subject: [PATCH 06/19] shooter tuning command setup --- robotcontainer.py | 16 ++++++++++------ subsystems/shooter/__init__.py | 2 +- subsystems/shooter/command.py | 34 +++++++++++++++++++++++++++++++-- subsystems/shooter/constants.py | 1 - utils/shooter_utils.py | 14 +++----------- 5 files changed, 46 insertions(+), 21 deletions(-) diff --git a/robotcontainer.py b/robotcontainer.py index 3ba71d7..540522d 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -99,12 +99,16 @@ def configureButtonBindings(self) -> None: self.drivetrain.runOnce(self.drivetrain.seed_field_centric) ) - # Aim drivetrain and shooter - self.driver_controller.rightTrigger().whileTrue( - ParallelCommandGroup( - AimDrivetrain(self.drivetrain, self.driver_controller), - AimShooter(self.shooter, self.drivetrain) - ) + # # Aim drivetrain and shooter + # self.driver_controller.rightTrigger().whileTrue( + # ParallelCommandGroup( + # AimDrivetrain(self.drivetrain, self.driver_controller), + # AimShooter(self.shooter, self.drivetrain) + # ) + # ) + + self.driver_controller.rightTrigger().onTrue( + TuneShooter(self.shooter) ) Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue( diff --git a/subsystems/shooter/__init__.py b/subsystems/shooter/__init__.py index 57a6798..4330371 100644 --- a/subsystems/shooter/__init__.py +++ b/subsystems/shooter/__init__.py @@ -1,2 +1,2 @@ from .subsystem import Shooter -from .command import AimShooter, SetShooterAuto, SetShooterIdle \ No newline at end of file +from .command import AimShooter, SetShooterAuto, SetShooterIdle, TuneShooter \ No newline at end of file diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 9693348..7f9f1c2 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -6,6 +6,8 @@ from .constants import * from phoenix6 import units +from ntcore import NetworkTableInstance + class AimShooter(commands2.Command): """ uses target_stationary function to set left and right flywheels to specified velocity and set hood to specified angle. @@ -104,10 +106,10 @@ class SetShooter(commands2.Command): angle (radians): desired hood angle """ - def __init__(self, subsytem: Shooter, velocity: units.rotations_per_second, angle: units.radian): + def __init__(self, subsystem: Shooter, velocity: units.rotations_per_second, angle: units.radian): super().__init__() - self.subsystem = subsytem + self.subsystem = subsystem self.addRequirements(self.subsystem) self.velocity = velocity self.angle = angle @@ -123,5 +125,33 @@ def execute(self): def isFinished(self): return False + def end(self, interrupted): + pass + + +class TuneShooter(commands2.Command): + def __init__(self, subsystem: Shooter): + super().__init__() + + self.subsystem = subsystem + self.addRequirements(self.subsystem) + + self.nt_inst = NetworkTableInstance.getDefault() + self.shot_tuner = self.nt_inst.getTable("Shot Tuner") + + self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").subscribe(20.0) + self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").subscribe(15.0) + + def initialize(self): + self.subsystem.set_left_target_velocity(self.flywheel_rps_sub.get()) + self.subsystem.set_right_target_velocity(self.flywheel_rps_sub.get()) + self.subsystem.set_hood_angle(self.hood_angle_sub.get()) + + def execute(self): + pass + + def isFinished(self): + return False + def end(self, interrupted): pass \ No newline at end of file diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 85d8bbd..165834a 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -78,7 +78,6 @@ def load_shooter_table_csv(rel_path: str) -> np.ndarray: return table -shooter_offset = Translation2d(1, 1) # placeholder max_hood_angle = 43 # placeholder min_hood_angle = 0 # placeholder diff --git a/utils/shooter_utils.py b/utils/shooter_utils.py index 10830bd..8a0f855 100644 --- a/utils/shooter_utils.py +++ b/utils/shooter_utils.py @@ -2,7 +2,7 @@ from utils import alliance_flip_util, field_constants import math -from subsystems.shooter.constants import DIST_M, HOOD_DEG, RPS, shooter_offset, PASS_DIST_M, PASS_HOOD_DEG, PASS_RPS +from subsystems.shooter.constants import DIST_M, HOOD_DEG, RPS, PASS_DIST_M, PASS_HOOD_DEG, PASS_RPS from utils.field_constants import Hub import numpy as np @@ -51,11 +51,7 @@ def shot_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: """ hub2d: Translation2d = alliance_flip_util.get_alliance(Translation2d(Hub.INNER_CENTER_POINT.x, Hub.INNER_CENTER_POINT.y)) - shooter_origin_field: Translation2d = ( - robot_pose.translation() + shooter_offset.rotateBy(robot_pose.rotation()) - ) - - distance_m: float = shooter_origin_field.distance(hub2d) + distance_m: float = robot_pose.translation().distance(hub2d) hood_deg: float = float(np.interp(distance_m, DIST_M, HOOD_DEG)) rps: float = float(np.interp(distance_m, DIST_M, RPS)) @@ -72,11 +68,7 @@ def pass_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: """ pass_setpoint: Translation2d = alliance_flip_util.get_alliance(get_pass_setpoint(robot_pose)) - shooter_origin_field: Translation2d = ( - robot_pose.translation() + shooter_offset.rotateBy(robot_pose.rotation()) - ) - - distance_m: float = shooter_origin_field.distance(pass_setpoint) + distance_m: float = robot_pose.translation().distance(pass_setpoint) hood_deg: float = float(np.interp(distance_m, PASS_DIST_M, PASS_HOOD_DEG)) rps: float = float(np.interp(distance_m, PASS_DIST_M, PASS_RPS)) From 62db52ba77b87032f2451d2fc44185817f648f79 Mon Sep 17 00:00:00 2001 From: alexl1239 Date: Thu, 12 Feb 2026 17:13:59 -0500 Subject: [PATCH 07/19] added drivetrain to publish --- robotcontainer.py | 5 ++++- subsystems/shooter/command.py | 8 ++++++-- utils/shooter_utils.py | 5 +++++ 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/robotcontainer.py b/robotcontainer.py index 732f51b..0b37d1d 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -123,7 +123,10 @@ def configureButtonBindings(self) -> None: # ) self.driver_controller.rightTrigger().onTrue( - TuneShooter(self.shooter) + ParallelCommandGroup( + TuneShooter(self.shooter, self.drivetrain), + AimDrivetrain(self.drivetrain, self.driver_controller) + ) ) diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 7f9f1c2..01a8e4b 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -2,7 +2,7 @@ from .subsystem import Shooter from wpimath.geometry import Pose2d from subsystems.drivetrain.command_swerve_drivetrain import CommandSwerveDrivetrain -from utils import alliance_flip_util, field_constants +from utils import alliance_flip_util, field_constants, shooter_utils from .constants import * from phoenix6 import units @@ -130,23 +130,27 @@ def end(self, interrupted): class TuneShooter(commands2.Command): - def __init__(self, subsystem: Shooter): + def __init__(self, subsystem: Shooter, drivetrain: CommandSwerveDrivetrain): super().__init__() self.subsystem = subsystem self.addRequirements(self.subsystem) + self.drivetrain = drivetrain self.nt_inst = NetworkTableInstance.getDefault() self.shot_tuner = self.nt_inst.getTable("Shot Tuner") self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").subscribe(20.0) self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").subscribe(15.0) + self.distance_pub = self.shot_tuner.getDoubleTopic("distance to hub").publish() def initialize(self): self.subsystem.set_left_target_velocity(self.flywheel_rps_sub.get()) self.subsystem.set_right_target_velocity(self.flywheel_rps_sub.get()) self.subsystem.set_hood_angle(self.hood_angle_sub.get()) + self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose())) + def execute(self): pass diff --git a/utils/shooter_utils.py b/utils/shooter_utils.py index 8a0f855..f9c4ef7 100644 --- a/utils/shooter_utils.py +++ b/utils/shooter_utils.py @@ -58,6 +58,11 @@ def shot_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: return hood_deg, rps +def shot_distance_from_pose(robot_pose: Pose2d) -> float: + hub2d: Translation2d = alliance_flip_util.get_alliance(Translation2d(Hub.INNER_CENTER_POINT.x, Hub.INNER_CENTER_POINT.y)) + + return robot_pose.translation().distance(hub2d) + #passing def pass_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: """ From d846f121609264dca0248c4779a9924d1498a73b Mon Sep 17 00:00:00 2001 From: e-bauman Date: Thu, 12 Feb 2026 17:20:30 -0500 Subject: [PATCH 08/19] i think it works --- subsystems/shooter/command.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 01a8e4b..85c2cec 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -140,6 +140,9 @@ def __init__(self, subsystem: Shooter, drivetrain: CommandSwerveDrivetrain): self.nt_inst = NetworkTableInstance.getDefault() self.shot_tuner = self.nt_inst.getTable("Shot Tuner") + + self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").publish() + self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").publish() self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").subscribe(20.0) self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").subscribe(15.0) self.distance_pub = self.shot_tuner.getDoubleTopic("distance to hub").publish() @@ -149,10 +152,8 @@ def initialize(self): self.subsystem.set_right_target_velocity(self.flywheel_rps_sub.get()) self.subsystem.set_hood_angle(self.hood_angle_sub.get()) - self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose())) - def execute(self): - pass + self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose())) def isFinished(self): return False From e920a2b53d2b690c798f3842536c76700ef6ea64 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Sat, 14 Feb 2026 13:09:55 -0500 Subject: [PATCH 09/19] absolute encoder --- subsystems/shooter/constants.py | 3 ++- subsystems/shooter/subsystem.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 165834a..92bcb16 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -56,7 +56,8 @@ ).with_feedback( configs.FeedbackConfigs() .with_sensor_to_mechanism_ratio(0) # placeholder - .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.ROTOR_SENSOR) + .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.REMOTE_CANCODER) + .with_feedback_remote_sensor_id(23) ) def load_shooter_table_csv(rel_path: str) -> np.ndarray: diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 69e31d4..f0b7551 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -35,7 +35,7 @@ def __init__(self): .with_inverted(right_direction) )) self.right_follower_motor.set_control(controls.Follower(right_lead_id, signals.MotorAlignmentValue.ALIGNED)) - + self.hood_motor.configurator.apply(hood_config) self.table = ntcore.NetworkTableInstance.getDefault().getTable("shooter") From 4a62119e83624c42b204607533d85e68a3db56d6 Mon Sep 17 00:00:00 2001 From: alexl1239 Date: Tue, 17 Feb 2026 15:47:04 -0500 Subject: [PATCH 10/19] shot tuner update for better network tables --- utils/shooter_utils.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/utils/shooter_utils.py b/utils/shooter_utils.py index f9c4ef7..a937e06 100644 --- a/utils/shooter_utils.py +++ b/utils/shooter_utils.py @@ -49,9 +49,7 @@ def shot_setpoints_from_pose(robot_pose: Pose2d) -> tuple[float, float]: - measures distance to hub center - interpolates hood and rps from DIST_M tables """ - hub2d: Translation2d = alliance_flip_util.get_alliance(Translation2d(Hub.INNER_CENTER_POINT.x, Hub.INNER_CENTER_POINT.y)) - - distance_m: float = robot_pose.translation().distance(hub2d) + distance_m: float = shot_distance_from_pose(robot_pose) hood_deg: float = float(np.interp(distance_m, DIST_M, HOOD_DEG)) rps: float = float(np.interp(distance_m, DIST_M, RPS)) From a598e3540e29ad3d42bdfeb8b9e293def725cdc4 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Thu, 19 Feb 2026 16:50:47 -0500 Subject: [PATCH 11/19] fused cancoder for hoodmotor im so sorry lord eben for not initializing the cancoder in init --- subsystems/shooter/constants.py | 8 +++++--- subsystems/shooter/subsystem.py | 2 ++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 92bcb16..471cf08 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -8,6 +8,7 @@ right_lead_id: int = 60 # placeholder right_follow_id: int = 61 # placeholder hood_id: int = 62 # placeholder +hood_cancoder_id: int = 63 #placeholder flywheel_threshold = 2.0 # placeholder hood_threshold = 2.0 # placeholder @@ -56,9 +57,10 @@ ).with_feedback( configs.FeedbackConfigs() .with_sensor_to_mechanism_ratio(0) # placeholder - .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.REMOTE_CANCODER) - .with_feedback_remote_sensor_id(23) -) + .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.FUSED_CANCODER) + .with_feedback_remote_sensor_id(hood_cancoder_id) +) + def load_shooter_table_csv(rel_path: str) -> np.ndarray: """ diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index f0b7551..f9364c0 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -18,6 +18,8 @@ def __init__(self): self.velocity_torque_current = controls.VelocityTorqueCurrentFOC(0) self.hood_motor = hardware.TalonFX(hood_id) + self.hood_cancoder = hardware.CANcoder(hood_cancoder_id) + self.motion_magic = controls.MotionMagicVoltage(0) self.left_target_velocity = 0 From 0ed72ccc8d5baf8af05b983a13e67c0c0deb08e5 Mon Sep 17 00:00:00 2001 From: e-bauman Date: Fri, 20 Feb 2026 14:04:47 -0500 Subject: [PATCH 12/19] updated gear ratios --- subsystems/shooter/constants.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 471cf08..746db5d 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -8,12 +8,13 @@ right_lead_id: int = 60 # placeholder right_follow_id: int = 61 # placeholder hood_id: int = 62 # placeholder -hood_cancoder_id: int = 63 #placeholder +hood_cancoder_id: int = 57 #placeholder flywheel_threshold = 2.0 # placeholder hood_threshold = 2.0 # placeholder -hood_gear_ratio = 0 # placeholder +max_hood_angle = 43 # placeholder +min_hood_angle = 0 # placeholder idle_velocity = 0 # placeholder @@ -56,7 +57,8 @@ .with_k_g(0) # placeholder ).with_feedback( configs.FeedbackConfigs() - .with_sensor_to_mechanism_ratio(0) # placeholder + .with_rotor_to_sensor_ratio(23*1.5) + .with_sensor_to_mechanism_ratio(2) .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.FUSED_CANCODER) .with_feedback_remote_sensor_id(hood_cancoder_id) ) @@ -81,9 +83,6 @@ def load_shooter_table_csv(rel_path: str) -> np.ndarray: return table -max_hood_angle = 43 # placeholder -min_hood_angle = 0 # placeholder - # robot distance to hub, hood angle, and RPS SHOT_TABLE = load_shooter_table_csv("shot_table.csv") From f9adf32d8e30e966eb80f32eff0b00f5da7a6c5f Mon Sep 17 00:00:00 2001 From: achung28 Date: Fri, 20 Feb 2026 14:31:50 -0500 Subject: [PATCH 13/19] hood cancoder Don't know if this is right at all --- subsystems/shooter/constants.py | 6 ++++++ subsystems/shooter/subsystem.py | 4 +++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 746db5d..1719820 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -13,6 +13,7 @@ flywheel_threshold = 2.0 # placeholder hood_threshold = 2.0 # placeholder +hood_gear_ratio = 69 # 69:1 max_hood_angle = 43 # placeholder min_hood_angle = 0 # placeholder @@ -23,6 +24,11 @@ left_direction = signals.InvertedValue.COUNTER_CLOCKWISE_POSITIVE right_direction = signals.InvertedValue.CLOCKWISE_POSITIVE +hood_cancoder_config = configs.CANcoderConfiguration() +hood_cancoder_config.magnet_sensor.sensor_direction = signals.SensorDirectionValue.COUNTER_CLOCKWISE_POSITIVE +hood_cancoder_config.magnet_sensor.magnet_offset = 0 #placeholder +hood_cancoder_config.magnet_sensor.absolute_sensor_discontinuity_point = 0 #placeholder + flywheel_config = configs.TalonFXConfiguration().with_motor_output( configs.MotorOutputConfigs() .with_neutral_mode(signals.NeutralModeValue.BRAKE) diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index f9364c0..9f25d59 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -25,7 +25,9 @@ def __init__(self): self.left_target_velocity = 0 self.right_target_velocity = 0 self.hood_target_angle = 0 - + + self.hood_cancoder.configurator.apply(hood_cancoder_config) + self.left_leader_motor.configurator.apply(flywheel_config.with_motor_output( configs.MotorOutputConfigs() .with_inverted(left_direction) From 1fc875c52a65f2b9c6145480bc12862693d95400 Mon Sep 17 00:00:00 2001 From: achung28 Date: Fri, 20 Feb 2026 16:23:17 -0500 Subject: [PATCH 14/19] Update constants.py CHanged the way we do configs for the cancoder --- subsystems/shooter/constants.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 1719820..53844de 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -24,10 +24,12 @@ left_direction = signals.InvertedValue.COUNTER_CLOCKWISE_POSITIVE right_direction = signals.InvertedValue.CLOCKWISE_POSITIVE -hood_cancoder_config = configs.CANcoderConfiguration() -hood_cancoder_config.magnet_sensor.sensor_direction = signals.SensorDirectionValue.COUNTER_CLOCKWISE_POSITIVE -hood_cancoder_config.magnet_sensor.magnet_offset = 0 #placeholder -hood_cancoder_config.magnet_sensor.absolute_sensor_discontinuity_point = 0 #placeholder +hood_cancoder_config = configs.CANcoderConfiguration().with_magnet_sensor( + configs.MagnetSensorConfigs() + .with_absolute_sensor_discontinuity_point(0) # placeholder + .with_magnet_offset(0) # placeholder + .with_sensor_direction(signals.SensorDirectionValue.COUNTER_CLOCKWISE_POSITIVE) +) flywheel_config = configs.TalonFXConfiguration().with_motor_output( configs.MotorOutputConfigs() From c595a9e8798e30a8d5ddbbf7cbfe62734a7ad1b6 Mon Sep 17 00:00:00 2001 From: achung28 Date: Fri, 20 Feb 2026 16:27:57 -0500 Subject: [PATCH 15/19] Fixed shooter issues Get_hood_angle assumed we returned a float so it converted it to a rotation but it was already a rotation. Is_hood_at_angle didn't get the value of the get_hood_angle status signal correctly so it highkey broke. added .value to fix dwdw --- subsystems/shooter/subsystem.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 9f25d59..547edc4 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -136,7 +136,7 @@ def get_hood_angle(self): Returns: return_float: current hood position in radians """ - return (self.hood_motor.get_position()) * (2 * math.pi) + return self.hood_motor.get_position() def hood_is_at_angle(self, angle: float): """ @@ -148,7 +148,7 @@ def hood_is_at_angle(self, angle: float): Returns: boolean: whether or not the hood is at the angle (true it is and false it isn't) """ - return abs(self.get_hood_angle() - angle) < hood_threshold + return abs(self.get_hood_angle().value - angle) < hood_threshold def ready_to_shoot(self): """ From 9976edd03bb847fd74d23ecab3855cc5698101ec Mon Sep 17 00:00:00 2001 From: achung28 Date: Fri, 20 Feb 2026 16:28:58 -0500 Subject: [PATCH 16/19] One last issue published the status signal not the value itself --- subsystems/shooter/subsystem.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 547edc4..4a756a4 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -181,7 +181,7 @@ def update_table(self): self.right_velocity_pub.set(self.get_right_velocity()) self.left_target_velocity_pub.set(self.left_target_velocity) self.right_target_velocity_pub.set(self.right_target_velocity) - self.hood_angle_pub.set(self.get_hood_angle()) + self.hood_angle_pub.set(self.get_hood_angle().value) self.hood_target_angle_pub.set(self.hood_target_angle) self.shooter_ready_pub.set(self.ready_to_shoot()) From 330dc9f2fe1a642dfa91c34d4aaa42b7520b0108 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Sat, 21 Feb 2026 16:01:04 -0500 Subject: [PATCH 17/19] stationary commands --- robot_constants.py | 6 ++++-- robotcontainer.py | 23 +++++++++++++++++++++-- subsystems/shooter/__init__.py | 2 +- subsystems/shooter/command.py | 6 +++--- 4 files changed, 29 insertions(+), 8 deletions(-) diff --git a/robot_constants.py b/robot_constants.py index b4c333c..82693fb 100644 --- a/robot_constants.py +++ b/robot_constants.py @@ -1,9 +1,11 @@ + +from wpimath.geometry import Rotation2d LOGGING = True -tower_drivetrain_angle = 0 # PLACEHOLDER +tower_drivetrain_angle = Rotation2d() # PLACEHOLDER tower_flywheel_velocity = 0 # PLACEHOLDER tower_hood_angle = 0 # PLACEHOLDER -hub_drivetrain_angle = 0 # PLACEHOLDER +hub_drivetrain_angle = Rotation2d() # PLACEHOLDER hub_flywheel_velocity = 0 # PLACEHOLDER hub_hood_angle = 0 # PLACEHOLDER \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py index ee9db7b..85a6a41 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -3,7 +3,7 @@ # Open Source Software; you can modify and/or share it under the terms of # the WPILib BSD license file in the root directory of this project. # -from commands2 import ParallelCommandGroup, ConditionalCommand +from commands2 import ParallelCommandGroup, ConditionalCommand, SelectCommand from commands2.button import CommandXboxController, Trigger from commands2.sysid import SysIdRoutine @@ -121,6 +121,26 @@ def configureButtonBindings(self) -> None: # lambda: self.operator_controller.getHID().getPOV() == 180 # ) # ) + + + # stationary commands + self.tower = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, tower_drivetrain_angle.radians()) + ) + + self.hub = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, hub_drivetrain_angle.radians()) + ) + + self.stationaryselector = SelectCommand( + { + 90: self.hub, + 180: self.tower + }, + self.operator_controller.getHID().getPOV + ) self.driver_controller.rightTrigger().onTrue( ParallelCommandGroup( @@ -129,7 +149,6 @@ def configureButtonBindings(self) -> None: ) ) - Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue( RunIndexer(self.indexer) ) diff --git a/subsystems/shooter/__init__.py b/subsystems/shooter/__init__.py index 1c40db6..97b6484 100644 --- a/subsystems/shooter/__init__.py +++ b/subsystems/shooter/__init__.py @@ -1,2 +1,2 @@ from .subsystem import Shooter -from .command import AimShooter, SetShooterAuto, SetShooterIdle, TuneShooter +from .command import AimShooter, SetShooterAuto, SetShooterIdle, TuneShooter, SetShooter diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 85c2cec..02b99ea 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -61,7 +61,7 @@ def __init__(self, subsystem: Shooter, pose: Pose2d): self.pose = pose def initialize(self): - self.subsystem.target_stationary(Pose2d, False) + self.subsystem.target_stationary(self.pose, False) def execute(self): pass @@ -141,8 +141,8 @@ def __init__(self, subsystem: Shooter, drivetrain: CommandSwerveDrivetrain): self.shot_tuner = self.nt_inst.getTable("Shot Tuner") - self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").publish() - self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").publish() + self.hood_angle_pub = self.shot_tuner.getDoubleTopic("hood angle").publish() + self.flywheel_rps_pub = self.shot_tuner.getDoubleTopic("flywheel rps").publish() self.hood_angle_sub = self.shot_tuner.getDoubleTopic("hood angle").subscribe(20.0) self.flywheel_rps_sub = self.shot_tuner.getDoubleTopic("flywheel rps").subscribe(15.0) self.distance_pub = self.shot_tuner.getDoubleTopic("distance to hub").publish() From 550e68819aa3b17b5c54d3c7e30e14dc2a0ba635 Mon Sep 17 00:00:00 2001 From: theA-Train <146664346+theA-Train@users.noreply.github.com> Date: Sat, 21 Feb 2026 18:11:07 -0500 Subject: [PATCH 18/19] pass right pass left commands i get a type error because the drivetrain angles are not rotation2ds. I changed the drivetrain angles to be rotation2ds. in your original commented out code, you just passed the drivetrain angles as 0 and it was also red. Was setshooter like that before? --- robot_constants.py | 10 +++++++++- robotcontainer.py | 20 +++++++++++++++++--- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/robot_constants.py b/robot_constants.py index 82693fb..43c8d90 100644 --- a/robot_constants.py +++ b/robot_constants.py @@ -8,4 +8,12 @@ hub_drivetrain_angle = Rotation2d() # PLACEHOLDER hub_flywheel_velocity = 0 # PLACEHOLDER -hub_hood_angle = 0 # PLACEHOLDER \ No newline at end of file +hub_hood_angle = 0 # PLACEHOLDER + +leftpass_drivetrain_angle = Rotation2d() # PLACEHOLDER +leftpass_flywheel_velocity = 0 # PLACEHOLDER +leftpass_hood_angle = 0 # PLACEHOLDER + +rightpass_drivetrain_angle = Rotation2d() # PLACEHOLDER +rightpass_flywheel_velocity = 0 # PLACEHOLDER +rightpass_hood_angle = 0 # PLACEHOLDER \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py index 85a6a41..0eb68cb 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -126,18 +126,32 @@ def configureButtonBindings(self) -> None: # stationary commands self.tower = ParallelCommandGroup( DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), - SetShooter(self.shooter, tower_flywheel_velocity, tower_drivetrain_angle.radians()) + SetShooter(self.shooter, tower_flywheel_velocity, tower_hood_angle) ) self.hub = ParallelCommandGroup( DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), - SetShooter(self.shooter, tower_flywheel_velocity, hub_drivetrain_angle.radians()) + SetShooter(self.shooter, hub_flywheel_velocity, hub_hood_angle) ) + self.pass_right = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, rightpass_drivetrain_angle), + SetShooter(self.shooter, rightpass_flywheel_velocity, rightpass_hood_angle) + ) + + self.pass_left = ParallelCommandGroup( + DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, hub_hood_angle) + ) + + #dpad selector for stationary commands self.stationaryselector = SelectCommand( { + 0: self.pass_right, 90: self.hub, - 180: self.tower + 180: self.tower, + 270: self.pass_left + }, self.operator_controller.getHID().getPOV ) From b6dbd0a4c9a62f5bac88c94600372243f6a2bda2 Mon Sep 17 00:00:00 2001 From: e-bauman Date: Mon, 23 Feb 2026 09:34:12 -0500 Subject: [PATCH 19/19] fix shooter to use rotations instead of radians --- robot_constants.py | 8 ++-- robotcontainer.py | 60 +++++++++++------------------- subsystems/drivetrain/command.py | 18 ++++----- subsystems/drivetrain/constants.py | 12 +++--- subsystems/shooter/command.py | 4 +- subsystems/shooter/constants.py | 20 ++++++---- subsystems/shooter/subsystem.py | 45 +++++++++++----------- 7 files changed, 77 insertions(+), 90 deletions(-) diff --git a/robot_constants.py b/robot_constants.py index 43c8d90..5045ed4 100644 --- a/robot_constants.py +++ b/robot_constants.py @@ -2,18 +2,18 @@ from wpimath.geometry import Rotation2d LOGGING = True -tower_drivetrain_angle = Rotation2d() # PLACEHOLDER +tower_drivetrain_angle = Rotation2d(0) # PLACEHOLDER tower_flywheel_velocity = 0 # PLACEHOLDER tower_hood_angle = 0 # PLACEHOLDER -hub_drivetrain_angle = Rotation2d() # PLACEHOLDER +hub_drivetrain_angle = Rotation2d(0) # PLACEHOLDER hub_flywheel_velocity = 0 # PLACEHOLDER hub_hood_angle = 0 # PLACEHOLDER -leftpass_drivetrain_angle = Rotation2d() # PLACEHOLDER +leftpass_drivetrain_angle = Rotation2d(-160) # PLACEHOLDER leftpass_flywheel_velocity = 0 # PLACEHOLDER leftpass_hood_angle = 0 # PLACEHOLDER -rightpass_drivetrain_angle = Rotation2d() # PLACEHOLDER +rightpass_drivetrain_angle = Rotation2d(160) # PLACEHOLDER rightpass_flywheel_velocity = 0 # PLACEHOLDER rightpass_hood_angle = 0 # PLACEHOLDER \ No newline at end of file diff --git a/robotcontainer.py b/robotcontainer.py index 0eb68cb..4a39791 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -39,7 +39,6 @@ def __init__(self) -> None: ) # Use open-loop control for drive motors ) self._brake = swerve.requests.SwerveDriveBrake() - self._point = swerve.requests.PointWheelsAt() self._logger = Telemetry(max_speed) @@ -100,34 +99,12 @@ def configureButtonBindings(self) -> None: self.drivetrain.runOnce(self.drivetrain.seed_field_centric) ) - # Aim drivetrain and shooter - # self.driver_controller.rightTrigger().whileTrue( - # ConditionalCommand( - # ParallelCommandGroup( - # DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), - # SetShooter(self.shooter, self.driver_controller, tower_flywheel_velocity, tower_hood_angle) - # ), - # ConditionalCommand( - # ParallelCommandGroup( - # DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), - # SetShooter(self.shooter, self.driver_controller, hub_flywheel_velocity, hub_hood_angle) - # ), - # ParallelCommandGroup( - # AimDrivetrain(self.drivetrain, self.driver_controller), - # AimShooter(self.shooter, self.drivetrain) - # ), - # lambda: self.operator_controller.getHID().getPOV() == 0 - # ), - # lambda: self.operator_controller.getHID().getPOV() == 180 - # ) - # ) - - # stationary commands + self.tower = ParallelCommandGroup( - DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), - SetShooter(self.shooter, tower_flywheel_velocity, tower_hood_angle) - ) + DriveAtAngle(self.drivetrain, self.driver_controller, tower_drivetrain_angle), + SetShooter(self.shooter, tower_flywheel_velocity, tower_hood_angle) + ) self.hub = ParallelCommandGroup( DriveAtAngle(self.drivetrain, self.driver_controller, hub_drivetrain_angle), @@ -144,18 +121,24 @@ def configureButtonBindings(self) -> None: SetShooter(self.shooter, tower_flywheel_velocity, hub_hood_angle) ) - #dpad selector for stationary commands - self.stationaryselector = SelectCommand( - { - 0: self.pass_right, - 90: self.hub, - 180: self.tower, - 270: self.pass_left - - }, - self.operator_controller.getHID().getPOV - ) + # aim drivetrain and shooter based on operator input + # self.driver_controller.rightTrigger().onTrue( + # SelectCommand( + # { + # 0: self.hub, + # 90: self.pass_right, + # 180: self.tower, + # 270: self.pass_left, + # -1: ParallelCommandGroup( + # AimShooter(self.shooter, self.drivetrain), + # AimDrivetrain(self.drivetrain, self.driver_controller) + # ) + # }, + # self.operator_controller.getHID().getPOV + # ) + # ) + # command used to tune the shooter by taking in a value from networktables self.driver_controller.rightTrigger().onTrue( ParallelCommandGroup( TuneShooter(self.shooter, self.drivetrain), @@ -167,6 +150,7 @@ def configureButtonBindings(self) -> None: RunIndexer(self.indexer) ) + # drive in "snake mode" (intake faces direction of travel) self.driver_controller.rightBumper().whileTrue( SnakeMode(self.drivetrain, self.driver_controller) ) diff --git a/subsystems/drivetrain/command.py b/subsystems/drivetrain/command.py index ec75ea4..e8973c3 100644 --- a/subsystems/drivetrain/command.py +++ b/subsystems/drivetrain/command.py @@ -17,9 +17,9 @@ def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.but self.controller = controller self._brake = swerve.requests.SwerveDriveBrake() self._aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( - aiming_pid_p, - aiming_pid_i, - aiming_pid_d + aiming_kP, + aiming_kI, + aiming_kD ) self.addRequirements(self.drivetrain) @@ -80,9 +80,9 @@ def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.but self.controller = controller self._brake = swerve.requests.SwerveDriveBrake() self._aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( - aiming_pid_p, - aiming_pid_i, - aiming_pid_d + aiming_kP, + aiming_kI, + aiming_kD ) self.addRequirements(self.drivetrain) @@ -131,9 +131,9 @@ def __init__(self, subsystem: CommandSwerveDrivetrain, controller: commands2.but self.drivetrain = subsystem self.controller = controller self.aim_at = swerve.requests.FieldCentricFacingAngle().with_heading_pid( - snake_mode_pid_p, - snake_mode_pid_i, - snake_mode_pid_d + snake_mode_kP, + snake_mode_kI, + snake_mode_kD ) self.drive = swerve.requests.FieldCentric().with_drive_request_type( swerve.SwerveModule.DriveRequestType.OPEN_LOOP_VOLTAGE diff --git a/subsystems/drivetrain/constants.py b/subsystems/drivetrain/constants.py index 1c8873a..70649dc 100644 --- a/subsystems/drivetrain/constants.py +++ b/subsystems/drivetrain/constants.py @@ -14,13 +14,13 @@ curve = 2 # curve exponent for controller inputs #heading pid tolerance ALL PLACEHOLDERS -aiming_pid_p = 10 -aiming_pid_i = 0 -aiming_pid_d = 0 +aiming_kP = 10 +aiming_kI = 0 +aiming_kD = 0 -snake_mode_pid_p = 10 -snake_mode_pid_i = 0 -snake_mode_pid_d = 0 +snake_mode_kP = 10 +snake_mode_kI = 0 +snake_mode_kD = 0 drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees drivetrain_shooting_velocity_tolerance = 0.1 # PLACEHOLDER, percentage (0 to 1) \ No newline at end of file diff --git a/subsystems/shooter/command.py b/subsystems/shooter/command.py index 02b99ea..ddbb886 100644 --- a/subsystems/shooter/command.py +++ b/subsystems/shooter/command.py @@ -103,7 +103,7 @@ class SetShooter(commands2.Command): Args: velocity (rotations per second): desired left and right flywheel velocity - angle (radians): desired hood angle + angle (rotations): desired hood angle """ def __init__(self, subsystem: Shooter, velocity: units.rotations_per_second, angle: units.radian): @@ -150,7 +150,7 @@ def __init__(self, subsystem: Shooter, drivetrain: CommandSwerveDrivetrain): def initialize(self): self.subsystem.set_left_target_velocity(self.flywheel_rps_sub.get()) self.subsystem.set_right_target_velocity(self.flywheel_rps_sub.get()) - self.subsystem.set_hood_angle(self.hood_angle_sub.get()) + self.subsystem.set_hood_angle(self.hood_angle_sub.get() / 360) def execute(self): self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose())) diff --git a/subsystems/shooter/constants.py b/subsystems/shooter/constants.py index 53844de..f02dc75 100644 --- a/subsystems/shooter/constants.py +++ b/subsystems/shooter/constants.py @@ -1,4 +1,4 @@ -from phoenix6 import hardware, configs, signals +from phoenix6 import hardware, configs, signals, units from wpimath.geometry import Translation2d import wpilib import numpy as np @@ -10,14 +10,14 @@ hood_id: int = 62 # placeholder hood_cancoder_id: int = 57 #placeholder -flywheel_threshold = 2.0 # placeholder -hood_threshold = 2.0 # placeholder +flywheel_velocity_threshold: units.rotations_per_second = 4.0 # placeholder +hood_angle_threshold: units.rotation = 1.0 / 360# placeholder hood_gear_ratio = 69 # 69:1 -max_hood_angle = 43 # placeholder -min_hood_angle = 0 # placeholder +max_hood_angle: units.rotation = 43 / 360 # placeholder +min_hood_angle: units.rotation = 10 / 360 -idle_velocity = 0 # placeholder +idle_velocity: units.rotations_per_second = 0 # placeholder NT_SHOOTER: bool = True @@ -42,6 +42,9 @@ .with_k_s(0) # placeholder .with_k_v(0) # placeholder .with_k_a(0) # placeholder +).with_current_limits( + configs.CurrentLimitsConfigs() + .with_stator_current_limit(80) # placeholder ) hood_config = configs.TalonFXConfiguration().with_motor_output( @@ -69,7 +72,10 @@ .with_sensor_to_mechanism_ratio(2) .with_feedback_sensor_source(signals.FeedbackSensorSourceValue.FUSED_CANCODER) .with_feedback_remote_sensor_id(hood_cancoder_id) -) +).with_current_limits( + configs.CurrentLimitsConfigs() + .with_stator_current_limit(60) # placeholder +) def load_shooter_table_csv(rel_path: str) -> np.ndarray: diff --git a/subsystems/shooter/subsystem.py b/subsystems/shooter/subsystem.py index 4a756a4..15b316d 100644 --- a/subsystems/shooter/subsystem.py +++ b/subsystems/shooter/subsystem.py @@ -51,7 +51,7 @@ def __init__(self): self.hood_target_angle_pub = self.table.getDoubleTopic("hood target angle").publish() self.shooter_ready_pub = self.table.getDoubleTopic("shooter ready").publish() - def set_left_target_velocity(self, velocity: float): + def set_left_target_velocity(self, velocity: units.rotations_per_second): """ sets the target velocity of the left flywheel @@ -62,7 +62,7 @@ def set_left_target_velocity(self, velocity: float): self.left_leader_motor.set_control(self.velocity_torque_current.with_velocity(self.left_target_velocity)) - def set_right_target_velocity(self, velocity: float): + def set_right_target_velocity(self, velocity: units.rotations_per_second): """ sets the target velocity of the right flywheel @@ -73,7 +73,7 @@ def set_right_target_velocity(self, velocity: float): self.right_leader_motor.set_control(self.velocity_torque_current.with_velocity(self.right_target_velocity)) - def get_left_velocity(self): + def get_left_velocity(self) -> units.rotations_per_second: """ obtains the current velocity of the left flywheel @@ -82,7 +82,7 @@ def get_left_velocity(self): """ return self.left_leader_motor.get_velocity().value_as_double - def get_right_velocity(self): + def get_right_velocity(self) -> units.rotations_per_second: """ obtains the current velocity of the right flywheel @@ -91,7 +91,7 @@ def get_right_velocity(self): """ return self.right_leader_motor.get_velocity().value_as_double - def left_is_at_velocity(self, velocity: float): + def left_is_at_velocity(self, velocity: units.rotations_per_second): """ checks if the left flywheel is at a certain velocity @@ -101,9 +101,9 @@ def left_is_at_velocity(self, velocity: float): Returns: boolean: whether or not the left flywheel is at the velocity (true it is and false it isn't) """ - return abs(self.get_left_velocity() - velocity) < flywheel_threshold + return abs(self.get_left_velocity() - velocity) < flywheel_velocity_threshold - def right_is_at_velocity(self, velocity: float): + def right_is_at_velocity(self, velocity: units.rotations_per_second): """ checks if the right flywheel is at a certain velocity @@ -113,42 +113,39 @@ def right_is_at_velocity(self, velocity: float): Returns: boolean: whether or not the right flywheel is at the velocity (true it is and false it isn't) """ - return abs(self.get_right_velocity() - velocity) < flywheel_threshold + return abs(self.get_right_velocity() - velocity) < flywheel_velocity_threshold - def set_hood_angle(self, angle: float): + def set_hood_angle(self, angle: units.rotation): """ brings the hood to given angle Args: - angle (radians): intended hood angle in radians + angle (rotations): intended hood angle in rotations """ self.hood_target_angle = max(min_hood_angle, min(angle, max_hood_angle)) - - rotations = self.hood_target_angle / (2 * math.pi) - - self.hood_motor.set_control(self.motion_magic.with_position(rotations)) + self.hood_motor.set_control(self.motion_magic.with_position(self.hood_target_angle)) def get_hood_angle(self): """ obtains the current position of the hood Returns: - return_float: current hood position in radians + return_float: current hood position in rotations """ return self.hood_motor.get_position() - def hood_is_at_angle(self, angle: float): + def hood_is_at_angle(self, angle: units.rotation): """ checks if the right hood is at a certain angle Args: - angle (radians): angle to be checked + angle (rotations): angle to be checked Returns: boolean: whether or not the hood is at the angle (true it is and false it isn't) """ - return abs(self.get_hood_angle().value - angle) < hood_threshold + return abs(self.get_hood_angle().value - angle) < hood_angle_threshold def ready_to_shoot(self): """ @@ -166,11 +163,11 @@ def target_stationary(self, robot_pose: Pose2d, passing: bool): :param robot_pose: robot's pose on the field """ if passing: - hood_deg, rps = shooter_utils.pass_setpoints_from_pose(robot_pose) + hood_pos, rps = shooter_utils.pass_setpoints_from_pose(robot_pose) else: - hood_deg, rps = shooter_utils.shot_setpoints_from_pose(robot_pose) + hood_pos, rps = shooter_utils.shot_setpoints_from_pose(robot_pose) - self.set_hood_angle(math.radians(hood_deg)) + self.set_hood_angle(hood_pos) self.set_left_target_velocity(rps) self.set_right_target_velocity(rps) @@ -181,10 +178,10 @@ def update_table(self): self.right_velocity_pub.set(self.get_right_velocity()) self.left_target_velocity_pub.set(self.left_target_velocity) self.right_target_velocity_pub.set(self.right_target_velocity) - self.hood_angle_pub.set(self.get_hood_angle().value) - self.hood_target_angle_pub.set(self.hood_target_angle) + self.hood_angle_pub.set(self.get_hood_angle().value * 360) + self.hood_target_angle_pub.set(self.hood_target_angle * 360) self.shooter_ready_pub.set(self.ready_to_shoot()) def periodic(self): if NT_SHOOTER: - self.update_table \ No newline at end of file + self.update_table() \ No newline at end of file