Algae feeler

components/algae_manipulator.py

@@ -1,3 +1,5 @@
+import math
+
 from magicbot import feedback, tunable
 from phoenix6.configs import (
     ClosedLoopRampsConfigs,
@@ -9,9 +11,9 @@
 from phoenix6.hardware import TalonFX
 from phoenix6.signals import InvertedValue, NeutralModeValue
 from rev import SparkMax, SparkMaxConfig
-from wpilib import DigitalInput
+from wpilib import DigitalInput, Servo
 
-from ids import DioChannel, SparkId, TalonId
+from ids import DioChannel, PwmChannel, SparkId, TalonId
 
 
 class AlgaeManipulatorComponent:
@@ -31,6 +33,9 @@ def __init__(self) -> None:
 
         self.algae_limit_switch = DigitalInput(DioChannel.ALGAE_INTAKE_SWITCH)
 
+        self.feeler_limit_switch = DigitalInput(DioChannel.FEELER_LIMIT_SWITCH)
+        self.feeler_servo = Servo(PwmChannel.FEELER_SERVO)
+
         injector_config.inverted(True)
         self.injector_1.configure(
             injector_config,
@@ -84,6 +89,9 @@ def __init__(self) -> None:
         self.desired_flywheel_speed = 0.0
         self.desired_injector_speed = 0.25
 
+        self.algae_size = 0.0
+        self.desired_feeler_angle = math.radians(90)
+
     def spin_flywheels(self) -> None:
         self.desired_flywheel_speed = self.flywheel_shoot_speed
 
@@ -111,9 +119,29 @@ def intake(self) -> None:
     def has_algae(self) -> bool:
         return not self.algae_limit_switch.get()
 
+    @feedback
+    def feeler_touching_algae(self) -> bool:
+        return self.feeler_limit_switch.get()
+
+    @feedback
+    def get_algae_size(self) -> float:
+        return self.algae_size
+
+    def set_feeler(self, rot: float = 0.0, inverted: bool = False) -> None:
+        if not inverted:
+            self.desired_feeler_angle = rot
+        else:
+            self.desired_feeler_angle = math.radians(180) - rot
+
+    @feedback
+    def get_feeler_set_angle(self) -> float:
+        return math.degrees(self.desired_feeler_angle)
+
     def execute(self) -> None:
         self.injector_1.setVoltage(self.desired_injector_speed)
 
+        self.feeler_servo.setAngle(math.degrees(self.desired_feeler_angle))
+
         if self.desired_flywheel_speed == 0:
             self.flywheel_1.set_control(NeutralOut())
             self.flywheel_2.set_control(Follower(TalonId.FLYWHEEL_1, False))

controllers/reef_intake.py

@@ -16,6 +16,12 @@ class ReefIntake(StateMachine):
     L2_INTAKE_ANGLE = tunable(math.radians(-40.0))
     L3_INTAKE_ANGLE = tunable(math.radians(-10.0))
 
+    FEELER_START_ANGLE = tunable(math.radians(90))
+
+    FEELER_START_OFFSET = tunable(
+        math.radians(17)
+    )  # TODO: change to magic number we found from testing
+
     def __init__(self):
         self.last_l3 = False
 
@@ -25,7 +31,7 @@ def intake(self) -> None:
     @state(first=True, must_finish=True)
     def intaking(self, initial_call: bool):
         if self.algae_manipulator_component.has_algae():
-            self.done()
+            self.next_state("touch_the_algae")
             return
 
         current_is_L3 = self.is_L3()
@@ -39,10 +45,35 @@ def intaking(self, initial_call: bool):
 
         self.algae_manipulator_component.intake()
 
+    @state(must_finish=True)
+    def touch_the_algae(self, initial_call: bool):
+        if initial_call:
+            self.current_feeler_angle = (
+                self.FEELER_START_ANGLE + self.FEELER_START_OFFSET
+            )  # speed up beginning movement
+            self.algae_manipulator_component.algae_size = 0.0
+
+        if self.current_feeler_angle >= math.radians(160):
+            self.done()
+            return
+
+        self.algae_manipulator_component.set_feeler(self.current_feeler_angle, False)
+
+        if self.algae_manipulator_component.feeler_touching_algae():
+            self.algae_manipulator_component.algae_size = self.current_feeler_angle
+            self.done()
+            return
+
+        self.current_feeler_angle += math.radians(
+            0.69
+        )  # degrees per cycle that the feeler will move
+
     @feedback
     def is_L3(self) -> bool:
         return game.is_L3(game.nearest_reef_tag(self.chassis.get_pose()))
 
     def done(self) -> None:
         super().done()
         self.wrist.go_to_neutral()
+        self.current_feeler_angle = self.FEELER_START_ANGLE
+        self.algae_manipulator_component.set_feeler(self.current_feeler_angle, False)

ids.py

@@ -55,13 +55,17 @@ class DioChannel(enum.IntEnum):
 
     WRIST_LIMIT_SWITCH = 3
 
+    FEELER_LIMIT_SWITCH = 0
+
 
 @enum.unique
 class PwmChannel(enum.IntEnum):
     """roboRIO PWM output channel number."""
 
     VISION_SERVO = 0
 
+    FEELER_SERVO = 4
+
 
 @enum.unique
 class RioSerialNumber(enum.StrEnum):