Simple autonomous test paths

autonomous/auto_base.py

@@ -182,3 +182,129 @@ def shooting_algae(self) -> None:
 
         if not self.algae_shooter.is_executing:
             self.next_state("tracking_trajectory")
+
+
+class AutoBaseNoBalls(AutonomousStateMachine):
+    chassis: ChassisComponent
+
+    field: wpilib.Field2d
+
+    DISTANCE_TOLERANCE = 0.1  # metres
+    ANGLE_TOLERANCE = math.radians(3)
+    CORAL_DISTANCE_TOLERANCE = 0.2  # metres
+    TRANSLATIONAL_SPEED_TOLERANCE = 0.2
+    ROTATIONAL_SPEED_TOLERANCE = 0.1
+
+    def __init__(self, trajectory_names: list[str]) -> None:
+        # We want to parameterise these by paths and potentially a sequence of events
+        super().__init__()
+
+        self.current_leg = -1
+        self.starting_pose = None
+        self.trajectories: list[SwerveTrajectory] = []
+        for trajectory_name in trajectory_names:
+            try:
+                self.trajectories.append(choreo.load_swerve_trajectory(trajectory_name))
+                if self.starting_pose is None:
+                    self.starting_pose = self.get_starting_pose()
+            except ValueError:
+                # If the trajectory is not found, ChoreoLib already prints to DriverStation
+                pass
+
+    def setup(self) -> None:
+        #  setup path tracking controllers
+
+        # init any other defaults
+        pass
+
+    def on_enable(self) -> None:
+        # configure defaults for pose in sim
+
+        # Setup starting position in the simulator
+        starting_pose = self.get_starting_pose()
+        if RobotBase.isSimulation() and starting_pose is not None:
+            self.chassis.set_pose(starting_pose)
+        # Reset the counter for which leg we are executing
+        self.current_leg = -1
+
+        super().on_enable()
+
+    def get_starting_pose(self) -> Pose2d | None:
+        return self.trajectories[0].get_initial_pose(game.is_red())
+
+    def _get_full_path_poses(self) -> list[Pose2d]:
+        """Get a list of poses for the full path for display."""
+        return [
+            sample.get_pose()
+            for trajectory in self.trajectories
+            for sample in trajectory.get_samples()
+        ]
+
+    def display_trajectory(self) -> None:
+        self.field.getObject("trajectory").setPoses(self._get_full_path_poses())
+
+    def on_disable(self) -> None:
+        super().on_disable()
+        self.field.getObject("trajectory").setPoses([])
+
+    @state(first=True)
+    def initialising(self) -> None:
+        # Add any tasks that need doing first
+        self.chassis.do_smooth = False
+        self.chassis.heading_controller.setPID(Kp=1.0, Ki=0.0, Kd=0.0)
+        self.next_state("tracking_trajectory")
+
+    @state
+    def tracking_trajectory(self, initial_call, state_tm) -> None:
+        if initial_call:
+            self.current_leg += 1
+
+        if self.current_leg == len(self.trajectories):
+            self.done()
+            return
+
+        # get next leg on entry
+        current_pose = self.chassis.get_pose()
+        final_pose = self.trajectories[self.current_leg].get_final_pose(game.is_red())
+        if final_pose is None:
+            self.done()
+            return
+
+        distance = current_pose.translation().distance(final_pose.translation())
+        angle_error = (final_pose.rotation() - current_pose.rotation()).radians()
+        velocity = self.chassis.get_velocity()
+        speed = math.sqrt(math.pow(velocity.vx, 2.0) + math.pow(velocity.vy, 2.0))
+
+        if (
+            distance < self.DISTANCE_TOLERANCE
+            and math.isclose(angle_error, 0.0, abs_tol=self.ANGLE_TOLERANCE)
+            and math.isclose(speed, 0.0, abs_tol=self.TRANSLATIONAL_SPEED_TOLERANCE)
+            and math.isclose(
+                velocity.omega, 0.0, abs_tol=self.ROTATIONAL_SPEED_TOLERANCE
+            )
+            and state_tm > self.trajectories[self.current_leg].get_total_time() / 2.0
+        ):
+            self.done()
+            return
+
+        sample = self.trajectories[self.current_leg].sample_at(state_tm, game.is_red())
+        if sample is not None:
+            self.follow_trajectory(sample)
+
+    def follow_trajectory(self, sample: SwerveSample):
+        # track path
+
+        pose = self.chassis.get_pose()
+
+        # Generate the next speeds for the robot
+        speeds = ChassisSpeeds(
+            sample.vx + x_controller.calculate(pose.X(), sample.x),
+            sample.vy + y_controller.calculate(pose.Y(), sample.y),
+            sample.omega
+            + self.chassis.heading_controller.calculate(
+                pose.rotation().radians(), sample.heading
+            ),
+        )
+
+        # Apply the generated speeds
+        self.chassis.drive_field(speeds.vx, speeds.vy, speeds.omega)

autonomous/multi_algae_auto.py

@@ -1,4 +1,4 @@
-from autonomous.auto_base import AutoBase
+from autonomous.auto_base import AutoBase, AutoBaseNoBalls
 
 
 class CentreAuto(AutoBase):
@@ -59,3 +59,47 @@ def __init__(self):
                 "AlgaeKLToShootKL",
             ]
         )
+
+
+class LineRotateTest(AutoBaseNoBalls):
+    MODE_NAME = "TEST LINE rotation"
+
+    def __init__(self) -> None:
+        super().__init__(
+            [
+                "LineRotateTest",
+            ]
+        )
+
+
+class LineRotateJiggleReductionTest(AutoBaseNoBalls):
+    MODE_NAME = "TEST JIGGLE REDUCTION LINE rotation"
+
+    def __init__(self) -> None:
+        super().__init__(
+            [
+                "LineLessJiggleTest",
+            ]
+        )
+
+
+class CurveRotateTest(AutoBaseNoBalls):
+    MODE_NAME = "TEST CURVE rotation"
+
+    def __init__(self) -> None:
+        super().__init__(
+            [
+                "CurveRotateTest",
+            ]
+        )
+
+
+class CurveRotateJiggleReductionTest(AutoBaseNoBalls):
+    MODE_NAME = "TEST JIGGLE REDUCTION CURVE rotation"
+
+    def __init__(self) -> None:
+        super().__init__(
+            [
+                "CurveLessJiggleTest",
+            ]
+        )