implemented maple-sim to advanced swerve drive example

templates/AdvantageKit_AdvancedSwerveDriveProject/LICENSE-IronMaple

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 Shenzhen-Robotics-Alliance
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

templates/AdvantageKit_AdvancedSwerveDriveProject/README.md

@@ -0,0 +1,9 @@
+# Advantage Kit - Advanced Swerve Drive Project
+
+### With more enhanced swerve drive simulation using [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim).
+Original Project is [here](https://www.chiefdelphi.com/t/advantagekit-2024-log-replay-again/442968/54).
+
+To use the simulation in your own project, please refer to this [Commit Change-Log](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/commit/a0a469b3ec917b1246300f53d17d5558ad653e07).
+To see how to simulate intake for your robot, please refer to this [Commit Change-Log](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/commit/d13e542aaa6557ad946fd1f78dda3e133cbb6f5c)
+
+To run the simulation

templates/AdvantageKit_AdvancedSwerveDriveProject/build.gradle

@@ -49,15 +49,26 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
-// Configuration for AdvantageKit
 repositories {
+    // Repository for AdvantageKit
     maven {
         url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
         credentials {
             username = "Mechanical-Advantage-Bot"
             password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
         }
     }
+
+    // Repository for maple-sim
+    maven {
+        url = uri("https://maven.pkg.github.com/Shenzhen-Robotics-Alliance/maple-sim")
+        credentials {
+            username = "Shenzhen-Robotics-Alliance"
+            password = "\u0067\u0069\u0074\u0068\u0075\u0062\u005F\u0070\u0061\u0074\u005F\u0031\u0031\u0041\u0052\u0037\u0033\u0059\u004C\u0049\u0030\u0034\u0030\u0044\u004E\u006B\u0032\u006C\u0038\u004F\u004A\u006E\u0059\u005F\u0036\u0045\u0030\u006F\u0037\u004D\u0052\u004D\u0053\u0065\u006D\u0044\u0072\u0056\u006B\u0079\u0041\u006F\u0048\u004F\u0064\u0052\u007A\u0056\u0062\u0054\u0058\u0046\u004A\u0062\u0067\u006F\u0032\u0032\u0055\u0056\u0064\u0058\u0069\u004F\u0037\u0079\u0041\u004F\u0053\u0052\u004F\u005A\u0032\u0032\u0054\u0079\u006E\u0031\u0056\u0054\u004B\u006C\u0042"
+        }
+    }
+
+    // maven local
     mavenLocal()
 }
 
@@ -109,7 +120,7 @@ test {
 // AdvantageKit log replay from the command line. Set the
 // value to "true" to enable the sim GUI by default (this
 // is the standard WPILib behavior).
-wpi.sim.addGui().defaultEnabled = false
+wpi.sim.addGui().defaultEnabled = true
 wpi.sim.addDriverstation()
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/Constants.java

@@ -22,9 +22,9 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
-  public static enum Mode {
+  public enum Mode {
     /** Running on a real robot. */
     REAL,
 

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/Robot.java

@@ -156,5 +156,7 @@ public void simulationInit() {}
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    robotContainer.updateSimulationField();
+  }
 }

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/RobotContainer.java

@@ -16,24 +16,29 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
-import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.GyroIO;
-import frc.robot.subsystems.drive.GyroIOPigeon2;
-import frc.robot.subsystems.drive.ModuleIO;
-import frc.robot.subsystems.drive.ModuleIOSim;
-import frc.robot.subsystems.drive.ModuleIOSparkMax;
+import frc.robot.subsystems.IntakeExample;
+import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.flywheel.Flywheel;
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import java.util.List;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.drivesims.GyroSimulation;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
+import org.ironmaple.simulation.drivesims.SwerveModuleSimulation.DRIVE_WHEEL_TYPE;
+import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -44,9 +49,13 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  // Simulations are store in the robot container
+  private final SwerveDriveSimulation swerveDriveSimulation;
+
   // Subsystems
   private final Drive drive;
   private final Flywheel flywheel;
+  private final IntakeExample intake;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -60,7 +69,12 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.currentMode) {
       case REAL:
-        // Real robot, instantiate hardware IO implementations
+        /* Real robot, instantiate hardware IO implementations */
+
+        /* Disable Simulations */
+        this.swerveDriveSimulation = null;
+
+        /* Subsystems */
         drive =
             new Drive(
                 new GyroIOPigeon2(false),
@@ -76,22 +90,60 @@ public RobotContainer() {
         // new ModuleIOTalonFX(2),
         // new ModuleIOTalonFX(3));
         // flywheel = new Flywheel(new FlywheelIOTalonFX());
+        this.intake = null;
         break;
 
       case SIM:
-        // Sim robot, instantiate physics sim IO implementations
+        /* Sim robot, instantiate physics sim IO implementations */
+
+        /* create simulation for pigeon2 IMU (different IMUs have different measurement errors) */
+        final GyroSimulation gyroSimulation = GyroSimulation.createPigeon2();
+        /* create a swerve drive simulation */
+        this.swerveDriveSimulation =
+            new SwerveDriveSimulation(
+                45,
+                0.65,
+                0.65,
+                0.74,
+                0.74,
+                SwerveModuleSimulation.getMark4( // creates a mark4 module
+                    DCMotor.getKrakenX60(1), // drive motor is a Kraken x60
+                    DCMotor.getFalcon500(1), // steer motor is a Falcon 500
+                    80, // current limit: 80 Amps
+                    DRIVE_WHEEL_TYPE.RUBBER, // rubber wheels
+                    3 // l3 gear ratio
+                    ),
+                gyroSimulation,
+                new Pose2d( // initial starting pose on field, set it to where-ever you want
+                    3, 3, new Rotation2d()));
+        SimulatedArena.getInstance()
+            .addDriveTrainSimulation(swerveDriveSimulation); // register the drive train simulation
+
+        // reset the field for auto (placing game-pieces in positions)
+        SimulatedArena.getInstance().resetFieldForAuto();
+        this.intake = new IntakeExample(SimulatedArena.getInstance(), swerveDriveSimulation);
+
         drive =
             new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim());
+                new GyroIOSim(
+                    gyroSimulation), // GyroIOSim is a wrapper around gyro simulation, that reads
+                // the simulation result
+                /* ModuleIOSim are edited such that they also wraps around module simulations */
+                new ModuleIOSim(swerveDriveSimulation.getModules()[0]),
+                new ModuleIOSim(swerveDriveSimulation.getModules()[1]),
+                new ModuleIOSim(swerveDriveSimulation.getModules()[2]),
+                new ModuleIOSim(swerveDriveSimulation.getModules()[3]));
+
+        /* other subsystems are created with hardware simulation IOs */
         flywheel = new Flywheel(new FlywheelIOSim());
         break;
 
       default:
-        // Replayed robot, disable IO implementations
+        /* Replayed robot, disable IO implementations */
+
+        /* physics simulations are also not needed */
+        this.swerveDriveSimulation = null;
+        this.intake = null;
         drive =
             new Drive(
                 new GyroIO() {},
@@ -157,14 +209,24 @@ private void configureButtonBindings() {
             Commands.runOnce(
                     () ->
                         drive.setPose(
-                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+                            this.swerveDriveSimulation == null
+                                ? new Pose2d(drive.getPose().getTranslation(), new Rotation2d())
+                                : swerveDriveSimulation.getSimulatedDriveTrainPose()),
                     drive)
                 .ignoringDisable(true));
     controller
         .a()
         .whileTrue(
             Commands.startEnd(
                 () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+
+    if (intake != null) {
+      controller
+          .leftTrigger(0.5)
+          .onTrue(Commands.runOnce(intake::startIntake))
+          .onFalse(Commands.runOnce(intake::stopIntake));
+      controller.leftBumper().onTrue(Commands.runOnce(intake::clearGamePiece));
+    }
   }
 
   /**
@@ -175,4 +237,16 @@ private void configureButtonBindings() {
   public Command getAutonomousCommand() {
     return autoChooser.get();
   }
+
+  public void updateSimulationField() {
+    SimulatedArena.getInstance().simulationPeriodic();
+
+    Logger.recordOutput(
+        "FieldSimulation/RobotPosition", swerveDriveSimulation.getSimulatedDriveTrainPose());
+
+    final List<Pose3d> notes = SimulatedArena.getInstance().getGamePiecesByType("Note");
+    if (notes != null) Logger.recordOutput("FieldSimulation/Notes", notes.toArray(Pose3d[]::new));
+
+    intake.visualizeNoteInIntake();
+  }
 }

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/IntakeExample.java

@@ -0,0 +1,42 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import org.ironmaple.simulation.IntakeSimulation;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
+import org.littletonrobotics.junction.Logger;
+
+/** An example of a simulated intake, which will such a note if activated. */
+public class IntakeExample extends IntakeSimulation {
+  private final AbstractDriveTrainSimulation driveTrainSimulation;
+
+  public IntakeExample(SimulatedArena arena, AbstractDriveTrainSimulation driveTrainSimulation) {
+    super(arena, driveTrainSimulation, 0.6, IntakeSimulation.IntakeSide.BACK, 1);
+    this.driveTrainSimulation = driveTrainSimulation;
+  }
+
+  @Override
+  public void startIntake() {
+    super.startIntake();
+  }
+
+  @Override
+  public void stopIntake() {
+    super.stopIntake();
+  }
+
+  /** visualizes the note in the intake */
+  public void visualizeNoteInIntake() {
+    final Pose3d robotPose = new Pose3d(driveTrainSimulation.getSimulatedDriveTrainPose()),
+        noteInIntakePose =
+            robotPose.plus(new Transform3d(0.1, 0, 0.4, new Rotation3d(0, -Math.toRadians(60), 0)));
+    Logger.recordOutput(
+        "Intake/NoteInIntake", this.gamePieceCount != 0 ? noteInIntakePose : new Pose3d());
+  }
+
+  public void clearGamePiece() {
+    super.gamePieceCount = 0;
+  }
+}

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/GyroIOSim.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.drive;
+
+import frc.robot.util.OdometryTimeStampsSim;
+import org.ironmaple.simulation.drivesims.GyroSimulation;
+
+public class GyroIOSim implements GyroIO {
+  private final GyroSimulation gyroSimulation;
+
+  public GyroIOSim(GyroSimulation gyroSimulation) {
+    this.gyroSimulation = gyroSimulation;
+  }
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = true;
+    inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings();
+    inputs.odometryYawTimestamps = OdometryTimeStampsSim.getTimeStamps();
+    inputs.yawPosition = gyroSimulation.getGyroReading();
+    inputs.yawVelocityRadPerSec = gyroSimulation.getMeasuredAngularVelocityRadPerSec();
+  }
+}

templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/Module.java

@@ -19,7 +19,6 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import frc.robot.Constants;
 import org.littletonrobotics.junction.Logger;
 
 public class Module {
@@ -42,26 +41,11 @@ public Module(ModuleIO io, int index) {
     this.io = io;
     this.index = index;
 
-    // Switch constants based on mode (the physics simulator is treated as a
-    // separate robot with different tuning)
-    switch (Constants.currentMode) {
-      case REAL:
-      case REPLAY:
-        driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
-        driveFeedback = new PIDController(0.05, 0.0, 0.0);
-        turnFeedback = new PIDController(7.0, 0.0, 0.0);
-        break;
-      case SIM:
-        driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
-        driveFeedback = new PIDController(0.1, 0.0, 0.0);
-        turnFeedback = new PIDController(10.0, 0.0, 0.0);
-        break;
-      default:
-        driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0);
-        driveFeedback = new PIDController(0.0, 0.0, 0.0);
-        turnFeedback = new PIDController(0.0, 0.0, 0.0);
-        break;
-    }
+    // Unlike the original project, the physics simulator robot can be treated exactly like the real
+    // robot
+    driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
+    driveFeedback = new PIDController(0.05, 0.0, 0.0);
+    turnFeedback = new PIDController(7.0, 0.0, 0.0);
 
     turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
     setBrakeMode(true);