add wrist com difference and add wrist_COM in sim

Author: LeanMeanBeanMachine4774

components/ballistics.py

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

components/wrist.py

@@ -21,10 +21,11 @@
 
 
 class WristComponent:
-    ENCODER_ZERO_OFFSET = 4.011863
-    MAXIMUM_DEPRESSION = math.radians(-112.0)
-    MAXIMUM_ELEVATION = math.radians(0)
-    NEUTRAL_ANGLE = math.radians(-90.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) + COM_DIFFERENCE
 
     WRIST_MAX_VEL = math.radians(180.0)
     WRIST_MAX_ACC = math.radians(360.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 test_inclination(self) -> float:
+        return math.degrees(self.inclination() - self.COM_DIFFERENCE)
+
     @feedback
     def shoot_angle_deg(self) -> float:
         return self.inclination_deg() + 90
@@ -169,5 +178,9 @@ 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.inclination_deg() - math.degrees(WristComponent.COM_DIFFERENCE)
+        )
+
+        self.wrist_COM_ligament.setAngle(self.inclination_deg())
         # self.wrist_ligament.setAngle(math.degrees(desired_state.position))

controllers/algae_shooter.py

@@ -20,11 +20,13 @@ class AlgaeShooter(StateMachine):
     floor_intake: FloorIntake
     reef_intake: ReefIntake
 
-    SHOOT_ANGLE = tunable(-50.0)
+    SHOOT_ANGLE = tunable(-50.0 + math.degrees(WristComponent.COM_DIFFERENCE))
     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
+
     def __init__(self) -> None:
         pass
 
@@ -44,7 +46,7 @@ def shoot(self) -> None:
 
     @state(first=True)
     def measuring(self):
-        self.wrist.tilt_to(math.radians(-15.0))
+        self.wrist.tilt_to(self.MEASURING_ANGLE)
 
         self.next_state(self.preparing)
 

controllers/floor_intake.py

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

controllers/reef_intake.py

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