Merge pull request #185 from thedropbears/auto-refeactor

Auto refactor

autonomous/autonomous.py

@@ -1,23 +1,36 @@
-from utilities.position import NotePositions, Path, ShootingPositions, PathPositions
-from autonomous.base import AutoBase, rotation_to_red_speaker
-from wpimath.geometry import Pose2d, Translation2d
+import math
+
+from utilities.position import (
+    NotePositions,
+    Path,
+    ShootingPositions,
+    PathPositions,
+)
+from utilities import game
+from autonomous.base import AutoBase
+from wpimath.geometry import Pose2d, Translation2d, Rotation2d
+
+
+def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
+    t = game.RED_SPEAKER_POSE.toPose2d().translation() - position
+    return t.angle() + Rotation2d(math.pi)
 
 
 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]),
+            Path([NotePositions.podium_NW], face_target=False),
+            Path([NotePositions.speaker], face_target=False),
+            Path([NotePositions.amp], face_target=False),
+            Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False),
         ]
         shoot_paths = [
-            Path([ShootingPositions.close_straight]),
-            Path([ShootingPositions.amp_speaker_bounce]),
-            Path([NotePositions.amp]),
-            Path([PathPositions.avoid_wall, NotePositions.amp]),
+            Path([ShootingPositions.close_straight], face_target=True),
+            Path([ShootingPositions.amp_speaker_bounce], face_target=True),
+            Path([NotePositions.amp], face_target=True),
+            Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
         super().__init__(note_paths, shoot_paths)
 
@@ -27,14 +40,14 @@ class PodiumSpeakerAmp(AutoBase):
 
     def __init__(self) -> None:
         note_paths = [
-            Path([NotePositions.podium_NW]),
-            Path([NotePositions.speaker]),
-            Path([NotePositions.amp]),
+            Path([NotePositions.podium_NW], face_target=False),
+            Path([NotePositions.speaker], face_target=False),
+            Path([NotePositions.amp], face_target=False),
         ]
         shoot_paths = [
-            Path([ShootingPositions.close_straight]),
-            Path([ShootingPositions.amp_speaker_bounce]),
-            Path([NotePositions.amp]),
+            Path([ShootingPositions.close_straight], face_target=True),
+            Path([ShootingPositions.amp_speaker_bounce], face_target=True),
+            Path([NotePositions.amp], face_target=True),
         ]
         # Start pose only needs to be on the correct half of the field,
         # so choose the podium as a reference point
@@ -50,12 +63,12 @@ class AmpCentre1(AutoBase):
 
     def __init__(self) -> None:
         note_paths = [
-            Path([NotePositions.amp]),
-            Path([PathPositions.avoid_wall, NotePositions.Centre1]),
+            Path([NotePositions.amp], face_target=False),
+            Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False),
         ]
         shoot_paths = [
-            Path([NotePositions.amp]),
-            Path([PathPositions.avoid_wall, NotePositions.amp]),
+            Path([NotePositions.amp], face_target=True),
+            Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
         # Start pose only needs to be on the correct half of the field,
         # so choose the amp as a reference point
@@ -71,12 +84,18 @@ class SpeakerCentre3(AutoBase):
 
     def __init__(self) -> None:
         note_paths = [
-            Path([NotePositions.speaker]),
-            Path([PathPositions.stage_transition_N, NotePositions.Centre3]),
+            Path([NotePositions.speaker], face_target=False),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.Centre3],
+                face_target=False,
+            ),
         ]
         shoot_paths = [
-            Path([NotePositions.speaker]),
-            Path([PathPositions.stage_transition_N, NotePositions.speaker]),
+            Path([NotePositions.speaker], face_target=True),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.speaker],
+                face_target=True,
+            ),
         ]
         # Start pose only needs to be on the correct half of the field,
         # so choose the speaker as a reference point
@@ -99,9 +118,10 @@ def __init__(self) -> None:
                     PathPositions.stage_transition_S_entry,
                     PathPositions.stage_transition_S,
                     NotePositions.Centre3,
-                ]
+                ],
+                face_target=False,
             ),
-            Path([NotePositions.Centre5]),
+            Path([NotePositions.Centre5], face_target=False),
         ]
 
         shoot_paths = [
@@ -110,9 +130,10 @@ def __init__(self) -> None:
                     PathPositions.stage_transition_S,
                     PathPositions.stage_transition_S_entry,
                     ShootingPositions.source_side,
-                ]
+                ],
+                face_target=True,
             ),
-            Path([ShootingPositions.source_side]),
+            Path([ShootingPositions.source_side], face_target=True),
         ]
         sim_start_pos = Translation2d(15.4, 2.94)
         rotation = rotation_to_red_speaker(sim_start_pos)

autonomous/base.py

@@ -20,7 +20,6 @@
 
 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
@@ -36,7 +35,7 @@ class AutoBase(AutonomousStateMachine):
     intake_component: IntakeComponent
 
     POSITION_TOLERANCE = 0.05
-    SHOOTING_POSITION_TOLERANCE = 0.5
+    SHOOTING_POSITION_TOLERANCE = 1
     ANGLE_TOLERANCE = math.radians(5)
     MAX_VEL = 4
     MAX_ACCEL = 3
@@ -64,12 +63,7 @@ def setup(self) -> None:
         # Since robot is stationary from one action to another, point the control vector at the goal to avoid the robot taking unnecessary turns before moving towards the goal
         self.kD = 0.3
 
-        for i, path in enumerate(self.shoot_paths):
-            self.shoot_paths[i].final_heading = rotation_to_red_speaker(
-                path.waypoints[-1]
-            )
-
-        self.goal_heading: Rotation2d
+        self.goal_heading: float
         self.trajectory_marker = self.field.getObject("auto_trajectory")
         self.trajectory: Optional[Trajectory] = None
 
@@ -136,17 +130,14 @@ def pick_up(self, state_tm: float, initial_call: bool) -> None:
         self.note_manager.try_intake()
 
         # Drive with the intake always facing the tangent
-        self.drive_on_trajectory(state_tm, enforce_tangent_heading=True)
+        self.drive_on_trajectory(state_tm)
 
         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(self.drive_and_shoot)
 
-    def translation_to_goal(self, position: Translation2d) -> Translation2d:
-        return get_goal_speaker_position().toTranslation2d() - position
-
     @state
     def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None:
         if initial_call:
@@ -169,9 +160,7 @@ def drive_and_shoot(self, state_tm: float, initial_call: bool) -> None:
             else:
                 self.done()
 
-    def drive_on_trajectory(
-        self, trajectory_tm: float, enforce_tangent_heading: bool = False
-    ):
+    def drive_on_trajectory(self, trajectory_tm: float):
         if not self.trajectory:
             return
 
@@ -182,24 +171,17 @@ def drive_on_trajectory(
         chassis_speed = self.drive_controller.calculate(
             self.chassis.get_pose(),
             target_state,
-            self.goal_heading,
+            Rotation2d(self.goal_heading),
         )
         self.chassis.drive_local(
             chassis_speed.vx,
             chassis_speed.vy,
             0,
         )
 
-        # if we are enforcing heading, hijack rotational control from the main controller
-        if enforce_tangent_heading:
-            speed = Translation2d(chassis_speed.vx, chassis_speed.vy).norm()
-            if speed > self.ENFORCE_HEADING_SPEED:
-                field_chassis_speeds = self.chassis.to_field_oriented(chassis_speed)
-                heading_target = math.atan2(
-                    field_chassis_speeds.vy, field_chassis_speeds.vx
-                )
-                self.goal_heading = Rotation2d(heading_target)
-                self.chassis.snap_to_heading(heading_target)
+        # let aiming override path headings
+        if not self.note_manager.shooter.is_executing:
+            self.chassis.snap_to_heading(self.goal_heading)
 
     def calculate_trajectory(self, path: Path) -> Trajectory:
         pose = self.chassis.get_pose()
@@ -215,7 +197,7 @@ def calculate_trajectory(self, path: Path) -> Trajectory:
                 for waypoint in path.waypoints[:-1]
             ]
             self.goal = game.field_flip_translation2d(path.waypoints[-1])
-            self.goal_heading = game.field_flip_rotation2d(path.final_heading)
+            self.goal_heading = game.field_flip_angle(path.final_heading)
 
         traj_config = TrajectoryConfig(
             maxVelocity=self.MAX_VEL, maxAcceleration=self.MAX_ACCEL
@@ -256,6 +238,18 @@ def calculate_trajectory(self, path: Path) -> Trajectory:
         except Exception:
             return Trajectory([Trajectory.State(0, 0, 0, pose)])
 
+        # face along final path leg if we are not trying to shoot
+        if not path.face_target:
+            waypoints = path.waypoints
+            endpoint = waypoints[-1]
+            # second last pose might be our our current pose
+            second_last = waypoints[-2] if len(waypoints) > 1 else pose.translation()
+            disp = endpoint - second_last
+            if not game.is_red():
+                disp = game.field_flip_translation2d(disp)
+            heading_target = math.atan2(disp.y, disp.x)
+            self.goal_heading = heading_target
+
         self.trajectory_marker.setTrajectory(trajectory)
         return trajectory
 
@@ -268,8 +262,3 @@ def done(self):
         self.chassis.stop_snapping()
         self.trajectory_marker.setPoses([])
         super().done()
-
-
-def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
-    t = game.RED_SPEAKER_POSE.toPose2d().translation() - position
-    return t.angle() + Rotation2d(math.pi)

utilities/game.py

@@ -1,3 +1,4 @@
+import math
 import typing
 
 import robotpy_apriltag
@@ -58,6 +59,10 @@ def field_flip_rotation2d(r: Rotation2d):
     return Rotation2d(-r.cos(), r.sin())
 
 
+def field_flip_angle(r: float):
+    return math.atan2(math.sin(math.pi - r), math.cos(math.pi - r))
+
+
 def field_flip_translation2d(t: Translation2d):
     return Translation2d(FIELD_LENGTH - t.x, t.y)
 
@@ -72,3 +77,7 @@ def get_goal_speaker_position() -> Translation3d:
         return RED_SPEAKER_POSE.translation()
 
     return BLUE_SPEAKER_POSE.translation()
+
+
+def translation_to_goal(position: Translation2d) -> Translation2d:
+    return get_goal_speaker_position().toTranslation2d() - position

utilities/position.py

@@ -2,16 +2,36 @@
 
 from wpimath.geometry import Rotation2d, Translation2d, Pose2d
 
-from utilities.game import RED_SPEAKER_POSE, field_flip_pose2d
+from utilities.game import (
+    RED_SPEAKER_POSE,
+    BLUE_SPEAKER_POSE,
+    field_flip_pose2d,
+    field_flip_translation2d,
+    field_flip_angle,
+)
 
 
 class Path:
     waypoints: list[Translation2d]
-    final_heading: Rotation2d
+    final_heading: float
+    face_target: bool
 
-    def __init__(self, waypoints: list[Translation2d]):
+    def __init__(self, waypoints: list[Translation2d], face_target: bool):
         self.waypoints = waypoints
-        self.final_heading = Rotation2d(0)
+        self.face_target = face_target
+        if face_target:
+            last_waypoint = waypoints[-1]
+            self.final_heading = (
+                (
+                    BLUE_SPEAKER_POSE.translation().toTranslation2d()
+                    - field_flip_translation2d(last_waypoint)
+                )
+                .angle()
+                .radians()
+            )
+            self.final_heading = field_flip_angle(self.final_heading) + math.pi
+        else:
+            self.final_heading = 0
 
 
 stage_tolerance = 0.35