diff --git a/physics.py b/physics.py index befa4098..c8b96b2d 100644 --- a/physics.py +++ b/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)