diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 940f1c8b..422c64e0 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -40,14 +40,12 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Optional; + +import java.util.*; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; @@ -232,8 +230,6 @@ public SwerveDrive( config.getAngleMotorSim(), config.physicalCharacteristics.conversionFactor.drive.gearRatio, config.physicalCharacteristics.conversionFactor.angle.gearRatio, - Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit), - Amps.of(20), Volts.of(config.physicalCharacteristics.driveFrictionVoltage), Volts.of(config.physicalCharacteristics.angleFrictionVoltage), Inches.of( @@ -247,7 +243,7 @@ public SwerveDrive( // feed module simulation instances to modules for (int i = 0; i < swerveModules.length; i++) { - this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i]); + this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i], config.physicalCharacteristics); } // register the drivetrain simulation @@ -784,6 +780,20 @@ public Pose2d getPose() return poseEstimation; } + /** + * Gets the maple-sim drivetrain simulation instance + * This is used to add intake simulation / launch game pieces from the robot + * + * @return an optional maple-sim {@link SwerveDriveSimulation} object, or {@link Optional#empty()} when calling from a real robot + */ + public Optional getMapleSimDrive() { + if (SwerveDriveTelemetry.isSimulation) { + return Optional.of(mapleSimDrive); + } + + return Optional.empty(); + } + /** * Gets the actual pose of the drivetrain during simulation * @@ -792,15 +802,10 @@ public Pose2d getPose() */ public Optional getSimulationDriveTrainPose() { - if (SwerveDriveTelemetry.isSimulation) - { - odometryLock.lock(); - Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose(); - odometryLock.unlock(); - return Optional.of(simulationPose); - } - - return Optional.empty(); + odometryLock.lock(); + Optional simulationPose = getMapleSimDrive().map(AbstractDriveTrainSimulation::getSimulatedDriveTrainPose); + odometryLock.unlock(); + return simulationPose; } /** @@ -818,27 +823,6 @@ public ChassisSpeeds getFieldVelocity() return robotRelativeSpeeds; } - /** - * Gets the actual field-relative robot velocity (x, y and omega) during simulation - * - * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty - * optional when running on real robot - * @deprecated for testing version of maple-sim only - */ - @Deprecated - public Optional getSimulationFieldVelocity() - { - if (SwerveDriveTelemetry.isSimulation) - { - odometryLock.lock(); - ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative(); - odometryLock.unlock(); - return Optional.of(simulationFieldRelativeVelocity); - } - - return Optional.empty(); - } - /** * Gets the current robot-relative velocity (x, y and omega) of the robot * @@ -849,27 +833,6 @@ public ChassisSpeeds getRobotVelocity() return kinematics.toChassisSpeeds(getStates()); } - /** - * Gets the actual robot-relative robot velocity (x, y and omega) during simulation - * - * @return An {@link Optional} {@link ChassisSpeeds} representing the actual robot-relative velocity of the robot, or - * an empty optional when running on real robot - * @deprecated for testing version of maple-sim only - */ - @Deprecated - public Optional getSimulationRobotVelocity() - { - if (SwerveDriveTelemetry.isSimulation) - { - odometryLock.lock(); - ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative(); - odometryLock.unlock(); - return Optional.of(simulationFieldRelativeVelocity); - } - - return Optional.empty(); - } - /** * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 9fd86a95..bba690e4 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -19,6 +19,7 @@ import swervelib.parser.Cache; import swervelib.parser.PIDFConfig; import swervelib.parser.SwerveModuleConfiguration; +import swervelib.parser.SwerveModulePhysicalCharacteristics; import swervelib.simulation.SwerveModuleSimulation; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -752,8 +753,8 @@ public SwerveModuleSimulation getSimModule() * configure with. */ public void configureModuleSimulation( - org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) + org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation, SwerveModulePhysicalCharacteristics physicalCharacteristics) { - this.simModule.configureSimModule(swerveModuleSimulation); + this.simModule.configureSimModule(swerveModuleSimulation, physicalCharacteristics); } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 5e365b84..138318ed 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -1,15 +1,12 @@ package swervelib.simulation; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Radian; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import org.ironmaple.simulation.motorsims.ControlRequest; +import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation; +import swervelib.parser.SwerveModulePhysicalCharacteristics; + +import static edu.wpi.first.units.Units.*; /** * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} @@ -17,19 +14,19 @@ public class SwerveModuleSimulation { - private org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule = null; + private SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null; /** * Configure the maple sim module * - * @param mapleSimModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation + * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation */ - public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) + public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule, SwerveModulePhysicalCharacteristics physicalCharacteristics) { - this.mapleSimModule = mapleSimModule; - mapleSimModule.getDriveMotorConfigs() - .withDefaultFeedForward(Volts.zero()) - .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0 / 3000.0), true); + this.mapleSimModule = new SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation(simModule); + this.mapleSimModule.withCurrentLimits( + Amps.of(physicalCharacteristics.driveMotorCurrentLimit), + Amps.of(physicalCharacteristics.angleMotorCurrentLimit)); } /** @@ -40,10 +37,7 @@ public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSi */ public void updateStateAndPosition(SwerveModuleState desiredState) { - mapleSimModule.requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); - mapleSimModule.requestDriveControl(new ControlRequest.VelocityVoltage( - RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS.in(Meters)) - )); + mapleSimModule.runModuleState(desiredState); } /** @@ -53,10 +47,7 @@ public void updateStateAndPosition(SwerveModuleState desiredState) */ public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - mapleSimModule.getDriveWheelFinalPosition().in(Radian) * mapleSimModule.WHEEL_RADIUS.in(Meters), - mapleSimModule.getSteerAbsoluteFacing() - ); + return mapleSimModule.getModulePosition(); } /** @@ -70,7 +61,7 @@ public SwerveModuleState getState() { return new SwerveModuleState(); } - SwerveModuleState state = mapleSimModule.getCurrentState(); + SwerveModuleState state = mapleSimModule.getMeasuredState(); state.angle = state.angle.minus(Rotation2d.kZero); return state; } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 4a50a0c8..2d843600 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.2", + "version": "0.2.3", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.2" + "version": "0.2.3" }, { "groupId": "org.dyn4j",