Commiting general changes to try to fix swerve issues

src/main/deploy/pathplanner/autos/From Subwoofer Far Edge to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7046435911053639,
+      "y": 4.3751818412727745
+    },
+    "rotation": 120.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Far Edge to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/From Subwoofer Front to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.29,
+      "y": 5.58
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Front to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7046435911053639,
+        "y": 4.3751818412727745
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4368920788482864,
+        "y": 3.1622483921028106
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82409310273166,
+        "y": 7.565612949407717
+      },
+      "prevControl": {
+        "x": 1.8176568333482528,
+        "y": 6.336387277909188
+      },
+      "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": 89.21368742867472,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 122.73522627210761,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/From Subwoofer Front to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2945574370576298,
+        "y": 5.580559282764361
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2786077412442953,
+        "y": 5.580559282764361
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8232420006879377,
+        "y": 7.533755031731888
+      },
+      "prevControl": {
+        "x": 1.8232420006879377,
+        "y": 6.694612522270728
+      },
+      "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": 90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/constants/AutoConstants.java

@@ -13,9 +13,9 @@ public final class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 5;
+    public static final double kPXController = 1.05;//5;
-    public static final double kPYController = 5;
+    public static final double kPYController = 1.05;//5;
-    public static final double kPThetaController = 0.5; // needs to be separate from heading control
+    public static final double kPThetaController = 0.95;//0.5; // needs to be separate from heading control
 
     public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
     public static final double kDHeadingController = 0.0025;

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -26,16 +26,18 @@ public final class DrivetrainConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     // Angular offsets of the modules relative to the chassis in radians
-    /* 
+    
     public static final double kFrontLeftChassisAngularOffset = Math.PI;
     public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
     public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
     public static final double kBackRightChassisAngularOffset = 0;
-*/
+
+    /*
     public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
     public static final double kFrontRightChassisAngularOffset = -2*Math.PI;
     public static final double kBackLeftChassisAngularOffset = -2*Math.PI;
     public static final double kBackRightChassisAngularOffset = -2*Math.PI-(4/180.0);
+    */
 
     // SPARK MAX CAN IDs
     public static final int kFrontLeftDrivingCanId = 8;

src/main/java/frc/robot/constants/SwerveModuleConstants.java

@@ -16,8 +16,8 @@ public class SwerveModuleConstants {
     public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
     public static final double kWheelDiameterMeters = 0.0762;
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
-    // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
+    // 45 teeth on the wheel's bevel gear, 20 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -299,6 +299,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
         SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
 
         m_frontLeft.setDesiredState(states[0]);