AutoNav

autonomous/autonav.py

@@ -0,0 +1,148 @@
+import math
+from typing import List
+
+from wpimath.geometry import Pose2d, Translation2d
+from magicbot import AutonomousStateMachine, state
+
+from components.chassis import Chassis
+from utilities.path_follow import Path, PathFollow
+
+
+class AutoNavBase(AutonomousStateMachine):
+
+    chassis: Chassis
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.path_num = 0
+        self.paths: List[Path]
+
+    def setup(self) -> None:
+        self.path_follow: PathFollow = PathFollow(self.chassis)
+
+    def on_enable(self) -> None:
+        self.chassis.reset_odometry(self.paths[0].start)
+        self.path_num = 0
+        super().on_enable()
+
+    def done(self) -> None:
+        super().done()
+        self.chassis.disable_brake_mode()
+
+
+    @state(first=True)
+    def move(self, initial_call) -> None:
+        """
+        Follow the trajectory defined by our waypoints
+        """
+        if initial_call:
+            path = self.paths[self.path_num]
+            self.path_follow.new_path(path)
+        self.path_follow.run()
+        if self.path_follow.path_done():
+            self.next_state("done_move")
+
+    @state
+    def done_move(self):
+        self.path_num += 1
+        if self.path_num >= len(self.paths):
+            self.done()
+        else:
+            self.next_state("move")
+
+
+class test(AutoNavBase):
+    MODE_NAME = "Test"
+    DEFAULT = True
+
+    def setup(self):
+        self.paths = [
+            Path(
+                [
+                    Pose2d(0, 0, 0),
+                    Translation2d(1, 1),
+                    Translation2d(0, 1),
+                    Translation2d(-1, 0),
+                    Pose2d(0, 0, 0),
+                ],
+                reversed=False,
+            )
+        ]
+        super().setup()
+
+
+class Slalom(AutoNavBase):
+    MODE_NAME = "Slalom"
+
+    def setup(self):
+        self.paths = [
+            Path(
+                [
+                    Pose2d(1.056, 0.618, 0),
+                    Translation2d(2.188, 0.908),
+                    Translation2d(2.678, 2.127),
+                    Translation2d(3.560, 2.579),
+                    Translation2d(5.600, 2.557),
+                    Translation2d(6.700, 2.120),
+                    Translation2d(7.150, 0.872),
+                    Translation2d(8.056, 0.834),
+                    Translation2d(8.464, 1.543),
+                    Translation2d(8.012, 2.304),
+                    Translation2d(7.194, 2.323),
+                    Translation2d(6.404, 0.834),
+                    Translation2d(4.537, 0.711),
+                    Translation2d(2.755, 0.747),
+                    Translation2d(2.087, 1.769),
+                    Pose2d(1.263, 2.479, math.radians(139.25)),
+                ],
+                reversed=False,
+            ),
+        ]
+        super().setup()
+
+
+class Bounce(AutoNavBase):
+    MODE_NAME = "Bounce"
+
+    def setup(self):
+        path_offset = 0.5
+        self.paths = [
+            Path(
+                [
+                    Pose2d(1.019, 2.139, 0),
+                    Translation2d(1.830, 2.359),
+                    Pose2d(2.250, 3.406, math.pi / 2),
+                ],
+                reversed=False,
+            ),
+            Path(
+                [
+                    Pose2d(2.250, 3.406, math.pi / 2),
+                    Translation2d(2.686, 2.169),
+                    Translation2d(3.164, 1.137),
+                    Translation2d(3.795, 0.786),
+                    Translation2d(4.443, 1.186),
+                    Pose2d(4.557, 3.529 + path_offset/2, -math.pi / 2),
+                ],
+                reversed=True,
+            ),
+            Path(
+                [
+                    Pose2d(4.557, 3.529 + path_offset/3, -math.pi / 2),
+                    Translation2d(4.668, 1.020 + path_offset),
+                    Translation2d(5.673, 0.666 + path_offset),
+                    Translation2d(6.800, 0.768 + path_offset),
+                    Pose2d(6.856, 3.614 + path_offset, math.pi / 2),
+                ],
+                reversed=False,
+            ),
+            Path(
+                [
+                    Pose2d(6.856, 3.614 + path_offset, math.pi / 2),
+                    Translation2d(7.157, 2.328 + 1.5 * path_offset),
+                    Pose2d(8.352, 2.168 + 1.5 * path_offset, math.pi),
+                ],
+                reversed=True,
+            ),
+        ]
+        super().setup()

autonomous/shoot_move_shoot.py

@@ -135,23 +135,6 @@ def has_fired_balls(self) -> bool:
         return self.shooter_controller.fired_count >= balls_to_fire
 
 
-class test(ShootMoveShootBase):
-    MODE_NAME = "Test"
-    DEFAULT = True
-
-    def setup(self):
-        self.start_poses = [to_pose(0, 0, math.pi)]
-        self.end_poses = [to_pose(-2, 0, math.pi)]
-        self.waypoints = [[geometry.Translation2d(-1, math.pi)]]
-        self.trajectory_config = trajectory.TrajectoryConfig(
-            maxVelocity=1, maxAcceleration=1
-        )
-        self.expected_balls = [0]
-        self.reversed = [False]
-        self.trajectory_max = 1
-        super().setup()
-
-
 class _3Right3(ShootMoveShootBase):
     MODE_NAME = "3RIGHT3"
 

robot.py

@@ -97,6 +97,10 @@ def autonomousInit(self) -> None:
         self.indexer.shimmying = False
         self.indexer.auto_retract = False
 
+    def disabledInit(self) -> None:
+        self.chassis.drive(0, 0)
+        super().disabledInit()
+
     def disabledPeriodic(self) -> None:
         self.vision.execute()
 

utilities/path_follow.py

@@ -0,0 +1,90 @@
+from typing import List
+
+from wpilib import controller
+from wpimath import trajectory
+from wpilib import Timer
+from wpimath.trajectory import constraint
+from wpimath.geometry import Pose2d, Translation2d
+
+from components.chassis import Chassis
+
+
+class Path:
+    start: Pose2d
+    waypoints: List[Translation2d]
+    end: Pose2d
+    reversed: bool
+
+    def __init__(self, points, reversed) -> None:
+        self.start, *self.waypoints, self.end = points
+        self.reversed = reversed
+
+
+class PathFollow:
+    """
+    A class to follow a given class, run must be called in each loop where
+    an output is wanted
+    """
+
+    def __init__(self, chassis: Chassis) -> None:
+        self.chassis = chassis
+        self.controller = controller.RamseteController()
+        self.trajectory_config = trajectory.TrajectoryConfig(
+            maxVelocity=2.5, maxAcceleration=1.5
+        )
+        self.gen = trajectory.TrajectoryGenerator()
+        self.trajectory_config.setKinematics(self.chassis.kinematics)
+        self.trajectory_config.addConstraint(
+            constraint.DifferentialDriveVoltageConstraint(
+                self.chassis.ff_calculator, self.chassis.kinematics, maxVoltage=10
+            )
+        )
+        self.trajectory_config.addConstraint(constraint.CentripetalAccelerationConstraint(maxCentripetalAcceleration=2.5))
+        self.trajectory: trajectory.Trajectory
+        self.start_time: float
+
+    def new_path(self, path: Path) -> None:
+        """
+        Give the path follower a new path, it will abandon the current one and
+        follow it instead
+        """
+        self.trajectory_config.setReversed(path.reversed)
+        self.trajectory = self.gen.generateTrajectory(
+            path.start,
+            path.waypoints,
+            path.end,
+            self.trajectory_config,
+        )
+        self.start_time = Timer.getFPGATimestamp()
+
+    def new_quintic_path(self, waypoints: List[Pose2d], reversed: bool):
+        """
+        Give the path follower a new path, it will abandon the current one and
+        follow it instead. This method specifies the heading at each waypoint.
+        """
+        self.trajectory_config.setReversed(reversed)
+        self.trajectory = self.gen.generateTrajectory(
+            waypoints,
+            self.trajectory_config,
+        )
+        self.start_time = Timer.getFPGATimestamp()
+
+    def run(self) -> None:
+        """
+        Send the chassis control inputs for this loop
+        """
+        if self.path_done():
+            self.chassis.drive(0, 0)
+        else:
+            pos = self.chassis.get_pose()
+            goal = self.trajectory.sample(self.current_path_time)
+            speeds = self.controller.calculate(pos, goal)
+            self.chassis.drive(speeds.vx, speeds.omega)
+
+    def path_done(self) -> bool:
+        """
+        Check to see if enough time has passed to complete the path
+        """
+        # TODO investigate closing the loop here
+        self.current_path_time = Timer.getFPGATimestamp() - self.start_time
+        return self.current_path_time > self.trajectory.totalTime()