Merge pull request #421 from thedropbears/update-wrist-coord-scheme

Update wrist coordinate scheme

Author: L9-bms

components/ballistics.py

@@ -58,9 +58,17 @@ class BallisticsComponent:
     }
 
     FLYWHEEL_ANGLE_LOOKUP = {
-        ALGAE_MIN_DIAMETER: (math.radians(-10), math.radians(-15), math.radians(-25)),
+        ALGAE_MIN_DIAMETER: (
+            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), math.radians(-15), math.radians(-25)),
+        ALGAE_MAX_DIAMETER: (
+            math.radians(-10),
+            math.radians(-15),
+            math.radians(-25)
+        ),
     }
     # fmt: on
     BALL_SIZES = list(FLYWHEEL_ANGLE_LOOKUP.keys())

components/wrist.py

@@ -21,9 +21,10 @@
 
 
 class WristComponent:
-    ENCODER_ZERO_OFFSET = 4.011863
-    MAXIMUM_DEPRESSION = math.radians(-112.0)
-    MAXIMUM_ELEVATION = math.radians(0)
+    ENCODER_ZERO_OFFSET = 3.46463832679
+    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)
 
     WRIST_MAX_VEL = math.radians(180.0)
@@ -37,6 +38,10 @@ def __init__(self, mech_root: wpilib.MechanismRoot2d):
             "wrist", length=0.5, angle=0, color=wpilib.Color8Bit(wpilib.Color.kYellow)
         )
 
+        self.wrist_COM_ligament = mech_root.appendLigament(
+            "wrist_COM", length=0.5, angle=0, color=wpilib.Color8Bit(wpilib.Color.kBlue)
+        )
+
         self.wrist_encoder = DutyCycleEncoder(DioChannel.WRIST_ENCODER, math.tau, 0)
         configure_through_bore_encoder(self.wrist_encoder)
         self.wrist_encoder.setInverted(False)
@@ -117,6 +122,10 @@ def inclination(self) -> float:
     def inclination_deg(self) -> float:
         return math.degrees(self.inclination())
 
+    @feedback
+    def shooter_FOR_inclination_deg(self) -> float:
+        return math.degrees(self.inclination() - self.COM_DIFFERENCE)
+
     @feedback
     def shoot_angle_deg(self) -> float:
         return self.inclination_deg() + 90
@@ -134,6 +143,13 @@ 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:
+        self.tilt_to(pos + self.COM_DIFFERENCE)
+
+    # 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)
 
@@ -151,7 +167,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:
@@ -169,5 +185,7 @@ def execute(self) -> None:
             self.pid.calculate(self.inclination(), tracked_state.position) + ff
         )
 
-        self.wrist_ligament.setAngle(self.inclination_deg())
+        self.wrist_ligament.setAngle(self.shooter_FOR_inclination_deg())
+
+        self.wrist_COM_ligament.setAngle(self.inclination_deg())
         # self.wrist_ligament.setAngle(math.degrees(desired_state.position))

controllers/algae_shooter.py

@@ -25,6 +25,8 @@ class AlgaeShooter(StateMachine):
     BOTTOM_SHOOT_SPEED = tunable(65.0)
     use_ballistics = tunable(True)
 
+    MEASURING_ANGLE = math.radians(-15.0)
+
     def __init__(self) -> None:
         pass
 
@@ -44,20 +46,20 @@ def shoot(self) -> None:
 
     @state(first=True)
     def measuring(self):
-        self.wrist.tilt_to(math.radians(-15.0))
+        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

@@ -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,9 +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))
+            self.wrist.tilt_to_shooter_FOR(math.radians(self.L3_INTAKE_ANGLE))
         else:
-            self.wrist.tilt_to(math.radians(self.L2_INTAKE_ANGLE))
+            self.wrist.tilt_to_shooter_FOR(math.radians(self.L2_INTAKE_ANGLE))
         self.last_l3 = current_is_L3
 
         if not self.holding_coral: