adds auto L2 and L3 control

thedropbears · Jan 25, 2025 · 40d455e · 40d455e
1 parent 2d2521d
commit 40d455e
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 31 deletions.
diff --git 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
@@ -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
@@ -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()