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

BroncBotz3481 · Jan 9, 2025 · bb9ff63 · bb9ff63
2 parents 196e630 + b659a4a
commit bb9ff63
Show file tree

Hide file tree

Showing 3 changed files with 74 additions and 7 deletions.
diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java
@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.telemetry.SwerveDriveTelemetry;
 
 /**
  * Class to perform tests on the swerve drive.
@@ -101,7 +102,7 @@ public static void powerAngleMotorsDutyCycle(SwerveDrive swerveDrive, double per
    * Power the drive motors for the swerve drive to a set voltage.
    *
    * @param swerveDrive {@link SwerveDrive} to control.
-   * @param volts       DutyCycle percentage of voltage to send to drive motors.
+   * @param volts       Voltage to send to drive motors.
    */
   public static void powerDriveMotorsVoltage(SwerveDrive swerveDrive, double volts)
   {
@@ -135,6 +136,34 @@ public static void centerModules(SwerveDrive swerveDrive)
     angleModules(swerveDrive, Rotation2d.fromDegrees(0));
   }
 
+  /**
+   * Set the sim modules to center to 0 and power them to drive in a voltage.
+   * Calling this function in sim is equivalent to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real robot.
+   *
+   * @param swerveDrive {@link SwerveDrive} to control.
+   * @param volts       Voltage to send to drive motors.
+   */
+  public static void runDriveMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts) {
+    for (SwerveModule module: swerveDrive.getModules())
+    {
+      module.getSimModule().runDriveMotorCharacterization(Rotation2d.kZero, volts);
+    }
+  }
+
+  /**
+   * Set the sim modules to center to 0 and power them to drive in a voltage.
+   * Calling this function in sim is equivalent to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real robot.
+   *
+   * @param swerveDrive {@link SwerveDrive} to control.
+   * @param volts       Voltage to send to angle motors.
+   */
+  public static void runAngleMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts) {
+    for (SwerveModule module: swerveDrive.getModules())
+    {
+      module.getSimModule().runAngleMotorCharacterization(volts);
+    }
+  }
+
   /**
    * Find the minimum amount of power required to move the swerve drive motors.
    *
@@ -310,8 +339,14 @@ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swe
   {
     return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
         (Voltage voltage) -> {
-          SwerveDriveTest.centerModules(swerveDrive);
-          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+          if (!SwerveDriveTelemetry.isSimulation)
+          {
+            SwerveDriveTest.centerModules(swerveDrive);
+            SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+          } else
+          {
+            SwerveDriveTest.runDriveMotorsCharacterizationOnSimModules(swerveDrive, voltage.in(Volts));
+          }
         },
         log -> {
           for (SwerveModule module : swerveDrive.getModules())
@@ -382,8 +417,14 @@ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swe
   {
     return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
         (Voltage voltage) -> {
-          SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
-          SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+          if (!SwerveDriveTelemetry.isSimulation)
+          {
+            SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
+            SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+          } else
+          {
+            SwerveDriveTest.runAngleMotorsCharacterizationOnSimModules(swerveDrive, voltage.in(Volts));
+          }
         },
         log -> {
           for (SwerveModule module : swerveDrive.getModules())

diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
+import swervelib.SwerveDrive;
 import swervelib.parser.SwerveModulePhysicalCharacteristics;
 
 /**
@@ -42,6 +43,31 @@ public void updateStateAndPosition(SwerveModuleState desiredState)
     mapleSimModule.runModuleState(desiredState);
   }
 
+  /**
+   * Runs a drive motor characterization on the sim module.
+   * This is called from {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double)}  to run sysId during simulation
+   *
+   * @param desiredFacing the desired facing of the module
+   * @param volts the voltage to run
+   * @see org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation#runDriveMotorCharacterization(Rotation2d, double).
+   */
+  public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts)
+  {
+    mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts);
+  }
+
+  /**
+   * Runs a drive motor characterization on the sim module.
+   * This method is called from {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId during simulation
+   *
+   * @param volts the voltage to run
+   * @see org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation#runSteerMotorCharacterization(double).
+   */
+  public void runAngleMotorCharacterization(double volts)
+  {
+    mapleSimModule.runSteerMotorCharacterization(volts);
+  }
+
   /**
    * Get the simulated swerve module position.
    *

diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.3.0",
+    "version": "0.3.1",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.3.0"
+            "version": "0.3.1"
         },
         {
             "groupId": "org.dyn4j",