Merge pull request #23 from auscompgeek/pure-pursuit-types

Clean up pure pursuit

autonomous/autonomous.py

@@ -7,11 +7,11 @@
 from components.vision import Vision
 from pyswervedrive.chassis import SwerveChassis
 from utilities.navx import NavX
-from utilities.pure_pursuit import PurePursuit, insert_trapezoidal_waypoints
+from utilities.pure_pursuit import PurePursuit, Waypoint, insert_trapezoidal_waypoints
 
 
-def reflect_2d_y(v: tuple) -> tuple:
-    return (v[0], -v[1], v[2], v[3])
+def reflect_y(v: Waypoint) -> Waypoint:
+    return Waypoint(v.x, -v.y, v.theta, v.v)
 
 
 class AutoBase(AutonomousStateMachine):
@@ -30,17 +30,16 @@ class AutoBase(AutonomousStateMachine):
 
     def __init__(self):
         super().__init__()
-        self.front_cargo_bay = (5.6 - SwerveChassis.LENGTH / 2, 0.2, 0, 0.75)
-        self.setup_loading_bay = (3.3, 3.3, math.pi, 2)
-        self.loading_bay = (0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1)
-        self.side_cargo_bay = (7, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 1)
-        self.side_cargo_bay_alignment_point = (
-            7,
-            1.8 + SwerveChassis.WIDTH / 2,
-            -math.pi / 2,
-            0.75,
+        self.front_cargo_bay = Waypoint(5.6 - SwerveChassis.LENGTH / 2, 0.2, 0, 0.75)
+        self.setup_loading_bay = Waypoint(3.3, 3.3, math.pi, 2)
+        self.loading_bay = Waypoint(0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1)
+        self.side_cargo_bay = Waypoint(
+            7, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 1
         )
-        self.start_pos = (
+        self.side_cargo_bay_alignment_point = Waypoint(
+            7, 1.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 0.75
+        )
+        self.start_pos = Waypoint(
             1.2 + SwerveChassis.LENGTH / 2,
             0,
             0,
@@ -111,7 +110,7 @@ def drive_to_loading_bay(self, initial_call):
                 waypoints = insert_trapezoidal_waypoints(
                     (
                         self.current_pos,
-                        (
+                        Waypoint(
                             self.current_pos[0] - 0.5,
                             self.current_pos[1],
                             self.imu.getAngle(),
@@ -153,21 +152,17 @@ def stop(self):
 
     @property
     def current_pos(self):
-        return (
-            self.chassis.odometry_x,
-            self.chassis.odometry_y,
-            self.imu.getAngle(),
-            2,
+        return Waypoint(
+            self.chassis.odometry_x, self.chassis.odometry_y, self.imu.getAngle(), 2
         )
 
     def follow_path(self):
-        vx, vy, heading = self.pursuit.find_velocity(self.current_pos)
+        vx, vy, heading = self.pursuit.find_velocity(self.chassis.position)
         if self.pursuit.completed_path:
             self.chassis.set_inputs(0, 0, 0, field_oriented=False)
             return
         # TODO implement a system to allow for rotation in waypoints
-        self.chassis.set_heading_sp(heading)
-        self.chassis.set_inputs(vx, vy, 0)
+        self.chassis.set_velocity_heading(vx, vy, heading)
 
     def ready_for_vision(self):
         if self.pursuit.waypoints[-1][4] - self.pursuit.distance_traveled < 1:
@@ -181,10 +176,10 @@ class RightStartAuto(AutoBase):
 
     def __init__(self):
         super().__init__()
-        self.front_cargo_bay = reflect_2d_y(self.front_cargo_bay)
-        self.setup_loading_bay = reflect_2d_y(self.setup_loading_bay)
-        self.loading_bay = reflect_2d_y(self.loading_bay)
-        self.side_cargo_bay = reflect_2d_y(self.side_cargo_bay)
+        self.front_cargo_bay = reflect_y(self.front_cargo_bay)
+        self.setup_loading_bay = reflect_y(self.setup_loading_bay)
+        self.loading_bay = reflect_y(self.loading_bay)
+        self.side_cargo_bay = reflect_y(self.side_cargo_bay)
 
 
 class LeftStartAuto(AutoBase):

autonomous/test.py

@@ -1,7 +1,8 @@
 from magicbot.state_machine import AutonomousStateMachine, state
-from utilities.pure_pursuit import PurePursuit
+
 from pyswervedrive.chassis import SwerveChassis
 from utilities.navx import NavX
+from utilities.pure_pursuit import PurePursuit
 
 
 class TestPursuitAuto(AutonomousStateMachine):
@@ -22,10 +23,7 @@ def __init__(self):
     def move_forwards(self, initial_call):
         if initial_call:
             self.pursuit.build_path(self.points)
-        heading = self.imu.getAngle()
-        x, y = self.chassis.position
-        position = (x, y, heading)
-        vx, vy, vz = self.pursuit.find_velocity(position)
+        vx, vy, vz = self.pursuit.find_velocity(self.chassis.position)
         self.chassis.set_inputs(vx, vy, 0)
         if self.pursuit.completed_path:
             self.chassis.set_inputs(0, 0, 0)

tests/test_pure_pursuit.py

@@ -2,14 +2,14 @@
 
 
 def test_trapezoidal():
-    waypoints = [(0, 0, 0, 0), (10, 10, 0, 2)]
+    waypoints = [pp.Waypoint(0, 0, 0, 0), pp.Waypoint(10, 10, 0, 2)]
     trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2)
     assert len(trap) == 3
     assert trap[1][2] == waypoints[1][2], (
         "Intermediate waypoint should have end speed when accelerating: %s" % trap
     )
 
-    waypoints = [(10, 10, 0, 2), (0, 0, 0, 0)]
+    waypoints = [pp.Waypoint(10, 10, 0, 2), pp.Waypoint(0, 0, 0, 0)]
     trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2)
     assert len(trap) == 3
     assert trap[1][2] == waypoints[0][2], (