Add simple shooter flywheel sim

physics.py

@@ -7,6 +7,7 @@
 
 import wpilib.simulation
 from wpimath.kinematics import SwerveDrive4Kinematics, SwerveModuleState
+from wpimath.system.plant import DCMotor
 
 from components.chassis import SwerveModule
 from components.turret import Turret
@@ -80,6 +81,16 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):
             for module in robot.chassis.modules
         ]
 
+        shooter_wheels_moi = 0.000727856
+        self.flywheel = wpilib.simulation.FlywheelSim(
+            DCMotor.falcon500(2),
+            gearing=1 / 1,
+            moi=shooter_wheels_moi,
+            measurementStdDevs=[0.01],
+        )
+        self.left_shooter = robot.shooter_left_motor.getSimCollection()
+        self.right_shooter = robot.shooter_right_motor.getSimCollection()
+
         self.imu = wpilib.simulation.SimDeviceSim("navX-Sensor", 4)
         self.imu_yaw = self.imu.getDouble("Yaw")
 
@@ -92,6 +103,14 @@ def update_sim(self, now: float, tm_diff: float) -> None:
         for steer in self.steer:
             steer.update(tm_diff)
 
+        self.flywheel.setInputVoltage(self.left_shooter.getMotorOutputLeadVoltage())
+        self.flywheel.update(tm_diff)
+        shooter_vel = int(
+            self.flywheel.getAngularVelocity() / math.tau * FALCON_CPR / 10
+        )
+        self.left_shooter.setIntegratedSensorVelocity(shooter_vel)
+        self.right_shooter.setIntegratedSensorVelocity(-shooter_vel)
+
         states = typing.cast(
             typing.Tuple[
                 SwerveModuleState,

robot.py

@@ -41,7 +41,7 @@ class MyRobot(magicbot.MagicRobot):
     lock_motion_while_shooting = magicbot.tunable(False)
     test_chassis_speed = magicbot.tunable(2)
 
-    def createObjects(self):
+    def createObjects(self) -> None:
         self.logger.info("pyrapidreact %s", GIT_INFO)
         self.data_log = wpilib.DataLogManager.getLog()
 
@@ -63,8 +63,8 @@ def createObjects(self):
         self.recorded_is_local_driving = False
         self.codriver = wpilib.XboxController(1)
 
-        self.shooter_left_motor = ctre.TalonFX(11)
-        self.shooter_right_motor = ctre.TalonFX(10)
+        self.shooter_left_motor = ctre.WPI_TalonFX(11)
+        self.shooter_right_motor = ctre.WPI_TalonFX(10)
 
         self.turret_motor = ctre.WPI_TalonSRX(15)
         self.turret_absolute_encoder = wpilib.DutyCycleEncoder(10)  # navX pin 0