WIP: and ability to invert servo

components/algae_manipulator.py

@@ -90,6 +90,7 @@ def __init__(self) -> None:
         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
@@ -122,12 +123,21 @@ def has_algae(self) -> bool:
     def feeler_touching_algae(self) -> bool:
         return not self.feeler_limit_switch.get()
 
-    def set_feeler(self, rot: float):
-        self.FeelerServo.setAngle(math.degrees(rot))
+    def set_feeler(self, rot: float, inverted: bool) -> None:
+        if inverted:
+            self.desired_feeler_angle = math.radians(180) - rot
+        else:
+            self.desired_feeler_angle = rot
+
+    @feedback
+    def get_feeler_set_angle(self) -> float:
+        return self.desired_feeler_angle
 
     def execute(self) -> None:
         self.injector_1.setVoltage(self.desired_injector_speed)
 
+        self.FeelerServo.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

@@ -51,9 +51,9 @@ def touch_the_algae(self, initial_call: bool):
             self.done()
             return
 
-        self.algae_manipulator_component.set_feeler(self.current_feeler_angle)
+        self.algae_manipulator_component.set_feeler(self.current_feeler_angle, True)
 
-        if self.algae_manipulator_component.feeler_limit_switch:
+        if self.algae_manipulator_component.feeler_touching_algae():
             self.algae_manipulator_component.algae_size = self.current_feeler_angle
             self.done()
             return
@@ -68,4 +68,4 @@ 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)
+        self.algae_manipulator_component.set_feeler(self.current_feeler_angle, True)