Ball pickup challenge auto code

autonomous/autonav.py

@@ -0,0 +1,180 @@
+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")
+
+    def done(self) -> None:
+        super().done()
+        self.chassis.disable_brake_mode()
+
+
+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.696, 2.269),
+                    Translation2d(4.596, 2.737),
+                    Translation2d(5.757, 2.522),
+                    Translation2d(6.509, 2.120),
+                    Translation2d(7.111, 1.088),
+                    Translation2d(8.151, 0.681),
+                    Translation2d(8.464, 1.543),
+                    Translation2d(7.955, 2.522),
+                    Translation2d(7.194, 2.323),
+                    Translation2d(6.531, 0.952),
+                    Translation2d(4.473, 0.572),
+                    Translation2d(2.382, 0.912),
+                    Translation2d(2.277, 2.335),
+                    Pose2d(0.946, 2.346, math.pi),
+                ],
+                reversed=False,
+            ),
+        ]
+        super().setup()
+
+
+class Bounce(AutoNavBase):
+    MODE_NAME = "Bounce"
+
+    def setup(self):
+        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.429, -math.pi / 2),
+                ],
+                reversed=True,
+            ),
+            Path(
+                [
+                    Pose2d(4.557, 3.429, -math.pi / 2),
+                    Translation2d(4.668, 1.020),
+                    Translation2d(5.873, 0.766),
+                    Translation2d(6.776, 1.068),
+                    Pose2d(6.856, 3.414, math.pi / 2),
+                ],
+                reversed=False,
+            ),
+            Path(
+                [
+                    Pose2d(6.856, 3.414, math.pi / 2),
+                    Translation2d(7.157, 2.328),
+                    Pose2d(8.352, 2.168, math.pi),
+                ],
+                reversed=True,
+            ),
+        ]
+        super().setup()
+
+
+PATHWEAVER_PATH = os.path.join(
+    os.path.dirname(os.path.abspath(__file__)), "pathweaver_paths", "output"
+)
+
+
+class BarrelRacing(AutoNavBase):
+    MODE_NAME = "Barrel Racing"
+
+    def setup(self):
+        self.paths = [
+            Path(
+                [
+                    Pose2d(1.019, 2.139, 0),
+                    Translation2d(4.1, 2.35),
+                    Translation2d(4.72, 1.5),
+                    Translation2d(3.6, 0.7),
+                    Translation2d(3.1, 1.9),
+                    Translation2d(7, 2.6),
+                    Translation2d(6.3, 3.7),
+                    Translation2d(5.2, 3.2),
+                    Translation2d(6.1, 1.25),
+                    Translation2d(7.3, 0.96),
+                    Translation2d(8.2, 1.15),
+                    Translation2d(8, 2),
+                    Translation2d(6, 2.346),
+                    Pose2d(0.946, 2.45, math.pi),
+                ],
+                reversed=False,
+            ),
+        ]
+        super().setup()

autonomous/ball_pickup.py

@@ -0,0 +1,150 @@
+from wpimath import geometry
+from wpilib.geometry import Pose2d, Translation2d
+from magicbot import AutonomousStateMachine, state
+
+from components.chassis import Chassis
+from components.indexer import Indexer
+from components.shooter import Shooter
+from components.turret import Turret
+from controllers.shooter import ShooterController
+from components.vision import Vision
+
+from utilities.path_follow import Path, PathFollow
+
+
+def to_pose(x: float, y: float, heading: float) -> geometry.Pose2d:
+    """
+    Convert inputs into a wpilib pose object
+    """
+    rotation = geometry.Rotation2d(heading)
+    return geometry.Pose2d(x, y, rotation)
+
+
+class BallPickup(AutonomousStateMachine):
+    MODE_NAME = "Ball Pickup"
+
+    shooter_controller: ShooterController
+    shooter: Shooter
+
+    chassis: Chassis
+    indexer: Indexer
+    vision: Vision
+    turret: Turret
+
+    TARGET_POSITION = geometry.Translation2d(0, 0)
+
+    path_names = ["None", "A1", "A2", "B1", "B2"]
+    # 1 is for red and 2 is for blue
+    all_paths = {}
+    all_paths["A1"] = Path(  # red A
+        [
+            Pose2d(2.286, 0.282, 0),
+            Translation2d(2.286, 2.286),
+            Translation2d(1.524, 3.810),
+            Translation2d(3.810, 4.572),
+            Pose2d(3.810, 9.144, 0),
+        ],
+        reversed=False,
+    )
+    all_paths["A2"] = Path(  # blue A
+        [
+            Pose2d(2.286, 0.282, 0),
+            Translation2d(0.762, 4.572),
+            Translation2d(3.048, 5.334),
+            Translation2d(2.286, 6.858),
+            Pose2d(2.286, 9.144, 0),
+        ],
+        reversed=False,
+    )
+    all_paths["B1"] = Path(  # red B
+        [
+            Pose2d(2.286, 0.282, 0),
+            Translation2d(3.048, 2.286),
+            Translation2d(1.524, 3.810),
+            Translation2d(3.048, 5.334),
+            Pose2d(3.048, 9.144, 0),
+        ],
+        reversed=False,
+    )
+    all_paths["B2"] = Path(  # blue B
+        [
+            Pose2d(2.286, 0.282, 0),
+            Translation2d(1.524, 4.572),
+            Translation2d(3.048, 6.096),
+            Translation2d(1.524, 7.620),
+            Pose2d(1.524, 9.144, 0),
+        ],
+        reversed=False,
+    )
+
+    expected_balls = 3
+
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.path_name = None
+
+    def setup(self):
+        self.path_follow: PathFollow = PathFollow(self.chassis)
+        self.indexer.set_max_balls(3)
+        self.shooter.toggle()  # turns fly wheels off
+        self.path_confidence = 0;
+        self.last_path = 0
+        self.confidence_thresh = 5
+
+    def on_enable(self) -> None:
+        self.indexer.lower_intake()
+        self.indexer.enable_intaking()
+        self.turret.slew_to_azimuth(0)
+        # self.has_zeroed = True
+        super().on_enable()
+
+    @state(first=True)
+    def findPath(self, initial_call, state_tm):
+        # wait for vision to know which path its on
+        self.vision_data = self.vision.get_data()
+        # self.vision_data.distance is actually the path num not the distance
+        print("vison data", self.vision_data)
+        if self.vision_data != None:
+            # check if its the balls vision which will only return whole numbers
+            if (
+                self.vision_data.distance % 1 == 0
+            ):  
+                if self.vision_data.distance == self.last_path:
+                    self.path_confidence += 1
+                else:
+                    self.path_confidence = 0
+
+                if self.path_confidence > self.confidence_thresh and self.vision_data.distance != 0:  # if it knowns which path its on
+                    self.path_name = self.path_names[int(self.vision_data.distance)]
+                    print(f"found path {self.path_name}")
+                    self.next_state("move")
+                self.last_path = self.vision_data.distance
+                return None
+            else:
+                print("wrong vision data")
+        else:
+            print("no vision data")
+        self.path_confidence = 0
+
+    @state
+    def move(self, initial_call, state_tm):
+        if initial_call:
+            path = self.all_paths[self.path_name]
+            self.chassis.reset_odometry(path.start)
+            self.path_follow.new_path(path)
+        self.path_follow.run()
+        if self.path_follow.path_done():
+            self.done()
+
+    def has_collected_balls(self) -> bool:
+        """
+        Has the robot collected all the balls for the current trajectory?
+        """
+        ball_target = self.expected_balls
+        if ball_target == 0:
+            return False
+        if self.indexer.balls_loaded() >= ball_target:
+            return True
+        else:
+            return False

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": "Galactic Search A",
+  "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"