diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index e753fb43..f8096ab8 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -20,9 +20,6 @@ * on a roboRIO. */ public final class Constants { - public static double SIM_TIME_PERIOD = - 0.02; // Update physics simulations every 20ms (like the actual bot). - public static final Transform3d FRONT_CAMERA_POSITION = new Transform3d( new Translation3d(Meters.of(-0.054564), Meters.of(0), Meters.of(0.501754)), diff --git a/src/main/java/com/team2813/subsystems/Simulation.java b/src/main/java/com/team2813/subsystems/Simulation.java new file mode 100644 index 00000000..67d1dd0e --- /dev/null +++ b/src/main/java/com/team2813/subsystems/Simulation.java @@ -0,0 +1,26 @@ +package com.team2813.subsystems; + +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import org.littletonrobotics.junction.LoggedRobot; + +/** Utilities for working in simulation. */ +public class Simulation { + /** + * The time period to use in simulation. + * + *

Currently set to the same time period as we use in the actual robot. + */ + public static final double TIME_PERIOD = LoggedRobot.defaultPeriodSecs; + + /** Gets the voltage to apply to simulated motors. */ + public static Voltage getMotorSupplyVoltage() { + return Volts.of(RobotController.getBatteryVoltage()); + } + + private Simulation() { + throw new AssertionError("Not instantiable"); + } +} diff --git a/src/main/java/com/team2813/subsystems/drive/ModuleIOSim.java b/src/main/java/com/team2813/subsystems/drive/ModuleIOSim.java index 184a830e..6a30d914 100644 --- a/src/main/java/com/team2813/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/com/team2813/subsystems/drive/ModuleIOSim.java @@ -7,9 +7,12 @@ package com.team2813.subsystems.drive; +import static edu.wpi.first.units.Units.Volts; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -83,8 +86,9 @@ public void updateInputs(ModuleIOInputs inputs) { } // Update simulation state - driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + double supplyVoltage = Simulation.getMotorSupplyVoltage().in(Volts); + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -supplyVoltage, supplyVoltage)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -supplyVoltage, supplyVoltage)); driveSim.update(0.02); turnSim.update(0.02); diff --git a/src/main/java/com/team2813/subsystems/hopper/HopperIOSim.java b/src/main/java/com/team2813/subsystems/hopper/HopperIOSim.java index 9463f076..880b7c2c 100644 --- a/src/main/java/com/team2813/subsystems/hopper/HopperIOSim.java +++ b/src/main/java/com/team2813/subsystems/hopper/HopperIOSim.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team2813.Constants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Voltage; @@ -56,9 +57,10 @@ public HopperIOSim() { public void updateState(HopperIOInputs inputs) { updateSimulation(); - mainRollerMotorSimState.setSupplyVoltage(Volts.of(12)); - followerRollerMotorSimState.setSupplyVoltage(Volts.of(12)); - feederMotorSimState.setSupplyVoltage(Volts.of(12)); + Voltage supplyVoltage = Simulation.getMotorSupplyVoltage(); + mainRollerMotorSimState.setSupplyVoltage(supplyVoltage); + followerRollerMotorSimState.setSupplyVoltage(supplyVoltage); + feederMotorSimState.setSupplyVoltage(supplyVoltage); inputs.mainRollerMotorVoltage = mainRollerMotor.getMotorVoltage().getValue(); inputs.mainRollerMotorRPS = mainRollerMotor.getRotorVelocity().getValue(); @@ -73,8 +75,8 @@ public void updateState(HopperIOInputs inputs) { inputs.feederCurrent = feederMotor.getStatorCurrent().getValue(); } - public void updateSimulation() { - rollerSim.update(Constants.SIM_TIME_PERIOD); + private void updateSimulation() { + rollerSim.update(Simulation.TIME_PERIOD); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them. diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java index f88d2069..58a714f0 100644 --- a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java +++ b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team2813.Constants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -59,7 +60,7 @@ public IntakeExtensionIOSim() { @Override public void updateState(IntakeExtensionIOInputs inputs) { // Continue supplying the simulated motor with 12V voltage. - extenderMotorSimState.setSupplyVoltage(12); + extenderMotorSimState.setSupplyVoltage(Simulation.getMotorSupplyVoltage()); // TalonFX simulation doesn't run the internal PID controller, so we simulate it ourselves // using a WPILib PIDController with the same gains. diff --git a/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerIOSim.java b/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerIOSim.java index f461c9ba..73b351d8 100644 --- a/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerIOSim.java +++ b/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerIOSim.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team2813.Constants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Voltage; @@ -35,14 +36,14 @@ public IntakeRollerIOSim() { public void updateState(IntakeRollerIOInputs inputs) { updateSimulation(); - intakeSimState.setSupplyVoltage(Volts.of(12)); + intakeSimState.setSupplyVoltage(Simulation.getMotorSupplyVoltage()); inputs.intakeMotorVoltage = intakeMotor.getMotorVoltage().getValue(); inputs.intakeMotorRPS = intakeMotor.getVelocity().getValue(); inputs.intakeMotorCurrent = intakeMotor.getStatorCurrent().getValue(); } - public void updateSimulation() { + private void updateSimulation() { // TODO: Once we fetch from main, change this to the SIM_TIME in Constants. intakeFlywheelSim.update(0.02); diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java index 65a7afcb..87bd2f92 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team2813.Constants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Voltage; @@ -44,12 +45,12 @@ public void updateState(KickerIOInputs inputs) { } private void updateSimulation() { - flywheelSim.update(Constants.SIM_TIME_PERIOD); + flywheelSim.update(Simulation.TIME_PERIOD); TalonFXSimState simState = motor.getSimState(); simState.setRotorAcceleration(flywheelSim.getAngularAcceleration()); simState.setRotorVelocity(flywheelSim.getAngularVelocity()); - simState.setSupplyVoltage(Volts.of(12)); + simState.setSupplyVoltage(Simulation.getMotorSupplyVoltage()); } @Override diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 7bf08eb2..3c57ac12 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.TalonFXSimState; import com.team2813.Constants; +import com.team2813.subsystems.Simulation; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; @@ -49,8 +50,9 @@ public ShooterIOSim() { public void updateState(ShooterIOInputs inputs) { updateSimulation(); - mainShooterSimState.setSupplyVoltage(Volts.of(12)); - followerShooterSimState.setSupplyVoltage(Volts.of(12)); + Voltage supplyVoltage = Simulation.getMotorSupplyVoltage(); + mainShooterSimState.setSupplyVoltage(supplyVoltage); + followerShooterSimState.setSupplyVoltage(supplyVoltage); inputs.mainShooterMotorVoltageVolts = mainShooterMotor.getMotorVoltage().getValue().in(Volts); inputs.mainShooterMotorAngleRotations = mainShooterMotor.getPosition().getValue().in(Rotations); @@ -67,9 +69,9 @@ public void updateState(ShooterIOInputs inputs) { followerShooterMotor.getStatorCurrent().getValue().in(Amps); } - public void updateSimulation() { + private void updateSimulation() { // Update physics simulations every 20ms (like the actual bot). - shooterSim.update(Constants.SIM_TIME_PERIOD); + shooterSim.update(Simulation.TIME_PERIOD); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them.