diff --git a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e975287..bfd8cbb 100644 --- a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -278,13 +278,14 @@ private void startSimThread() { DCMotor.getFalcon500(1), 1.2, getModuleLocations(), - getPigeon2().getSimState(), + getPigeon2(), + getModules(), TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight); /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> mapleSimSwerveDrivetrain.update(getModules())); + m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update); m_simNotifier.startPeriodic(kSimLoopPeriod); } } diff --git a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimDrivetrain.old b/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimDrivetrain.old deleted file mode 100644 index ce5f25f..0000000 --- a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimDrivetrain.old +++ /dev/null @@ -1,178 +0,0 @@ -package frc.robot.utils.simulation; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.ctre.phoenix6.swerve.SwerveModule; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.*; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.COTS; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.ironmaple.simulation.motorsims.SimulatedBattery; -import org.ironmaple.simulation.motorsims.SimulatedMotorController; - -import static edu.wpi.first.units.Units.*; - -public class MapleSimDrivetrain { - private final double periodSeconds; - private final SwerveDriveSimulation driveSimulation; - private final MapleSimModule[] simModules; - private final Pigeon2SimState imuSim; - - public MapleSimDrivetrain( - double simPeriodSeconds, - Mass robotMassWithBumpers, - Distance bumperLengthX, - Distance bumperWidthY, - DCMotor driveMotorModel, - DCMotor steerMotorModel, - double wheelCOF, - Translation2d[] moduleLocations, - SwerveModuleConstants[] moduleConstants, - SwerveModule[] modules, - Pigeon2 imu - ) { - this.simModules = new MapleSimModule[moduleLocations.length]; - DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() - .withRobotMass(robotMassWithBumpers) - .withBumperSize(bumperLengthX, bumperWidthY) - .withGyro(COTS.ofPigeon2()) - .withCustomModuleTranslations(moduleLocations) - .withSwerveModule(new SwerveModuleSimulationConfig( - driveMotorModel, - steerMotorModel, - moduleConstants[0].DriveMotorGearRatio, - moduleConstants[1].SteerMotorGearRatio, - Volts.of(moduleConstants[0].DriveFrictionVoltage), - Volts.of(moduleConstants[0].SteerFrictionVoltage), - Meters.of(moduleConstants[0].WheelRadius), - KilogramSquareMeters.of(moduleConstants[0].SteerInertia), - wheelCOF)); - driveSimulation = new SwerveDriveSimulation(simulationConfig, new Pose2d()); - - for (int i = 0; i < simModules.length; i++) - simModules[i] = new MapleSimModule( - driveSimulation.getModules()[i], - moduleConstants[i].DriveMotorInverted, - moduleConstants[i].SteerMotorInverted, - moduleConstants[i].EncoderInverted, - modules[i]); - this.imuSim = imu.getSimState(); - - this.periodSeconds = simPeriodSeconds; - SimulatedArena.overrideSimulationTimings(Seconds.of(periodSeconds), 1); - SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation); - } - - public SwerveDriveSimulation driveSimulation() { - return driveSimulation; - } - - public void update() { - SimulatedArena.getInstance().simulationPeriodic(); - imuSim.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - imuSim.setRawYaw(driveSimulation.getSimulatedDriveTrainPose().getRotation().getMeasure()); - imuSim.setAngularVelocityZ(RadiansPerSecond.of( - driveSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative().omegaRadiansPerSecond)); - } - - protected static class MapleSimModule { - public final SwerveModuleSimulation moduleSimulation; - protected MapleSimModule( - SwerveModuleSimulation moduleSimulation, - boolean driveMotorInverted, boolean steerMotorInverted, boolean encoderInverted, - SwerveModule module) { - this.moduleSimulation = moduleSimulation; - moduleSimulation.useDriveMotorController(new TalonFXMotorControllerSim( - module.getDriveMotor(), driveMotorInverted)); - moduleSimulation.useSteerMotorController(new TalonFXMotorControllerWithRemoteCanCoderSim( - module.getSteerMotor(), steerMotorInverted, module.getEncoder(), encoderInverted, Degrees.zero())); - } - } - - public static class TalonFXMotorControllerSim implements SimulatedMotorController { - public final int id; - - private final TalonFXSimState talonFXSimState; - private final boolean motorInverted; - public TalonFXMotorControllerSim(TalonFX talonFX, boolean motorInverted) { - this.id = talonFX.getDeviceID(); - this.motorInverted = motorInverted; - this.talonFXSimState = talonFX.getSimState(); - } - - @Override - public Voltage updateControlSignal( - Angle mechanismAngle, - AngularVelocity mechanismVelocity, - Angle encoderAngle, - AngularVelocity encoderVelocity) { - talonFXSimState.Orientation = - motorInverted ? ChassisReference.Clockwise_Positive : ChassisReference.CounterClockwise_Positive; - talonFXSimState.setRawRotorPosition(encoderAngle); - talonFXSimState.setRotorVelocity(encoderVelocity); - talonFXSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - - DogLog.log("CTRE Sim/Motor" + id + "/Inverted", motorInverted); - DogLog.log("CTRE Sim/Motor" + id + "/EncoderAngle (Rot)", encoderAngle.in(Rotations)); - DogLog.log("CTRE Sim/Motor" + id + "/EncoderVelocity (RPM)", encoderVelocity.in(RPM)); - DogLog.log("CTRE Sim/Motor" + id + "/Output Voltage", talonFXSimState.getMotorVoltageMeasure().in(Volts)); - return talonFXSimState.getMotorVoltageMeasure(); - } - } - - public static class TalonFXMotorControllerWithRemoteCanCoderSim extends TalonFXMotorControllerSim { - private final int encoderId; - private final boolean encoderInverted; - private final CANcoderSimState remoteCancoderSimState; - private final Angle encoderOffset; - public TalonFXMotorControllerWithRemoteCanCoderSim( - TalonFX talonFX, - boolean motorInverted, - CANcoder cancoder, - boolean encoderInverted, - Angle encoderOffset) { - super(talonFX, motorInverted); - this.remoteCancoderSimState = cancoder.getSimState(); - this.encoderInverted = encoderInverted; - this.encoderOffset = encoderOffset; - - this.encoderId = cancoder.getDeviceID(); - } - - @Override - public Voltage updateControlSignal( - Angle mechanismAngle, - AngularVelocity mechanismVelocity, - Angle encoderAngle, - AngularVelocity encoderVelocity) { - this.remoteCancoderSimState.Orientation = - encoderInverted ? ChassisReference.Clockwise_Positive : ChassisReference.CounterClockwise_Positive; - - remoteCancoderSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - remoteCancoderSimState.setRawPosition(mechanismAngle.minus(encoderOffset)); - remoteCancoderSimState.setVelocity(mechanismVelocity); - - System.out.println("encoder id " + encoderId +" inverted: " + encoderInverted); - DogLog.log("CTRE Sim/CanCoder" + encoderId + "/Inverted", encoderInverted); - DogLog.log("CTRE Sim/CanCoder" + encoderId + "/EncoderAngle (Rot)", mechanismAngle.in(Rotations)); - DogLog.log("CTRE Sim/CanCoder" + encoderId + "/EncoderVelocity (RPM)", mechanismVelocity.in(RPM)); - - return super.updateControlSignal(mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity); - } - } -} diff --git a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java b/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java index a2ceeda..5220cde 100644 --- a/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java +++ b/templates/CTRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java @@ -12,9 +12,9 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.Pigeon2SimState; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.swerve.SwerveModule; @@ -22,9 +22,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Mass; -import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.RobotBase; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.COTS; @@ -36,22 +34,6 @@ import org.ironmaple.simulation.motorsims.SimulatedMotorController; public class MapleSimSwerveDrivetrain { - protected static class SimSwerveModule { - public final SwerveModuleConstants - moduleConstant; - public final SwerveModuleSimulation moduleSimulation; - public final SimulatedMotorController.GenericMotorController driveMotor, steerMotor; - - public SimSwerveModule( - SwerveModuleConstants moduleConstant, - SwerveModuleSimulation moduleSimulation) { - this.moduleConstant = moduleConstant; - this.moduleSimulation = moduleSimulation; - this.driveMotor = moduleSimulation.useGenericMotorControllerForDrive(); - this.steerMotor = moduleSimulation.useGenericControllerForSteer(); - } - } - private final Pigeon2SimState pigeonSim; private final SimSwerveModule[] simModules; public final SwerveDriveSimulation mapleSimDrive; @@ -65,11 +47,12 @@ public MapleSimSwerveDrivetrain( DCMotor steerMotorModel, double wheelCOF, Translation2d[] moduleLocations, - Pigeon2SimState pigeonSim, + Pigeon2 pigeon, + SwerveModule[] modules, SwerveModuleConstants... moduleConstants) { - this.pigeonSim = pigeonSim; - simModules = new SimSwerveModule[4]; + this.pigeonSim = pigeon.getSimState(); + simModules = new SimSwerveModule[moduleConstants.length]; DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() .withRobotMass(robotMassWithBumpers) .withBumperSize(bumperLengthX, bumperWidthY) @@ -89,7 +72,7 @@ public MapleSimSwerveDrivetrain( SwerveModuleSimulation[] moduleSimulations = mapleSimDrive.getModules(); for (int i = 0; i < this.simModules.length; i++) - simModules[i] = new SimSwerveModule(moduleConstants[0], moduleSimulations[i]); + simModules[i] = new SimSwerveModule(moduleConstants[0], moduleSimulations[i], modules[i]); SimulatedArena.overrideSimulationTimings(Seconds.of(simPeriodSeconds), 1); SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); @@ -99,52 +82,79 @@ public MapleSimSwerveDrivetrain( * Update this simulation for the time duration. * *

This performs a simulation update on all the simulated devices - * - * @param modulesToApply What modules to apply the update to */ - public final void update(SwerveModule... modulesToApply) { - if (modulesToApply.length != simModules.length) - throw new IllegalArgumentException( - "Modules length incorrect!!! (given " + modulesToApply.length + " modules)"); - - for (int i = 0; i < simModules.length; ++i) { - TalonFXSimState driveTalonSim = modulesToApply[i].getDriveMotor().getSimState(); - TalonFXSimState steerTalonSim = modulesToApply[i].getSteerMotor().getSimState(); - CANcoderSimState encoderSim = modulesToApply[i].getEncoder().getSimState(); - - driveTalonSim.Orientation = simModules[i].moduleConstant.DriveMotorInverted - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; - steerTalonSim.Orientation = simModules[i].moduleConstant.SteerMotorInverted - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; - encoderSim.Orientation = simModules[i].moduleConstant.EncoderInverted - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; - - Voltage batteryVoltage = SimulatedBattery.getBatteryVoltage(); - driveTalonSim.setSupplyVoltage(batteryVoltage); - steerTalonSim.setSupplyVoltage(batteryVoltage); - encoderSim.setSupplyVoltage(batteryVoltage); - - simModules[i].driveMotor.requestVoltage(Volts.of(driveTalonSim.getMotorVoltage())); - simModules[i].steerMotor.requestVoltage(Volts.of(steerTalonSim.getMotorVoltage())); - - driveTalonSim.setRawRotorPosition(simModules[i].moduleSimulation.getDriveEncoderUnGearedPosition()); - driveTalonSim.setRotorVelocity(simModules[i].moduleSimulation.getDriveEncoderUnGearedSpeed()); - - steerTalonSim.setRawRotorPosition(simModules[i].moduleSimulation.getSteerRelativeEncoderPosition()); - steerTalonSim.setRotorVelocity(simModules[i].moduleSimulation.getSteerRelativeEncoderVelocity()); - - encoderSim.setRawPosition(simModules[i].moduleSimulation.getSteerAbsoluteAngle()); - encoderSim.setVelocity(simModules[i].moduleSimulation.getSteerAbsoluteEncoderSpeed()); - } + public final void update() { + SimulatedArena.getInstance().simulationPeriodic(); pigeonSim.setRawYaw( mapleSimDrive.getSimulatedDriveTrainPose().getRotation().getMeasure()); pigeonSim.setAngularVelocityZ(RadiansPerSecond.of( mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative().omegaRadiansPerSecond)); + } - SimulatedArena.getInstance().simulationPeriodic(); + protected static class SimSwerveModule { + public final SwerveModuleConstants + moduleConstant; + public final SwerveModuleSimulation moduleSimulation; + + public SimSwerveModule( + SwerveModuleConstants moduleConstant, + SwerveModuleSimulation moduleSimulation, + SwerveModule module) { + this.moduleConstant = moduleConstant; + this.moduleSimulation = moduleSimulation; + moduleSimulation.useDriveMotorController(new TalonFXMotorControllerSim(module.getDriveMotor())); + moduleSimulation.useSteerMotorController( + new TalonFXMotorControllerWithRemoteCanCoderSim(module.getSteerMotor(), module.getEncoder())); + } + } + + public static class TalonFXMotorControllerSim implements SimulatedMotorController { + public final int id; + + private final TalonFXSimState talonFXSimState; + + public TalonFXMotorControllerSim(TalonFX talonFX) { + this.id = talonFX.getDeviceID(); + this.talonFXSimState = talonFX.getSimState(); + } + + @Override + public Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity) { + talonFXSimState.setRawRotorPosition(encoderAngle); + talonFXSimState.setRotorVelocity(encoderVelocity); + talonFXSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); + + return talonFXSimState.getMotorVoltageMeasure(); + } + } + + public static class TalonFXMotorControllerWithRemoteCanCoderSim extends TalonFXMotorControllerSim { + private final int encoderId; + private final CANcoderSimState remoteCancoderSimState; + + public TalonFXMotorControllerWithRemoteCanCoderSim(TalonFX talonFX, CANcoder cancoder) { + super(talonFX); + this.remoteCancoderSimState = cancoder.getSimState(); + + this.encoderId = cancoder.getDeviceID(); + } + + @Override + public Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity) { + remoteCancoderSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); + remoteCancoderSimState.setRawPosition(mechanismAngle); + remoteCancoderSimState.setVelocity(mechanismVelocity); + + return super.updateControlSignal(mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity); + } } public static SwerveModuleConstants[] regulateModuleConstantsForSimulation(