Added support to load pathweaver files

autonomous/autonav.py

@@ -0,0 +1,115 @@
+from typing import List
+
+from wpilib.geometry import Pose2d, Translation2d
+from magicbot import AutonomousStateMachine, state
+
+from components.chassis import Chassis
+from utilities.path_follow import Path, LoadPath, PathFollow
+import math
+import os
+
+
+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()
+
+    @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 BarrelRacing(AutoNavBase):
+    MODE_NAME = "Barrel Racing"
+
+    def setup(self):
+        self.paths = [
+            Path(
+                [
+                    Pose2d(1.044, 2.139, 0),
+                    Translation2d(4.031, 2.25),
+                    Translation2d(4.718, 1.483),
+                    Translation2d(3.585, 0.689),
+                    Translation2d(2.925, 1.991),
+                    Translation2d(4.985, 2.330),
+                    Translation2d(4.985, 2.330),
+                    Translation2d(6.903, 2.562),
+                    Translation2d(6.456, 3.7),
+                    Translation2d(5.368, 3.346),
+                    Translation2d(6.126, 1.269),
+                    Translation2d(7.250, 0.921),
+                    Translation2d(8.0, 1.0),
+                    Translation2d(7.8, 2.0),
+                    Translation2d(6.5, 2.2),
+                    Pose2d(1.044, 2.2, math.pi),
+                ],
+                reversed=False,
+            )
+        ]
+        super().setup()
+
+
+PATHWEAVER_PATH = os.path.join(
+    os.path.dirname(os.path.abspath(__file__)), "pathweaver_paths", "output"
+)
+
+
+class LoadTest(AutoNavBase):
+    MODE_NAME = "Pathweaver Test"
+
+    def setup(self):
+        self.paths = [
+            LoadPath(
+                os.path.join(PATHWEAVER_PATH, "Barrel_Racing.wpilib.json"),
+                reversed=False,
+            )
+        ]
+        super().setup()

autonomous/pathweaver_paths/.project/Paths/Barrel_Racing

@@ -0,0 +1,9 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+1.124,-2.3,3.048,0.0,true,false,
+4.527092084799869,-3.097523450929033,-0.06736187280019656,-3.2,true,false,
+3.1274620610624537,-2.7681987394614063,1.1376671850699838,1.9235557010722766,true,false,
+6.92218089547352,-1.6230469018580667,0.6960726856020303,2.0358254890726037,true,false,
+5.4477043464025545,-1.2712682327903744,-0.7784038634689372,-2.2304264549398383,true,false,
+7.468560530408447,-3.7786268314643534,1.3921453712040597,0.26196283866743064,true,false,
+7.738008021609233,-2.206849799459769,-2.4998739461406236,1.1301825325366295,true,false,
+0.7024346402553819,-1.8550711303920766,-0.8682196938691987,0.02245395760006552,true,false,

autonomous/pathweaver_paths/.project/pathweaver.json

@@ -0,0 +1,9 @@
+{
+  "lengthUnit": "Meter",
+  "exportUnit": "Same as Project",
+  "maxVelocity": 2.4,
+  "maxAcceleration": 2.4,
+  "wheelBase": 0.8,
+  "gameName": "Barrel Racing Path",
+  "outputDir": ".."
+}

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"
 

utilities/path_follow.py

@@ -0,0 +1,107 @@
+from typing import List
+
+from wpilib import controller
+from wpilib import trajectory
+from wpilib import Timer
+from wpilib.trajectory import constraint
+from wpilib.trajectory import TrajectoryUtil
+from wpilib.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
+
+    def getTrajectory(self, config: trajectory.TrajectoryConfig):
+        return trajectory.TrajectoryGenerator().generateTrajectory(
+            self.start, self.waypoints, self.end, config
+        )
+
+
+class LoadPath:
+    start: Pose2d
+    filename: str
+    reversed: bool
+
+    def __init__(self, filename, reversed):
+        self.filename = filename
+        self.reversed = reversed
+        self.trajectory = TrajectoryUtil.fromPathweaverJson(self.filename)
+        self.start = self.trajectory.initialPose()  # is used to reset odometry
+
+    def getTrajectory(self, *_: trajectory.TrajectoryConfig):
+        # dosent care about the config you give it, set config through pathweaver
+        return self.trajectory
+
+
+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=1, maxAcceleration=1
+        )
+        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: trajectory.Trajectory
+        self.timer: Timer = Timer()
+        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 = path.getTrajectory(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()