adds auto L2 and L3 control

components/wrist.py

@@ -1,22 +1,15 @@
-import math
-
 from magicbot import feedback, tunable
 from rev import ClosedLoopSlot, SparkMax, SparkMaxConfig
 from wpilib import DigitalInput
 
 from ids import DioChannel, SparkId
-from utilities.functions import clamp
 
 
 class WristComponent:
-    MAXIMUM_DEPRESSION = -23.0
-    MAXIMUM_ELEVATION = 80.0
-    NEUTRAL_ANGLE = 0.0
-
-    angle_change_rate_while_zeroing = tunable(0.1)
-    wrist_gear_ratio = (150.0 / 15) * 20
+    maximum_angle = 73.0
+    angle_change_rate_while_zeroing = tunable(3.0)
+    wrist_gear_ratio = 20 * 66 / 26
     desired_angle = 0.0
-    TOLERANCE = 3.0
 
     def __init__(self):
         self.switch = DigitalInput(DioChannel.WRIST_LIMIT_SWITCH)
@@ -26,13 +19,10 @@ def __init__(self):
         self.wrist_controller = self.wrist.getClosedLoopController()
 
         wrist_config = SparkMaxConfig()
-        wrist_config.inverted(True)
-        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kCoast)
-        wrist_config.closedLoop.P(
-            3 / (self.MAXIMUM_ELEVATION - self.MAXIMUM_DEPRESSION),
-            ClosedLoopSlot.kSlot0,
-        )
-        wrist_config.closedLoop.D(0.0, ClosedLoopSlot.kSlot0)
+        wrist_config.inverted(False)
+        wrist_config.setIdleMode(SparkMaxConfig.IdleMode.kBrake)
+        wrist_config.closedLoop.P(6.0 / self.maximum_angle, ClosedLoopSlot.kSlot0)
+        wrist_config.closedLoop.D(0.1, ClosedLoopSlot.kSlot0)
 
         wrist_config.encoder.positionConversionFactor(360 * (1 / self.wrist_gear_ratio))
 
@@ -46,41 +36,25 @@ def __init__(self):
 
         self.encoder.setPosition(0.0)
 
-    def on_enable(self):
-        self.tilt_to(self.inclination())
-
     def zero_wrist(self) -> None:
-        if not self.wrist_at_bottom_limit():
-            self.tilt_to(self.desired_angle - self.angle_change_rate_while_zeroing)
-            if math.isclose(
-                self.desired_angle, self.MAXIMUM_DEPRESSION, abs_tol=1.0
-            ) and math.isclose(
-                self.inclination(), self.MAXIMUM_DEPRESSION, abs_tol=1.0
-            ):
-                self.encoder.setPosition(self.inclination() + 1)
+        if not self.wrist_at_top_limit():
+            self.desired_angle += self.angle_change_rate_while_zeroing
+        else:
+            self.encoder.setPosition(self.maximum_angle)
+            self.desired_angle = self.maximum_angle
 
     @feedback
-    def wrist_at_bottom_limit(self) -> bool:
+    def wrist_at_top_limit(self) -> bool:
         return not self.switch.get()
 
     @feedback
-    def inclination(self) -> float:
+    def get_encoder(self) -> float:
         return self.encoder.getPosition()
 
-    @feedback
-    def at_setpoint(self) -> bool:
-        return abs(self.desired_angle - self.inclination()) < WristComponent.TOLERANCE
-
-    def tilt_to(self, pos: float) -> None:
-        self.desired_angle = clamp(pos, self.MAXIMUM_DEPRESSION, self.MAXIMUM_ELEVATION)
-
-    def go_to_neutral(self) -> None:
-        self.tilt_to(WristComponent.NEUTRAL_ANGLE)
+    def tilt_to(self, pos) -> None:
+        self.desired_angle = pos
 
     def execute(self) -> None:
-        if self.wrist_at_bottom_limit():
-            self.encoder.setPosition(self.MAXIMUM_DEPRESSION)
-
         self.wrist_controller.setReference(
             self.desired_angle, SparkMax.ControlType.kPosition, ClosedLoopSlot.kSlot0
         )

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()

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()