Feat/shooter

robot_constants.py

@@ -1 +1,19 @@
-LOGGING = True
+
+from wpimath.geometry import Rotation2d
+LOGGING = True
+
+tower_drivetrain_angle = Rotation2d(0) # PLACEHOLDER
+tower_flywheel_velocity = 0 # PLACEHOLDER
+tower_hood_angle = 0 # PLACEHOLDER
+
+hub_drivetrain_angle = Rotation2d(0) # PLACEHOLDER
+hub_flywheel_velocity = 0 # PLACEHOLDER
+hub_hood_angle = 0 # PLACEHOLDER
+
+leftpass_drivetrain_angle = Rotation2d(-160) # PLACEHOLDER
+leftpass_flywheel_velocity = 0 # PLACEHOLDER
+leftpass_hood_angle = 0 # PLACEHOLDER
+
+rightpass_drivetrain_angle = Rotation2d(160) # PLACEHOLDER
+rightpass_flywheel_velocity = 0 # PLACEHOLDER
+rightpass_hood_angle = 0 # PLACEHOLDER

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, SelectCommand
 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
@@ -38,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)
 
@@ -99,20 +99,62 @@ def configureButtonBindings(self) -> None:
             self.drivetrain.runOnce(self.drivetrain.seed_field_centric)
         )
 
-        # Aim drivetrain and shooter
-        self.driver_controller.rightTrigger().whileTrue(
+        # stationary commands
+
+        self.tower = ParallelCommandGroup(
+            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),
+            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)
+        )            
+
+        # 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(
-                AimDrivetrain(self.drivetrain, self.driver_controller),
-                AimShooter(self.shooter, self.drivetrain)
+                TuneShooter(self.shooter, self.drivetrain),
+                AimDrivetrain(self.drivetrain, self.driver_controller)
             )
         )
 
+        Trigger(lambda: self.drivetrain.ready_to_shoot and self.shooter.ready_to_shoot()).whileTrue(
+            RunIndexer(self.indexer)
+        )
+
+        # drive in "snake mode" (intake faces direction of travel)
         self.driver_controller.rightBumper().whileTrue(
             SnakeMode(self.drivetrain, self.driver_controller)
         )
 
-
-
         # force the indexer to spin
         self.operator_controller.a().or_(self.driver_controller.a()).whileTrue(
             RunIndexer(self.indexer)

subsystems/drivetrain/__init__.py

@@ -1,3 +1,3 @@
 from .command_swerve_drivetrain import CommandSwerveDrivetrain
-from .command import AimDrivetrain, SnakeMode
+from .command import AimDrivetrain, DriveAtAngle, SnakeMode
 from .constants import max_angular_rate, max_speed, deadband, curve

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)
@@ -52,16 +52,71 @@ 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
+
+    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_kP,
+            aiming_kI,
+            aiming_kD
+        )
+
+        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
@@ -76,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

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
 

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
+drive_at_angle_tolerance: units.radian = 0.05 #placeholder value, ~2.86 degrees
+drivetrain_shooting_velocity_tolerance = 0.1 # PLACEHOLDER, percentage (0 to 1)

subsystems/shooter/__init__.py

@@ -1,2 +1,2 @@
 from .subsystem import Shooter
-from .command import AimShooter, SetShooterAuto, SetShooterIdle
+from .command import AimShooter, SetShooterAuto, SetShooterIdle, TuneShooter, SetShooter

subsystems/shooter/command.py

@@ -2,8 +2,11 @@
 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
+
+from ntcore import NetworkTableInstance
 
 class AimShooter(commands2.Command):
     """
@@ -39,6 +42,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.
@@ -57,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
@@ -89,5 +93,70 @@ 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 (rotations): desired hood angle
+    """
+
+    def __init__(self, subsystem: Shooter, velocity: units.rotations_per_second, angle: units.radian):
+        super().__init__()
+
+        self.subsystem = subsystem
+        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
+
+
+class TuneShooter(commands2.Command):
+    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_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()
+
+    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() / 360)
+
+    def execute(self):
+        self.distance_pub.set(shooter_utils.shot_distance_from_pose(self.drivetrain.get_pose()))
+
+    def isFinished(self):
+        return False
+
     def end(self, interrupted):
         pass