From 24336a798c8c96b325bebb76faf2366f84206f32 Mon Sep 17 00:00:00 2001
From: James Ward <james@thedropbears.org.au>
Date: Wed, 17 Jan 2024 21:46:13 +1100
Subject: [PATCH] Skeleton for autonomous routines

---
 autonomous/autonomous.py | 86 +++++++++++++++++++++++++++++++---------
 1 file changed, 68 insertions(+), 18 deletions(-)

diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py
index deafcc36..e9f8cc23 100644
--- a/autonomous/autonomous.py
+++ b/autonomous/autonomous.py
@@ -1,36 +1,86 @@
 from magicbot.state_machine import AutonomousStateMachine, state
 
 from components.chassis import Chassis
-from components.climb import Climber
-from components.intake import Intake
-from components.shooter import Shooter
 
+# Add controllers for intake and shooter when available
 
-class Autonomous(AutonomousStateMachine):
+from wpimath.geometry import Pose2d
+
+from dataclasses import dataclass
+
+
+@dataclass
+class NotePaths:
+    pick_up_path: tuple[Pose2d, ...]
+    shoot_path: tuple[Pose2d, ...]
+
+
+class AutoBase(AutonomousStateMachine):
     chassis: Chassis
-    climb: Climber
-    intake: Intake
-    shooter: Shooter
+    # Add controllers for intake and shooter when available
 
     def __init__(self) -> None:
-        pass
+        self.note_paths: list[NotePaths] = []
+        self.has_initial_note: bool = True
 
     @state(first=True)
-    def initialize(self) -> None:
-        pass
+    def initialise(self) -> None:
+        # We always start ready to shoot, so fire straight away
+        self.has_initial_note = True
+        self.next_state("shoot_note")
 
     @state
-    def driveto_note(self) -> None:
-        pass
+    def shoot_note(self, initial_call: bool) -> None:
+        if initial_call:
+            # Call the shooter state machine
+            pass
 
-    @state
-    def pickup_note(self) -> None:
-        pass
+        if True:
+            # This needs to check if the state machine has finished firing
+            if len(self.note_paths) == 0:
+                # Just shot the last note
+                self.done()
+            else:
+                self.next_state("drive_to_pick_up")
 
     @state
-    def driveto_target(self) -> None:
-        pass
+    def drive_to_pick_up(self, initial_call: bool) -> None:
+        if initial_call:
+            self.calculate_trajectory(self.note_paths[0].pick_up_path)
+            # Also deploy the intake
+
+        if True:
+            # Check if we have a note collected
+            self.next_state("drive_to_shoot")
 
     @state
-    def score_note(self) -> None:
+    def drive_to_shoot(self, initial_call) -> None:
+        if initial_call:
+            self.calculate_trajectory(self.note_paths[0].shoot_path)
+        # Do some driving...
+
+        if True:
+            # If we are in position, remove this note from the list and shoot it
+            self.note_paths.pop(0)
+            self.next_state("shoot_note")
+
+    def calculate_trajectory(self, path: tuple[Pose2d, ...]) -> None:
+        # Driving to notes or to shoot is the same
+        # Abstract trajectory generation into a function
         pass
+
+
+class PreloadOnly(AutoBase):
+    MODE_NAME = "Preload only"
+
+
+class Front2Note(AutoBase):
+    MODE_NAME = "Front of speaker 2 note"
+
+    def setup(self) -> None:
+        self.note_paths = [
+            NotePaths(
+                pick_up_path=(Pose2d(1.0, 1.0, 0.0),),
+                shoot_path=(Pose2d(1.0, 1.0, 0.0),),
+            )
+        ]