create tilt_to_shooter_FOR method and replaced tilt_to references

Author: LeanMeanBeanMachine4774

components/ballistics.py

@@ -14,7 +14,6 @@
 from components.injector import InjectorComponent
 from components.led_component import LightStrip
 from components.shooter import ShooterComponent
-from components.wrist import WristComponent
 from utilities.game import (
     ALGAE_MAX_DIAMETER,
     ALGAE_MIN_DIAMETER,
@@ -60,14 +59,15 @@ class BallisticsComponent:
 
     FLYWHEEL_ANGLE_LOOKUP = {
         ALGAE_MIN_DIAMETER: (
-            math.radians(-10) + WristComponent.COM_DIFFERENCE,
-            math.radians(-15) + WristComponent.COM_DIFFERENCE,
-            math.radians(-25) + WristComponent.COM_DIFFERENCE
+            math.radians(-10),
+            math.radians(-15),
+            math.radians(-25)
         ),
+        # 16.5: (math.radians(-11.0), math.radians(-19), -25),
         ALGAE_MAX_DIAMETER: (
-            math.radians(-10) + WristComponent.COM_DIFFERENCE,
-            math.radians(-15) + WristComponent.COM_DIFFERENCE,
-            math.radians(-25) + WristComponent.COM_DIFFERENCE
+            math.radians(-10),
+            math.radians(-15),
+            math.radians(-25)
         ),
     }
     # fmt: on

components/wrist.py

@@ -25,7 +25,7 @@ class WristComponent:
     COM_DIFFERENCE = 0.54722467321
     MAXIMUM_DEPRESSION = math.radians(-112.0) + COM_DIFFERENCE
     MAXIMUM_ELEVATION = math.radians(0) + COM_DIFFERENCE
-    NEUTRAL_ANGLE = math.radians(-90.0) + COM_DIFFERENCE
+    NEUTRAL_ANGLE = math.radians(-90.0)
 
     WRIST_MAX_VEL = math.radians(180.0)
     WRIST_MAX_ACC = math.radians(360.0)
@@ -123,7 +123,7 @@ def inclination_deg(self) -> float:
         return math.degrees(self.inclination())
 
     @feedback
-    def test_inclination(self) -> float:
+    def shooter_FOR_inclination(self) -> float:
         return math.degrees(self.inclination() - self.COM_DIFFERENCE)
 
     @feedback
@@ -143,6 +143,18 @@ def at_setpoint(self) -> bool:
             self.desired_state.velocity - self.current_velocity()
         ) < WristComponent.VEL_TOLERANCE
 
+    # Tilts to an angle with regards to the shooter frame of reference.
+    # Shooter FOR takes the arm as in-line with the backplate of the shooter
+    # SHould be called with angles in the shooter FOR
+    def tilt_to_shooter_FOR(self, pos: float) -> None:
+        clamped_angle = clamp(
+            (pos + self.COM_DIFFERENCE), self.MAXIMUM_DEPRESSION, self.MAXIMUM_ELEVATION
+        )
+        # If the new setpoint is within the tolerance we wouldn't move anyway
+        if abs(clamped_angle - self.desired_state.position) > self.TOLERANCE:
+            self._tilt_to(clamped_angle)
+
+    # Tilts to an angle with respect to the COM FOR
     def tilt_to(self, pos: float) -> None:
         clamped_angle = clamp(pos, self.MAXIMUM_DEPRESSION, self.MAXIMUM_ELEVATION)
 
@@ -160,7 +172,7 @@ def _tilt_to(self, pos: float):
         )
 
     def go_to_neutral(self) -> None:
-        self.tilt_to(WristComponent.NEUTRAL_ANGLE)
+        self.tilt_to_shooter_FOR(WristComponent.NEUTRAL_ANGLE)
 
     @feedback
     def at_limit(self) -> bool:
@@ -178,9 +190,7 @@ def execute(self) -> None:
             self.pid.calculate(self.inclination(), tracked_state.position) + ff
         )
 
-        self.wrist_ligament.setAngle(
-            self.inclination_deg() - math.degrees(WristComponent.COM_DIFFERENCE)
-        )
+        self.wrist_ligament.setAngle(self.shooter_FOR_inclination())
 
         self.wrist_COM_ligament.setAngle(self.inclination_deg())
         # self.wrist_ligament.setAngle(math.degrees(desired_state.position))

controllers/algae_shooter.py

@@ -20,12 +20,12 @@ class AlgaeShooter(StateMachine):
     floor_intake: FloorIntake
     reef_intake: ReefIntake
 
-    SHOOT_ANGLE = tunable(-50.0 + math.degrees(WristComponent.COM_DIFFERENCE))
+    SHOOT_ANGLE = tunable(-50.0)
     TOP_SHOOT_SPEED = tunable(60.0)
     BOTTOM_SHOOT_SPEED = tunable(65.0)
     use_ballistics = tunable(True)
 
-    MEASURING_ANGLE = math.radians(-15.0) + WristComponent.COM_DIFFERENCE
+    MEASURING_ANGLE = math.radians(-15.0)
 
     def __init__(self) -> None:
         pass
@@ -46,20 +46,20 @@ def shoot(self) -> None:
 
     @state(first=True)
     def measuring(self):
-        self.wrist.tilt_to(self.MEASURING_ANGLE)
+        self.wrist.tilt_to_shooter_FOR(self.MEASURING_ANGLE)
 
         self.next_state(self.preparing)
 
     @state(must_finish=True)
     def preparing(self):
         if self.use_ballistics:
             solution = self.ballistics_component.current_solution()
-            self.wrist.tilt_to(solution.inclination)
+            self.wrist.tilt_to_shooter_FOR(solution.inclination)
             self.shooter_component.spin_flywheels(
                 solution.top_speed, solution.bottom_speed
             )
         else:
-            self.wrist.tilt_to(math.radians(self.SHOOT_ANGLE))
+            self.wrist.tilt_to_shooter_FOR(math.radians(self.SHOOT_ANGLE))
             self.shooter_component.spin_flywheels(
                 self.TOP_SHOOT_SPEED, self.BOTTOM_SHOOT_SPEED
             )

controllers/floor_intake.py

@@ -14,7 +14,7 @@ class FloorIntake(StateMachine):
     wrist: WristComponent
     intake_component: IntakeComponent
 
-    HANDOFF_POSITION = tunable(math.radians(-112.0) + WristComponent.COM_DIFFERENCE)
+    HANDOFF_POSITION = tunable(math.radians(-112.0))
 
     def __init__(self):
         self.intake_upper = False
@@ -33,7 +33,7 @@ def intaking(self):
 
         self.intake_component.intake(self.intake_upper)
 
-        self.wrist.tilt_to(self.HANDOFF_POSITION)
+        self.wrist.tilt_to_shooter_FOR(self.HANDOFF_POSITION)
 
         self.shooter_component.intake()
         self.injector_component.intake()

controllers/reef_intake.py

@@ -72,13 +72,9 @@ def intaking(self, initial_call: bool):
         current_is_L3 = self.is_L3()
 
         if current_is_L3:
-            self.wrist.tilt_to(
-                math.radians(self.L3_INTAKE_ANGLE) + WristComponent.COM_DIFFERENCE
-            )
+            self.wrist.tilt_to_shooter_FOR(math.radians(self.L3_INTAKE_ANGLE))
         else:
-            self.wrist.tilt_to(
-                math.radians(self.L2_INTAKE_ANGLE) + WristComponent.COM_DIFFERENCE
-            )
+            self.wrist.tilt_to_shooter_FOR(math.radians(self.L2_INTAKE_ANGLE))
         self.last_l3 = current_is_L3
 
         if not self.holding_coral: