physics: Simulate algae interaction using physical properties

physics.py

@@ -2,6 +2,7 @@
 
 import math
 import typing
+from collections.abc import Callable
 
 import phoenix6
 import phoenix6.unmanaged
@@ -102,6 +103,33 @@ def update(self, dt: float) -> None:
             )
 
 
+class SparkMotorSim:
+    """Simulates Spark MAX/Flex with brushless motors."""
+
+    def __init__(
+        self,
+        # DCMotor gearbox factory, e.g. DCMotor.NEO
+        gearbox_motor: Callable[[int], DCMotor],
+        *motors: rev.SparkBase,
+        gearing: float,
+        moi: kilogram_square_meters,
+        velocity_units_per_rad_per_sec: float = 60 / math.tau,
+    ) -> None:
+        gearbox = gearbox_motor(len(motors))
+        self.plant = LinearSystemId.DCMotorSystem(gearbox, moi, gearing)
+        self.motors = [rev.SparkSim(motor, gearbox) for motor in motors]
+        self.motor_sim = DCMotorSim(self.plant, gearbox)
+        self.velocity_conversion_factor = velocity_units_per_rad_per_sec
+
+    def update(self, dt: float) -> None:
+        vbus = self.motors[0].getBusVoltage()
+        self.motor_sim.setInputVoltage(self.motors[0].getAppliedOutput() * vbus)
+        self.motor_sim.update(dt)
+        velocity = self.motor_sim.getAngularVelocity() * self.velocity_conversion_factor
+        for motor in self.motors:
+            motor.iterate(velocity, vbus, dt)
+
+
 class SparkArmSim:
     def __init__(self, mech_sim: SingleJointedArmSim, motor_sim: rev.SparkSim) -> None:
         self.mech_sim = mech_sim
@@ -203,6 +231,14 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
 
         self.imu = robot.chassis.imu.sim_state
 
+        self.injector = SparkMotorSim(
+            DCMotor.NEO550,
+            robot.injector_component.injector_1,
+            robot.injector_component.injector_2,
+            gearing=1 / 1,
+            moi=0.000105679992,  # TODO: measure
+        )
+
         self.vision_sim = VisionSystemSim("main")
         self.vision_sim.addAprilTags(game.apriltag_layout)
         properties = SimCameraProperties.OV9281_1280_720()
@@ -222,9 +258,6 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
             robot.injector_component.algae_limit_switch
         )
         self.algae_pickup_counter = 0
-        self.floor_intake = robot.floor_intake
-        self.reef_intake = robot.reef_intake
-        self.algae_shooter = robot.algae_shooter
 
     def update_sim(self, now: float, tm_diff: float) -> None:
         # Enable the Phoenix6 simulated devices
@@ -277,25 +310,37 @@ def update_sim(self, now: float, tm_diff: float) -> None:
 
         # Update intake arm simulation
         self.intake_arm.update(tm_diff)
+        intake_arm_angle = self.intake_arm.mech_sim.getAngle()
         self.intake_arm_encoder_sim.set(
-            self.intake_arm.mech_sim.getAngle() + IntakeComponent.ARM_ENCODER_OFFSET
+            intake_arm_angle + IntakeComponent.ARM_ENCODER_OFFSET
         )
 
         # Update wrist simulation
         self.wrist.update(tm_diff)
-        self.wrist_encoder_sim.set(
-            self.wrist.mech_sim.getAngle() + WristComponent.ENCODER_ZERO_OFFSET
-        )
+        wrist_angle = self.wrist.mech_sim.getAngle()
+        self.wrist_encoder_sim.set(wrist_angle + WristComponent.ENCODER_ZERO_OFFSET)
+
+        self.injector.update(tm_diff)
+        injector_output = self.injector.motor_sim.getAngularVelocity()
 
         # Simulate algae pick up
-        if self.floor_intake.current_state == "intaking":
+        if (
+            wrist_angle < WristComponent.NEUTRAL_ANGLE
+            and intake_arm_angle < IntakeComponent.RETRACTED_ANGLE
+            and injector_output < 0
+        ):
             # Simulate driving around for a couple of seconds
             self.algae_pickup_counter += 1
             if self.algae_pickup_counter == 100:
                 self.algae_limit_switch_sim.setValue(False)
         else:
             self.algae_pickup_counter = 0
-        if self.reef_intake.current_state == "intaking":
+
+        # Reef intake
+        if (
+            wrist_angle > WristComponent.NEUTRAL_ANGLE + WristComponent.TOLERANCE
+            and injector_output < 0
+        ):
             # Check near reef
             pose = self.physics_controller.get_pose()
             pos = pose.translation()
@@ -304,6 +349,7 @@ def update_sim(self, now: float, tm_diff: float) -> None:
                 or pos.distance(game.RED_REEF_POS) < 2.0
             ):
                 self.algae_limit_switch_sim.setValue(False)
+
         # Algae shooting
-        if self.algae_shooter.current_state == "shooting":
+        if injector_output > 0:
             self.algae_limit_switch_sim.setValue(True)