Skip to content

Commit

Permalink
improved code readability
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu committed Jan 11, 2025
1 parent f1af58c commit b27243d
Show file tree
Hide file tree
Showing 3 changed files with 77 additions and 244 deletions.
5 changes: 3 additions & 2 deletions ...TRE Swerve with maple-sim/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
178 changes: 0 additions & 178 deletions ...TRE Swerve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimDrivetrain.old
View file

This file was deleted.

138 changes: 74 additions & 64 deletions ...rve with maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,17 @@
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 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;
Expand All @@ -36,22 +34,6 @@
import org.ironmaple.simulation.motorsims.SimulatedMotorController;

public class MapleSimSwerveDrivetrain {
protected static class SimSwerveModule {
public final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
moduleConstant;
public final SwerveModuleSimulation moduleSimulation;
public final SimulatedMotorController.GenericMotorController driveMotor, steerMotor;

public SimSwerveModule(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> 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;
Expand All @@ -65,11 +47,12 @@ public MapleSimSwerveDrivetrain(
DCMotor steerMotorModel,
double wheelCOF,
Translation2d[] moduleLocations,
Pigeon2SimState pigeonSim,
Pigeon2 pigeon,
SwerveModule<TalonFX, TalonFX, CANcoder>[] modules,
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>...
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)
Expand All @@ -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);
Expand All @@ -99,52 +82,79 @@ public MapleSimSwerveDrivetrain(
* Update this simulation for the time duration.
*
* <p>This performs a simulation update on all the simulated devices
*
* @param modulesToApply What modules to apply the update to
*/
public final void update(SwerveModule<TalonFX, TalonFX, CANcoder>... 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
moduleConstant;
public final SwerveModuleSimulation moduleSimulation;

public SimSwerveModule(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> moduleConstant,
SwerveModuleSimulation moduleSimulation,
SwerveModule<TalonFX, TalonFX, CANcoder> 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(
Expand Down

0 comments on commit b27243d

Please sign in to comment.