Merge pull request #262 from Shenzhen-Robotics-Alliance/dev

maple-sim new version
BroncBotz3481 · Dec 9, 2024 · d9cff95 · d9cff95
2 parents 7fe558c + 4d126bf
commit d9cff95
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 86 deletions.
diff --git 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<SwerveDriveSimulation> 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<Pose2d> getSimulationDriveTrainPose()
   {
-    if (SwerveDriveTelemetry.isSimulation)
-    {
-      odometryLock.lock();
-      Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose();
-      odometryLock.unlock();
-      return Optional.of(simulationPose);
-    }
-
-    return Optional.empty();
+    odometryLock.lock();
+    Optional<Pose2d> 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<ChassisSpeeds> 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<ChassisSpeeds> 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
@@ -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
@@ -1,35 +1,32 @@
 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}
  */
 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
@@ -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",