From 40d455e3c1ff527c848e45e806fafce01b17a512 Mon Sep 17 00:00:00 2001
From: "Calvin O." <calvin.oakley@thedropbears.org.au>
Date: Sat, 25 Jan 2025 14:08:06 +1100
Subject: [PATCH] adds auto L2 and L3 control

---
 components/wrist.py         | 18 ++++++++++++++-
 controllers/algae_intake.py | 45 +++++++++++++------------------------
 robot.py                    |  2 +-
 3 files changed, 34 insertions(+), 31 deletions(-)

diff --git a/components/wrist.py b/components/wrist.py
index d8d54c46..e84ee60d 100644
--- a/components/wrist.py
+++ b/components/wrist.py
@@ -27,7 +27,7 @@ def __init__(self):
 
         wrist_config = SparkMaxConfig()
         wrist_config.inverted(True)
-        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast)
+        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
         wrist_config.closedLoop.P(
             3 / (self.MAXIMUM_ELEVATION - self.MAXIMUM_DEPRESSION),
             ClosedLoopSlot.kSlot0,
@@ -48,6 +48,22 @@ def __init__(self):
 
     def on_enable(self):
         self.tilt_to(self.inclination())
+        wrist_config = SparkMaxConfig()
+        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
+        self.wrist.configure(
+            wrist_config,
+            SparkMax.ResetMode.kNoResetSafeParameters,
+            SparkMax.PersistMode.kNoPersistParameters,
+        )
+
+    def on_disable(self):
+        wrist_config = SparkMaxConfig()
+        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast)
+        self.wrist.configure(
+            wrist_config,
+            SparkMax.ResetMode.kNoResetSafeParameters,
+            SparkMax.PersistMode.kNoPersistParameters,
+        )
 
     def zero_wrist(self) -> None:
         if not self.wrist_at_bottom_limit():
diff --git a/controllers/algae_intake.py b/controllers/algae_intake.py
index 2da45de6..7637c936 100644
--- a/controllers/algae_intake.py
+++ b/controllers/algae_intake.py
@@ -1,54 +1,41 @@
-from magicbot import StateMachine, state, tunable
+from magicbot import StateMachine, feedback, state, tunable
 
 from components.algae_manipulator import AlgaeManipulatorComponent
+from components.chassis import ChassisComponent
 from components.wrist import WristComponent
+from utilities import game
 
 
 class AlgaeIntake(StateMachine):
     algae_manipulator_component: AlgaeManipulatorComponent
     wrist: WristComponent
+    chassis: ChassisComponent
 
     L2_INTAKE_ANGLE = tunable(40.0)
-    L3_INTAKE_ANGLE = tunable(40.0)
+    L3_INTAKE_ANGLE = tunable(80.0)
 
     def __init__(self):
         pass
 
-    @state(first=True, must_finish=True)
-    def raising_to_L2(self):
-        if self.algae_manipulator_component.has_algae():
-            self.done()
-            return
+    def intake(self) -> None:
+        self.engage()
 
-        self.wrist.tilt_to(self.L2_INTAKE_ANGLE)
-
-        if self.wrist.at_setpoint():
-            self.next_state("intaking")
-
-    @state(must_finish=True)
-    def raising_to_L3(self):
-        if self.algae_manipulator_component.has_algae():
-            self.done()
-            return
-
-        self.wrist.tilt_to(self.L3_INTAKE_ANGLE)
-
-        if self.wrist.at_setpoint():
-            self.next_state("intaking")
-
-    @state(must_finish=True)
+    @state(first=True, must_finish=True)
     def intaking(self):
         if self.algae_manipulator_component.has_algae():
             self.done()
             return
 
-        self.algae_manipulator_component.intake()
+        if self.is_L3():
+            self.wrist.tilt_to(self.L3_INTAKE_ANGLE)
+        else:
+            self.wrist.tilt_to(self.L2_INTAKE_ANGLE)
 
-    def intake_L2(self) -> None:
-        self.engage("raising_to_L2")
+        self.algae_manipulator_component.intake()
 
-    def intake_L3(self) -> None:
-        self.engage("raising_to_L3")
+    @feedback
+    def is_L3(self) -> bool:
+        return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose()))
 
     def done(self) -> None:
         super().done()
diff --git a/robot.py b/robot.py
index 85e22990..007ab96e 100644
--- a/robot.py
+++ b/robot.py
@@ -223,7 +223,7 @@ def testPeriodic(self) -> None:
         if self.gamepad.getRightTriggerAxis() > 0.5:
             self.algae_shooter.shoot()
         if self.gamepad.getAButton():
-            self.algae_intake.intake_L2()
+            self.algae_intake.intake()
 
         self.algae_shooter.execute()