maple sim swerve sim

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -1,20 +1,26 @@
 package org.curtinfrc.frc2025;
 
+import static edu.wpi.first.units.Units.Inches;
 import static org.curtinfrc.frc2025.subsystems.intake.IntakeConstants.intakeVolts;
 import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.*;
 
 import choreo.auto.AutoFactory;
 import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.Set;
 import org.curtinfrc.frc2025.Constants.Mode;
+import org.curtinfrc.frc2025.Constants.RobotType;
 import org.curtinfrc.frc2025.Constants.Setpoints;
 import org.curtinfrc.frc2025.generated.TunerConstants;
 import org.curtinfrc.frc2025.subsystems.drive.Drive;
@@ -44,6 +50,10 @@
 import org.curtinfrc.frc2025.subsystems.vision.VisionIOQuestNav;
 import org.curtinfrc.frc2025.util.AutoChooser;
 import org.curtinfrc.frc2025.util.VirtualSubsystem;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.drivesims.COTS;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -75,6 +85,10 @@ public class Robot extends LoggedRobot {
   private final AutoFactory autoFactory;
   private final Autos autos;
 
+  private Joystick simJoystick; // Joystick for simulation input
+  private final Trigger Z = new Trigger(() -> simJoystick.getRawAxis(0) == 1);
+  private final Trigger X = new Trigger(() -> simJoystick.getRawAxis(1) == 1);
+
   public Robot() {
     // Record metadata
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
@@ -167,15 +181,39 @@ public Robot() {
 
         case SIMBOT -> {
           // Sim robot, instantiate physics sim IO implementations
+          // Create and configure a drivetrain simulation configuration
+          final DriveTrainSimulationConfig driveTrainSimulationConfig =
+              DriveTrainSimulationConfig.Default()
+                  // Specify gyro type (for realistic gyro drifting and error simulation)
+                  .withGyro(COTS.ofPigeon2())
+                  // Specify swerve module (for realistic swerve dynamics)
+                  .withSwerveModule(
+                      COTS.ofMark4(
+                          DCMotor.getKrakenX60Foc(1), // Drive motor is a Kraken X60
+                          DCMotor.getFalcon500(1), // Steer motor is a Falcon 500
+                          COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels
+                          3)) // L3 Gear ratio
+                  // Configures the track length and track width (spacing between swerve modules)
+                  .withTrackLengthTrackWidth(Inches.of(24), Inches.of(24))
+                  // Configures the bumper size (dimensions of the robot bumper)
+                  .withBumperSize(Inches.of(30), Inches.of(30));
+
+          final SwerveDriveSimulation swerveDriveSimulation =
+              new SwerveDriveSimulation(
+                  // Specify Configuration
+                  driveTrainSimulationConfig,
+                  // Specify starting pose
+                  new Pose2d(3, 3, new Rotation2d()));
+
+          SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation);
+
           drive =
               new Drive(
-                  new GyroIOSim(
-                      () -> drive.getKinematics(),
-                      () -> drive.getModuleStates()) {}, // work around crash
-                  new ModuleIOSim(TunerConstants.FrontLeft),
-                  new ModuleIOSim(TunerConstants.FrontRight),
-                  new ModuleIOSim(TunerConstants.BackLeft),
-                  new ModuleIOSim(TunerConstants.BackRight));
+                  new GyroIOSim(swerveDriveSimulation.getGyroSimulation()) {}, // work around crash
+                  new ModuleIOSim(swerveDriveSimulation.getModules()[0]),
+                  new ModuleIOSim(swerveDriveSimulation.getModules()[1]),
+                  new ModuleIOSim(swerveDriveSimulation.getModules()[2]),
+                  new ModuleIOSim(swerveDriveSimulation.getModules()[3]));
           vision =
               new Vision(
                   drive::addVisionMeasurement,
@@ -186,6 +224,8 @@ public Robot() {
           elevator = new Elevator(new ElevatorIO() {});
           intake = new Intake(new IntakeIOSim());
           ejector = new Ejector(new EjectorIOSim());
+
+          simJoystick = new Joystick(0); // Assuming keyboard/joystick is ID 0
         }
       }
     } else {
@@ -262,12 +302,35 @@ public Robot() {
     RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler());
 
     // Default command, normal field-relative drive
-    drive.setDefaultCommand(
-        drive.joystickDrive(
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
+    if (Constants.robotType == RobotType.SIMBOT) {
+      // drive.setDefaultCommand(
+      // drive.joystickDrive(
+      // () -> -simJoystick.getRawAxis(0), () -> -simJoystick.getRawAxis(1), () -> 0));
+      drive.setDefaultCommand(drive.joystickDrive(() -> 0, () -> 0, () -> 0));
+      Z.whileTrue(superstructure.align(Setpoints.L2));
+      X.whileTrue(superstructure.align(Setpoints.COLLECT));
+    } else {
+      drive.setDefaultCommand(
+          drive.joystickDrive(
+              () -> -controller.getLeftY(),
+              () -> -controller.getLeftX(),
+              () -> -controller.getRightX()));
+
+      controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3));
+      controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2));
+      controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT));
+    }
+
+    elevator.setDefaultCommand(
+        Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator)));
+
+    // controller
+    // .rightBumper()
+    // .negate()
+    // .and(controller.leftBumper().negate())
+    // .onTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator)));
 
+    // controller.a().whileTrue(elevator.zero());
     elevator
         .isNotAtCollect
         .and(elevator.atSetpoint)
@@ -278,6 +341,16 @@ public Robot() {
     ejector.setDefaultCommand(ejector.stop());
     elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT));
 
+    drive
+        .atSetpointPose
+        .and(elevator.isNotAtCollect)
+        .and(elevator.atSetpoint)
+        .onTrue(
+            ejector.eject(1000).until(elevator.isNotAtCollect.negate()).andThen(ejector.stop()));
+
+    // elevator.toZero.whileTrue(intake.intake(intakeVolts));
+    // elevator.toZero.().whileTrue(intake.stop());
+
     intake
         .backSensor
         .and(intake.frontSensor.negate())
@@ -316,10 +389,6 @@ public Robot() {
                             new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)),
                     drive)
                 .ignoringDisable(true));
-
-    controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3));
-    controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2));
-    controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT));
   }
 
   /** This function is called periodically during all modes. */
@@ -391,7 +460,7 @@ public void teleopPeriodic() {
       Logger.recordOutput("IntakeCommand", "null");
     }
 
-    if (intake.getCurrentCommand() != null) {
+    if (drive.getCurrentCommand() != null) {
       Logger.recordOutput("DriveCommand", drive.getCurrentCommand().getName());
     } else {
       Logger.recordOutput("DriveCommand", "null");
@@ -415,5 +484,7 @@ public void simulationInit() {}
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    SimulatedArena.getInstance().simulationPeriodic();
+  }
 }

src/main/java/org/curtinfrc/frc2025/Superstructure.java

@@ -20,9 +20,16 @@ public Superstructure(Drive drive, Elevator elevator) {
   public Command align(Setpoints setpoint) {
     return Commands.defer(
         () ->
-            Commands.parallel(
-                drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))),
-                elevator.goToSetpoint(setpoint)),
+            drive
+                .autoAlign(setpoint.toPose(new Pose3d(drive.getPose())))
+                .until(
+                    () ->
+                        Math.abs(drive.xController.getError()) < 0.2
+                            && Math.abs(drive.yController.getError()) < 0.2)
+                .andThen(
+                    elevator
+                        .goToSetpoint(setpoint)
+                        .alongWith(drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))))),
         Set.of(elevator, drive));
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java

@@ -81,15 +81,19 @@ public class Drive extends SubsystemBase {
   private double d = 0.2;
   private double i = 0.03;
 
-  private final PIDController xController = new PIDController(5.0, 0.0, 0.1);
-  private final PIDController yController = new PIDController(5.0, 0.0, 0.1);
+  public final PIDController xController = new PIDController(2.0, 0, 0);
+  public final PIDController yController = new PIDController(2.0, 0, 0);
   private final PIDController headingController = new PIDController(p, i, d);
 
-  private final PIDController xSetpointController = new PIDController(25.0, 0.0, 0.1);
-  private final PIDController ySetpointController = new PIDController(25.0, 0.0, 0.1);
+  private final PIDController xSetpointController = new PIDController(0.0, 0.0, 0);
+  private final PIDController ySetpointController = new PIDController(0.0, 0.0, 0);
 
   public Trigger atSetpointPose =
-      new Trigger(() -> xController.atSetpoint() && yController.atSetpoint());
+      new Trigger(
+          () ->
+              xController.atSetpoint()
+                  && yController.atSetpoint()
+                  && headingController.atSetpoint());
 
   public Pose3d setpoint = Pose3d.kZero;
 
@@ -102,6 +106,9 @@ public class Drive extends SubsystemBase {
   private final SlewRateLimiter xLimiter = new SlewRateLimiter(7);
   private final SlewRateLimiter yLimiter = new SlewRateLimiter(7);
 
+  private final SlewRateLimiter omegaAutoLimiter = new SlewRateLimiter(100);
+  private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1);
+
   RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner();
 
   public Drive(
@@ -116,6 +123,7 @@ public Drive(
     modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft);
     modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight);
 
+    // this.xController.setTolerance(0.05);
     // Usage reporting for swerve template
     HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit);
 
@@ -145,6 +153,10 @@ public Drive(
                 (voltage) -> runSteerCharacterization(voltage.in(Volts)), null, this));
 
     headingController.enableContinuousInput(-Math.PI, Math.PI);
+
+    // SmartDashboard.putNumber("P", 0.9);
+    // SmartDashboard.putNumber("D", 0.02);
+    // SmartDashboard.putNumber("I", 0.01);
   }
 
   @Override
@@ -157,6 +169,10 @@ public void periodic() {
     }
     odometryLock.unlock();
 
+    // headingController.setP(SmartDashboard.getNumber("P", 2));
+    // headingController.setD(SmartDashboard.getNumber("D", 0.01));
+    // headingController.setI(SmartDashboard.getNumber("I", 0));
+
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {
       for (var module : modules) {
@@ -230,6 +246,9 @@ private void runVelocity(ChassisSpeeds speeds) {
     // Log unoptimized setpoints and setpoint speeds
     Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
     Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds);
+    Logger.recordOutput(
+        "SwerveChassisSpeeds/SetpointMag",
+        Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond));
 
     // Send setpoints to modules
     for (int i = 0; i < 4; i++) {
@@ -383,22 +402,41 @@ public void followTrajectory(SwerveSample sample) {
     Pose2d pose = getPose();
     Logger.recordOutput("Odometry/TrajectorySetpoint", pose);
     Logger.recordOutput("Drive/PID/error", headingController.getError());
-    var out = headingController.calculate(pose.getRotation().getRadians(), sample.heading);
-    Logger.recordOutput("Drive/PID/out", out);
-
+    // Logger.recordOutput("Drive/PID/out", out);
+    Logger.recordOutput("Drive/sample", sample);
+
+    var err = new Transform2d(sample.x - pose.getX(), sample.y - pose.getY(), new Rotation2d());
+    var dist = Math.hypot(err.getX(), err.getY());
+    Logger.recordOutput("Drive/dist", dist);
+
+    var target_pose =
+        (DriverStation.getAlliance().get() == Alliance.Blue
+            ? new Pose2d(4.476, 4.026, new Rotation2d())
+            : new Pose2d(13.071, 4.026, new Rotation2d()));
+    var transform = target_pose.relativeTo(pose).rotateBy(pose.getRotation());
+    Logger.recordOutput("Drive/targetpose", target_pose);
+    Logger.recordOutput("Drive/transform", transform);
+    Logger.recordOutput("Drive/theta", Math.atan2(transform.getY(), transform.getX()));
+    Logger.recordOutput(
+        "Drive/projected",
+        new Pose2d(
+            pose.getX(),
+            pose.getY(),
+            new Rotation2d(Math.atan2(transform.getY(), transform.getX()))));
     // Generate the next speeds for the robot
+    xController.setSetpoint(sample.x);
+    yController.setSetpoint(sample.y);
     ChassisSpeeds speeds =
         ChassisSpeeds.fromFieldRelativeSpeeds(
-            sample.vx + sample.vx != 0
-                ? xSetpointController.calculate(pose.getX(), sample.x)
-                : xController.calculate(pose.getX(), sample.x),
-            sample.vy + sample.vy != 0
-                ? ySetpointController.calculate(pose.getY(), sample.y)
-                : yController.calculate(pose.getY(), sample.y),
-            sample.omega
-                + headingController.calculate(pose.getRotation().getRadians(), sample.heading),
+            sample.vx + (sample.vx != 0 ? 0 : xController.calculate(pose.getX(), sample.x)),
+            sample.vy + (sample.vy != 0 ? 0 : yController.calculate(pose.getY(), sample.y)),
+            dist < 1
+                ? headingController.calculate(pose.getRotation().getRadians(), sample.heading)
+                : headingController.calculate(
+                    pose.getRotation().getRadians(),
+                    Math.atan2(transform.getY(), transform.getX())),
             getRotation()); // Apply the generated speeds
-
+    Logger.recordOutput("Drive/ChassisSpeeds1", speeds);
     runVelocity(speeds);
   }
 
@@ -573,6 +611,12 @@ private ChassisSpeeds getChassisSpeeds() {
     return kinematics.toChassisSpeeds(getModuleStates());
   }
 
+  @AutoLogOutput(key = "SwerveChassisSpeeds/MeasuredMag")
+  private double getMeasuredChassisSpeedMag() {
+    var speeds = kinematics.toChassisSpeeds(getModuleStates());
+    return Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+  }
+
   /** Returns the position of each module in radians. */
   public double[] getWheelRadiusCharacterizationPositions() {
     double[] values = new double[4];
@@ -653,6 +697,12 @@ public Command autoAlign(Pose3d _setpoint) {
                   TunerConstants.kSpeedAt12Volts.in(MetersPerSecond),
                   true);
 
+          // Include rotational alignment using the existing heading controller
+          // double adjustedOmega =
+          //     headingController.calculate(
+          //         robotPose.getRotation().getRadians(),
+          //         _setpoint.toPose2d().getRotation().getRadians());
+
           // Apply the trajectory with rotation adjustment
           SwerveSample adjustedSample =
               new SwerveSample(
@@ -662,6 +712,7 @@ public Command autoAlign(Pose3d _setpoint) {
                   _setpoint.toPose2d().getRotation().getRadians(),
                   cmd.vx,
                   cmd.vy,
+                  // adjustedOmega,
                   0,
                   cmd.ax,
                   cmd.ay,