Add swerve sim support

build.gradle

@@ -79,7 +79,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    implementation "com.github.deepbluerobotics:lib199:14b09294500182f67060d8608ace9e710bb0a004"
+    implementation "com.github.deepbluerobotics:lib199:0e5932a4219f3439ef2be3981a9189baf777f61e"
 }
 
 test {

simgui-ds.json

@@ -15,9 +15,14 @@
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 74,
+          "incKey": 76
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -41,10 +46,7 @@
     },
     {
       "axisConfig": [
-        {
-          "decKey": 74,
-          "incKey": 76
-        },
+        {},
         {
           "decKey": 73,
           "incKey": 75
@@ -91,11 +93,10 @@
   ],
   "robotJoysticks": [
     {
-      "useGamepad": true
+      "guid": "Keyboard0"
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard1"
     }
   ]
 }

simgui.json

@@ -3,48 +3,25 @@
     "Addressable LEDs": {
       "0": {
         "columns": 32
-      },
-      "window": {
-        "visible": true
       }
     },
     "Other Devices": {
-      "CANCoder (v6)[0]": {
-        "header": {
-          "open": true
-        }
-      },
-      "CANCoder (v6)[1]": {
-        "header": {
-          "open": true
-        }
-      },
       "CANCoder (v6)[2]": {
         "header": {
           "open": true
         }
       },
-      "CANCoder (v6)[3]": {
+      "CANCoder[2]": {
         "header": {
           "open": true
         }
       },
-      "SparkMax[11]": {
+      "SparkMax[15]": {
         "header": {
           "open": true
         }
       },
-      "SparkMax[14]": {
-        "header": {
-          "open": true
-        }
-      },
-      "SparkMax[18]": {
-        "header": {
-          "open": true
-        }
-      },
-      "SparkMax[19]": {
+      "navX-Sensor[0]": {
         "header": {
           "open": true
         }
@@ -60,13 +37,44 @@
       "/Shuffleboard/arm SysID/quasistatic backward": "Command",
       "/Shuffleboard/arm SysID/quasistatic forward": "Command",
       "/SmartDashboard/Arm": "Subsystem",
+      "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/moveClimber": "Command"
+    },
+    "windows": {
+      "/Shuffleboard/Auto Chooser Tab/SendableChooser[0]": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {
     "transitory": {
       "SmartDashboard": {
+        "BL": {
+          "open": true
+        },
+        "BR": {
+          "open": true
+        },
+        "FL": {
+          "open": true
+        },
+        "FR": {
+          "open": true
+        },
         "open": true
       }
     }
@@ -87,9 +95,12 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 177
+          "height": 263
         }
-      ]
+      ],
+      "window": {
+        "visible": false
+      }
     }
   }
 }

src/main/java/org/carlmontrobotics/Constants.java

@@ -218,7 +218,7 @@ public static final class Drivetrainc {
 		public static final boolean[] reversed = { false, false, false, false };
 		// public static final boolean[] reversed = {true, true, true, true};
 		// Determine correct turnZero constants (FL, FR, BL, BR)
-		public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] { 0, 0, 0, 0 }
+		public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 }
 				: new double[] { -48.6914, 63.3691, 94.1309, -6.7676 };/* real values here */
 
 		// kP, kI, and kD constants for turn motor controllers in the order of

src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java

@@ -19,12 +19,16 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
+import org.carlmontrobotics.lib199.swerve.SwerveModuleSim;
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -40,15 +44,15 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 public class Drivetrain extends SubsystemBase {
     private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2
     private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.));
     // ^used by PathPlanner for chaining paths
 
     private SwerveDriveKinematics kinematics = null;
     private SwerveDriveOdometry odometry = null;
-    private Field2d field = new Field2d();
 
     private SwerveModule modules[];
     private boolean fieldOriented = true;
@@ -68,6 +72,12 @@ public class Drivetrain extends SubsystemBase {
     private SwerveModule moduleBL;
     private SwerveModule moduleBR;
 
+    private final Field2d field = new Field2d();
+
+    private SwerveModuleSim[] moduleSims;
+    private SimDouble gyroYawSim;
+    private Timer simTimer = new Timer();
+
     public Drivetrain() {
         // SmartDashboard.putNumber("set x", 0);
         // SmartDashboard.putNumber("set y", 0);
@@ -142,6 +152,14 @@ public Drivetrain() {
                     turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3,
                     pitchSupplier, rollSupplier);
             modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
+
+            if (RobotBase.isSimulation()) {
+                moduleSims = new SwerveModuleSim[] {
+                    moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim()
+                };
+                gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw");
+            }
+
             for (CANSparkMax driveMotor : driveMotors) {
                 driveMotor.setOpenLoopRampRate(secsPer12Volts);
                 driveMotor.getEncoder().setPositionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing);
@@ -163,6 +181,8 @@ public Drivetrain() {
 
             }
 
+            SmartDashboard.putData("Field", field);
+
             // for(CANSparkMax driveMotor : driveMotors)
             // driveMotor.setSmartCurrentLimit(80);
 
@@ -179,6 +199,36 @@ public Drivetrain() {
         //                             SmartDashboard.putNumber("chassis speeds theta", 0);
     }
 
+    @Override
+    public void simulationPeriodic() {
+        for (var moduleSim : moduleSims) {
+            moduleSim.update();
+        }
+        SwerveModuleState[] measuredStates =
+            new SwerveModuleState[] {
+                moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState()
+            };
+        ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates);
+
+        double dtSecs = simTimer.get();
+        simTimer.restart();
+
+        Pose2d simPose = field.getRobotPose();
+        simPose = simPose.exp(
+                new Twist2d(
+                    speeds.vxMetersPerSecond * dtSecs,
+                    speeds.vyMetersPerSecond * dtSecs,
+                    speeds.omegaRadiansPerSecond * dtSecs));
+        double newAngleDeg = simPose.getRotation().getDegrees();
+        // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back.
+        newAngleDeg -= simGyroOffset.getDegrees();
+        newAngleDeg *= (isGyroReversed ? -1.0 : 1.0);
+        gyroYawSim.set(newAngleDeg);
+        while (Math.abs(MathUtil.inputModulus(gyro.getAngle() - newAngleDeg, -180.0, 180.0)) > 0.1) {
+            Timer.delay(1.0/gyro.getActualUpdateRate());
+        }
+    }
+
     // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int
     // frontorback) {
     // switch(frontorback) {
@@ -251,7 +301,6 @@ public void periodic() {
         // SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle());
         // SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle());
         // SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle());
-
     }
 
     @Override
@@ -465,8 +514,13 @@ public Pose2d getPose() {
         return odometry.getPoseMeters();
     }
 
+    private Rotation2d simGyroOffset = new Rotation2d();
     public void setPose(Pose2d initialPose) {
-        odometry.resetPosition(gyro.getRotation2d(), getModulePositions(), initialPose);
+        Rotation2d gyroRotation = gyro.getRotation2d();
+        odometry.resetPosition(gyroRotation, getModulePositions(), initialPose);
+        // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future
+        // We need the offset so that we can compensate for it during simulationPeriodic().
+        simGyroOffset = initialPose.getRotation().minus(gyroRotation);
         //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose);
     }