got swerve working again

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,32 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SwerveDriveSubsystem": {
+        "double[]##v_/SwerveDriveSubsystem/Pose": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "visible": true
+  }
+}

src/main/deploy/pathplanner/autos/auto1.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.1372025464394855,
+      "y": 7.006556629982723
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "path1"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/path1.path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1372025464394855,
+        "y": 7.006556629982723
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.25523653518938,
+        "y": 7.006556629982723
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.459347429867334,
+        "y": 7.006556629982723
+      },
+      "prevControl": {
+        "x": 2.93865679729278,
+        "y": 7.006556629982723
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/java/frc/lib/swerve/SwerveModuleConstants.java

@@ -6,6 +6,7 @@ public class SwerveModuleConstants {
     public final int driveMotorID;
     public final int angleMotorID;
     public final int cancoderID;
+    public final double angleOffset;
     public Optional<String> canivoreName = Optional.empty();
 
     /**
@@ -19,6 +20,7 @@ public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID,
         this.driveMotorID = driveMotorID;
         this.angleMotorID = angleMotorID;
         this.cancoderID = canCoderID;
+        this.angleOffset = angleOffset;
     }
 
     public SwerveModuleConstants(

src/main/java/frc/robot/AutonomousManager.java

@@ -117,15 +117,9 @@ private enum AutonomousOption {
                 StartingLocation.OPEN,
                 3,
                 false,
-                "open_place3handoff_copy"),
+                "auto1"),
         STATION_PLACE1ANDCLIMB(
-                StartingLocation.STATIONOPEN, 1, true, "station_place1andclimb_pickup"),
-        STATIONCABLE_PLACE1ANDCLIMB(
-                StartingLocation.STATIONCABLE,
-                1,
-                true,
-                "stationcable_place1andclimb_pickup"),
-        CABLE_PLACE2(StartingLocation.CABLE, 2, false, "cable_place2");
+                StartingLocation.STATIONOPEN, 1, true, "auto1");
 
         private String pathName;
         public StartingLocation startPosition;

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

@@ -166,7 +166,7 @@ public static class CompBotConstants {
         public static final boolean canCoderInvert = false;
 
         /* Module Specific Constants */
-        // Note, bevel gears should face left (relative to back-to-front)
+        // Note, bevel gears should face left (if you're looking at the back)
 
         /* Front Left Module - Module 0 */
         public static final class Mod0 {

src/main/java/frc/robot/subsystems/swervedrive/SwerveModuleIOPhoenixPro.java

@@ -26,12 +26,14 @@ public class SwerveModuleIOPhoenixPro implements SwerveModuleIO {
     private TalonFX driveMotor;
     private CANcoder angleEncoder;
 
-    private VoltageOut voltageRequestDrive = new VoltageOut(0).withEnableFOC(true);
+    private final boolean enableFOC = true;
+
+    private VoltageOut voltageRequestDrive = new VoltageOut(0).withEnableFOC(enableFOC);
 
     private PositionVoltage positionVoltageRequestAngle =
-            new PositionVoltage(0).withSlot(0).withEnableFOC(true);
+            new PositionVoltage(0).withSlot(0).withEnableFOC(enableFOC);
     private VelocityVoltage velocityVoltageRequestDrive =
-            new VelocityVoltage(0).withSlot(0).withEnableFOC(true);
+            new VelocityVoltage(0).withSlot(0).withEnableFOC(enableFOC);
 
     SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(
             Constants.SwerveConstants.calculatedDriveKS,
@@ -65,7 +67,7 @@ public SwerveModuleIOPhoenixPro(int moduleNumber, SwerveModuleConstants moduleCo
         angleEncoder = moduleConstants.canivoreName.isEmpty()
                 ? new CANcoder(moduleConstants.cancoderID)
                 : new CANcoder(moduleConstants.cancoderID, moduleConstants.canivoreName.get());
-        configAngleEncoder();
+        configAngleEncoder(moduleConstants.angleOffset);
 
         /* Angle Motor Config */
         angleMotor = moduleConstants.canivoreName.isEmpty()
@@ -151,10 +153,11 @@ public void updateInputs(SwerveModuleIOInputs inputs) {
         inputs.angleCurrent = angleCurrentSS.refresh().getValue();
     }
 
-    private void configAngleEncoder() {
+    private void configAngleEncoder(double angleOffset) {
         angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCanCoderConfig);
         MagnetSensorConfigs myMagnetSensorConfigs = new MagnetSensorConfigs();
         angleEncoder.getConfigurator().refresh(myMagnetSensorConfigs);
+        myMagnetSensorConfigs.MagnetOffset = -angleOffset / 360;
         angleEncoder.getConfigurator().apply(myMagnetSensorConfigs);
     }
 

src/main/java/frc/robot/subsystems/swervedrive/SwerveModuleIOSim.java

@@ -38,12 +38,12 @@ public void setDesiredVelocityClosedLoop(double velocity) {
     };
 
     public void setDesiredAngularPosition(double angularPosition) {
-        this.angularPosition = Math.toRadians(angularPosition);
+        this.angularPosition = angularPosition;
         angularVelocity = 0;
     };
 
     public void setDesiredAngularPositionAndVelocity(double angularPosition, double angularVelocity) {
-        this.angularPosition = Math.toRadians(angularPosition);
+        this.angularPosition = angularPosition;
         this.angularVelocity = angularVelocity;
     };