Update autonomous

autonomous/autonomous.py

@@ -3,6 +3,25 @@
 from wpimath.geometry import Pose2d, Translation2d
 
 
+class PodiumSpeakerAmpTopcentre(AutoBase):
+    MODE_NAME = "5 notes: internal, podium, speaker, amp, top centre"
+
+    def __init__(self) -> None:
+        note_paths = [
+            Path([NotePositions.podium_NW]),
+            Path([NotePositions.speaker]),
+            Path([NotePositions.amp]),
+            Path([PathPositions.avoid_wall, NotePositions.Centre1]),
+        ]
+        shoot_paths = [
+            Path([ShootingPositions.close_straight]),
+            Path([ShootingPositions.amp_speaker_bounce]),
+            Path([NotePositions.amp]),
+            Path([PathPositions.avoid_wall, NotePositions.amp]),
+        ]
+        super().__init__(note_paths, shoot_paths)
+
+
 class PodiumSpeakerAmp(AutoBase):
     MODE_NAME = "4 notes: internal, podium, speaker, amp"
 

autonomous/base.py

@@ -1,6 +1,7 @@
 import math
 from typing import Optional
 from magicbot.state_machine import AutonomousStateMachine, state
+from magicbot import feedback
 from wpimath.trajectory import (
     TrajectoryConfig,
     Trajectory,
@@ -19,6 +20,7 @@
 
 from utilities.position import Path
 import utilities.game as game
+from utilities.game import get_goal_speaker_position
 
 from components.chassis import ChassisComponent
 from components.intake import IntakeComponent
@@ -34,9 +36,10 @@ class AutoBase(AutonomousStateMachine):
     intake_component: IntakeComponent
 
     POSITION_TOLERANCE = 0.05
+    SHOOTING_POSITION_TOLERANCE = 0.5
     ANGLE_TOLERANCE = math.radians(5)
-    MAX_VEL = 3
-    MAX_ACCEL = 2
+    MAX_VEL = 4
+    MAX_ACCEL = 3
     ENFORCE_HEADING_SPEED = MAX_VEL / 6
 
     def __init__(
@@ -51,8 +54,8 @@ def __init__(
         self.starting_pose = starting_pose
 
     def setup(self) -> None:
-        x_controller = PIDController(2.5, 0, 0)
-        y_controller = PIDController(2.5, 0, 0)
+        x_controller = PIDController(3.5, 0, 0.4)
+        y_controller = PIDController(3.5, 0, 0.4)
         heading_controller = self.chassis.heading_controller
 
         self.drive_controller = HolonomicDriveController(
@@ -68,6 +71,7 @@ def setup(self) -> None:
 
         self.goal_heading: Rotation2d
         self.trajectory_marker = self.field.getObject("auto_trajectory")
+        self.trajectory: Optional[Trajectory] = None
 
     def on_enable(self):
         # Setup starting position in the simulator
@@ -78,6 +82,15 @@ def on_enable(self):
             self.chassis.set_pose(starting_pose)
         super().on_enable()
 
+    @feedback
+    def is_close_enough_to_shoot(self) -> bool:
+        if self.trajectory:
+            last = self.trajectory.sample(self.trajectory.totalTime())
+            return (
+                last.pose.translation() - self.chassis.get_pose().translation()
+            ).norm() < self.SHOOTING_POSITION_TOLERANCE
+        return False
+
     @state(first=True)
     def initialise(self) -> None:
         # Make a working copy of the NotePaths so that we can pop
@@ -119,43 +132,43 @@ def pick_up(self, state_tm: float, initial_call: bool) -> None:
         # Drive with the intake always facing the tangent
         self.drive_on_trajectory(state_tm, enforce_tangent_heading=True)
 
-        if self.note_manager.has_note():
+        if self.note_manager.has_note() or self.is_at_goal():
             # Check if we have a note collected
             # Return heading control to path controller
             self.chassis.stop_snapping()
-            self.next_state("drive_to_shoot")
-        if self.is_at_goal():
-            if RobotBase.isSimulation():
-                self.next_state(self.drive_to_shoot)
-                return
-            # we did not find a note on the path, look for the next note
-            if len(self.note_paths_working_copy) == 0:
-                # Couldn't find the last note
-                self.done()
-                return
-            self.shoot_paths_working_copy.pop(0)
-            # reset the clock
-            self.next_state(self.pick_up)
+            self.next_state(self.drive_and_shoot)
+
+    def translation_to_goal(self, position: Translation2d) -> Translation2d:
+        return get_goal_speaker_position().toTranslation2d() - position
 
     @state
-    def drive_to_shoot(self, state_tm: float, initial_call: bool) -> None:
+    def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None:
         if initial_call:
             self.trajectory = self.calculate_trajectory(
                 self.shoot_paths_working_copy.pop(0)
             )
 
-        self.note_manager.try_cancel_intake()
-
         # Do some driving...
         self.drive_on_trajectory(state_tm)
 
-        if self.is_at_goal():
-            # If we are in position, remove this note from the list and shoot it
-            self.next_state("shoot_note")
+        # And maybe some shooting...
+        if self.is_close_enough_to_shoot():
+            self.note_manager.try_shoot()
+
+        if self.note_manager.has_just_fired() or (
+            self.is_at_goal() and not self.note_manager.has_note()
+        ):
+            if len(self.shoot_paths_working_copy) != 0:
+                self.next_state("pick_up")
+            else:
+                self.done()
 
     def drive_on_trajectory(
         self, trajectory_tm: float, enforce_tangent_heading: bool = False
     ):
+        if not self.trajectory:
+            return
+
         # Grabbing the target position at the current point in time from the trajectory.
         target_state = self.trajectory.sample(trajectory_tm)
 
@@ -168,7 +181,7 @@ def drive_on_trajectory(
         self.chassis.drive_local(
             chassis_speed.vx,
             chassis_speed.vy,
-            chassis_speed.omega,
+            0,
         )
 
         # if we are enforcing heading, hijack rotational control from the main controller

components/chassis.py

@@ -235,7 +235,7 @@ def __init__(self) -> None:
 
         self.on_red_alliance = False
 
-        self.modules = [
+        self.modules = (
             # Front Left
             SwerveModule(
                 self.WHEEL_BASE / 2,
@@ -268,7 +268,7 @@ def __init__(self) -> None:
                 TalonIds.steer_4,
                 CancoderIds.swerve_4,
             ),
-        ]
+        )
 
         self.kinematics = SwerveDrive4Kinematics(
             self.modules[0].translation,
@@ -291,6 +291,21 @@ def __init__(self) -> None:
 
         wpilib.SmartDashboard.putData("Heading PID", self.heading_controller)
 
+    def get_velocity(self) -> ChassisSpeeds:
+        return self.kinematics.toChassisSpeeds(self.get_module_states())
+
+    def get_module_states(
+        self,
+    ) -> tuple[
+        SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState
+    ]:
+        return (
+            self.modules[0].get(),
+            self.modules[1].get(),
+            self.modules[2].get(),
+            self.modules[3].get(),
+        )
+
     def setup(self) -> None:
         initial_pose = TeamPoses.RED_TEST_POSE if is_red() else TeamPoses.BLUE_TEST_POSE
 

components/shooter.py

@@ -117,12 +117,16 @@ def __init__(self) -> None:
         flywheel_right_config.apply(flywheel_pid)
         flywheel_right_config.apply(flywheel_gear_ratio)
 
-        self.inclinator_controller = PIDController(0.8, 0, 0.05)
+        self.inclinator_controller = PIDController(5.0, 0, 0)
         self.inclinator_controller.setTolerance(ShooterComponent.INCLINATOR_TOLERANCE)
         SmartDashboard.putData(self.inclinator_controller)
 
     range = tunable(0.0)
 
+    @feedback
+    def get_applied_output(self) -> float:
+        return self.inclinator.getAppliedOutput()
+
     def on_enable(self) -> None:
         self.inclinator_controller.reset()
 

controllers/note.py

@@ -87,11 +87,13 @@ def not_holding_note(self, initial_call) -> None:
         if self.intake_desired:
             self.shooter.update_range()
             self.intake.engage()
+        elif wpilib.DriverStation.isAutonomous():
+            self.shooter.update_range()
 
-        elif self.cancel_intake_desired:
+        if self.cancel_intake_desired:
             self.intake.try_cancel_intake()
 
-        elif self.has_note():
+        if self.has_note():
             self.next_state(self.holding_note)
 
     @state(must_finish=True)

controllers/shooter.py

@@ -2,7 +2,7 @@
 
 from wpimath.geometry import Translation2d
 
-from magicbot import StateMachine, state, timed_state, feedback
+from magicbot import StateMachine, state, timed_state, feedback, tunable
 
 from components.chassis import ChassisComponent
 from components.intake import IntakeComponent
@@ -18,9 +18,8 @@ class Shooter(StateMachine):
     intake_component: IntakeComponent
     status_lights: LightStrip
 
-    # make sure this is always > chassis heading tolerance
-    ANGLE_TOLERANCES = (math.radians(5), math.radians(1))
-    RANGES = (0, 5)
+    SPEED_LIMIT = tunable(1)
+    SPINNING_SPEED_LIMIT = tunable(1)
 
     def __init__(self):
         self.range = 0.0
@@ -65,11 +64,20 @@ def aiming(self, initial_call) -> None:
                 self.is_aiming_finished()
                 and self.shooter_component.is_ready()
                 and self.in_range()
+                and self.is_below_speed_limit()
+                and self.is_below_spinning_limit()
             ):
                 self.next_state(self.firing)
             else:
                 self.aim()
 
+    def is_below_speed_limit(self) -> bool:
+        vel = self.chassis.get_velocity()
+        return math.hypot(vel.vx, vel.vy) < self.SPEED_LIMIT
+
+    def is_below_spinning_limit(self) -> bool:
+        return self.chassis.get_velocity().omega < self.SPINNING_SPEED_LIMIT
+
     def aim(self) -> None:
         # Update range
         self.update_range()
@@ -78,7 +86,7 @@ def aim(self) -> None:
         translation_to_goal = self.translation_to_goal()
 
         # We need to aim at least a note's radius inside the outer bounds of the goal. Also add a safety margin
-        margin = 0.05
+        margin = 0.10
         offset = (SPEAKER_HOOD_WIDTH - NOTE_DIAMETER) / 2.0 - margin
         offset_bearing = constrain_angle(
             math.atan2(translation_to_goal.y + offset, translation_to_goal.x) + math.pi

physics.py

@@ -70,7 +70,9 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
         self.physics_controller = physics_controller
 
         self.kinematics: SwerveDrive4Kinematics = robot.chassis.kinematics
-        self.swerve_modules: list[SwerveModule] = robot.chassis.modules
+        self.swerve_modules: tuple[
+            SwerveModule, SwerveModule, SwerveModule, SwerveModule
+        ] = robot.chassis.modules
 
         # Motors
         self.wheels = [

utilities/position.py

@@ -14,6 +14,9 @@ def __init__(self, waypoints: list[Translation2d]):
         self.final_heading = Rotation2d(0)
 
 
+stage_tolerance = 0.35
+
+
 class NotePositions:
     # These are the 3 close notes, named for the nearest element
     amp = Translation2d(13.645, 7.00045)
@@ -29,9 +32,13 @@ class NotePositions:
 
     # The podium note is very close to the stage leg so we need different positions to avoid collisions
     # Directions are in the field coordinate system for red side
-    podium_NW = podium + Translation2d(0.5, 0.5)
-    podium_NE = podium + Translation2d(0.5, -0.5)
-    podium_N = podium + Translation2d(0.5, 0)
+    podium_N = podium + Translation2d(stage_tolerance, 0)
+    podium_NW = podium + Translation2d(
+        stage_tolerance / math.sqrt(2), stage_tolerance / math.sqrt(2)
+    )
+    podium_NE = podium + Translation2d(
+        stage_tolerance / math.sqrt(2), -stage_tolerance / math.sqrt(2)
+    )
 
 
 class ShootingPositions: