diff --git a/physics.py b/physics.py index 42a6ded..f657a15 100644 --- a/physics.py +++ b/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, diff --git a/robot.py b/robot.py index 1dc6118..5e188e5 100755 --- a/robot.py +++ b/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