pathplanner fixes

Team334 · Jan 25, 2024 · 4900bd8 · 4900bd8
1 parent b01c659
commit 4900bd8
Show file tree

Hide file tree

Showing 6 changed files with 154 additions and 11 deletions.
diff --git a/src/main/deploy/pathplanner/autos/spin.auto b/src/main/deploy/pathplanner/autos/spin.auto
@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.0,
+      "y": 6.9797508964387305
+    },
+    "rotation": 90.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "spin pt 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "spin pt 2"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/amp path.path b/src/main/deploy/pathplanner/paths/amp path.path
@@ -38,7 +38,7 @@
         "maxVelocity": 4.67,
         "maxAcceleration": 1.5,
         "maxAngularVelocity": 180.0,
-        "maxAngularAcceleration": 720.0
+        "maxAngularAcceleration": 180.0
       }
     }
   ],
@@ -50,8 +50,8 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0,
+    "velocity": 0.0,
+    "rotation": -90.0,
     "rotateFast": false
   },
   "reversed": false,

diff --git a/src/main/deploy/pathplanner/paths/spin pt 1.path b/src/main/deploy/pathplanner/paths/spin pt 1.path
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 6.9797508964387305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9931030853052696,
+        "y": 6.009057063159172
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 5.546821904454621
+      },
+      "prevControl": {
+        "x": 1.9931030853052696,
+        "y": 6.9797508964387305
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.67,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": -90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/spin pt 2.path b/src/main/deploy/pathplanner/paths/spin pt 2.path
@@ -0,0 +1,61 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 5.99056765681099
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9931030853052696,
+        "y": 5.019873823531432
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 3.7533494886809597
+      },
+      "prevControl": {
+        "x": 1.9931030853052696,
+        "y": 5.18627848066507
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 4.67,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 180.0,
+        "maxAngularAcceleration": 180.0
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.67,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -79,26 +79,26 @@ public static class Physical {
   public static class FeedForward {
     public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constants
 
-    public static final double MODULE_DRIVE_KS = 0.0;
-    public static final double MODULE_DRIVE_KV = 0.0;
+    public static final double MODULE_DRIVE_KS = 0.32;
+    public static final double MODULE_DRIVE_KV = 2.15;
   }
 
   public static class PID {
     // TODO: Tune everything
 
     // TODO: bruh these don't work 😭
-    public static final double FRONT_LEFT_DRIVE_KP = 0.08;
-    public static final double FRONT_RIGHT_DRIVE_KP = 0.08;
-    public static final double BACK_RIGHT_DRIVE_KP = 0.08;
-    public static final double BACK_LEFT_DRIVE_KP = 0.08;
+    public static final double FRONT_LEFT_DRIVE_KP = 0.05;
+    public static final double FRONT_RIGHT_DRIVE_KP = 0.05;
+    public static final double BACK_RIGHT_DRIVE_KP = 0.05;
+    public static final double BACK_LEFT_DRIVE_KP = 0.05;
 
     public static final double FRONT_LEFT_ROTATE_KP = 0.009;
     public static final double FRONT_RIGHT_ROTATE_KP = 0.009;
     public static final double BACK_RIGHT_ROTATE_KP = 0.009;
     public static final double BACK_LEFT_ROTATE_KP = 0.009;
 
-    public static final PIDConstants PP_TRANSLATION = new PIDConstants(2.5, 0, 0);
-    public static final PIDConstants PP_ROTATION = new PIDConstants(.9, 0, 0.25);
+    public static final PIDConstants PP_TRANSLATION = new PIDConstants(3.67, 0, 0);
+    public static final PIDConstants PP_ROTATION = new PIDConstants(1.25, 0, 0);
 
     public static final double SHOOTER_FLYWHEEL_KP = 0;
 

diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java
@@ -163,6 +163,8 @@ public void setState(SwerveModuleState state) {
     double drive_feedforward = UtilFuncs.FromVolts(_driveFeedforward.calculate(speed));
     double drive_pid = _driveController.calculate(getDriveVelocity(), speed);
 
+    drive_pid = 0;
+
     rotate(rotation_pid);
     drive(drive_feedforward + drive_pid);
     // drive(0.08);