Swerve rewrite

.gitignore

@@ -136,7 +136,7 @@ $RECYCLE.BIN/
 *.lnk
 
 ### Gradle ###
-.gradle
+settings.gradle
 /build/
 BuildConstants.kt
 

.gradle/buildOutputCleanup/cache.properties

@@ -0,0 +1,2 @@
+#Wed Jun 19 14:31:53 EDT 2024
+gradle.version=8.4

build.gradle

@@ -83,7 +83,7 @@ dependencies {
 	simulationRelease wpi.sim.enableRelease()
 
 	implementation 'org.jetbrains.kotlin:kotlin-test-junit5'
-	implementation 'com.github.team4099:FalconUtils:1.1.30r3'
+	implementation 'com.github.team4099:FalconUtils:1.1.32'
 	implementation 'org.apache.commons:commons-collections4:4.0'
 	implementation 'com.google.code.gson:gson:2.10.1'
 	implementation "io.javalin:javalin:5.3.2"

simgui-ds.json

@@ -13,12 +13,12 @@
     {
       "axisConfig": [
         {
-          "decKey": 68,
-          "incKey": 65
+          "decKey": 65,
+          "incKey": 68
         },
         {
-          "decKey": 83,
-          "incKey": 87
+          "decKey": 87,
+          "incKey": 83
         },
         {
           "decKey": 69,
@@ -106,6 +106,9 @@
     },
     {
       "guid": "Keyboard1"
+    },
+    {
+      "guid": "03000000ac0500004f02000011010000"
     }
   ]
 }

src/main/deploy/pathplanner/paths/Arav Auto.path

@@ -0,0 +1,84 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2392814346720042,
+        "y": 5.570246205735591
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.2977105906762232,
+        "y": 5.5605080130730915
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5636756374343124,
+        "y": 5.570246205735591
+      },
+      "prevControl": {
+        "x": 3.0603234634701773,
+        "y": 5.579984398402961
+      },
+      "nextControl": {
+        "x": 2.0670278113984475,
+        "y": 5.560508013068222
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5636756374343124,
+        "y": 7.001760527843838
+      },
+      "prevControl": {
+        "x": 1.6190709486994308,
+        "y": 6.7972584818242
+      },
+      "nextControl": {
+        "x": 3.2098986869589385,
+        "y": 7.141664487022604
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0408470781354384,
+        "y": 6.096108609778437
+      },
+      "prevControl": {
+        "x": 2.8558214174554095,
+        "y": 6.875164023169248
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 270.0,
+    "maxAngularAcceleration": 600.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -168.07067782276167,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -179.490584237178,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/New New New New New Path.path

@@ -0,0 +1,95 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2,
+        "y": 5.539164403593215
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9943868001107077,
+        "y": 5.5263167132238324
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6706052618878533,
+        "y": 6.854365862063553
+      },
+      "prevControl": {
+        "x": 2.304147793348643,
+        "y": 6.53763992417634
+      },
+      "nextControl": {
+        "x": 3.384861487118685,
+        "y": 7.471691178777106
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.165424363431695,
+        "y": 7.402113436696155
+      },
+      "prevControl": {
+        "x": 7.309263712108935,
+        "y": 7.699422628087695
+      },
+      "nextControl": {
+        "x": 8.628852806461502,
+        "y": 7.241183901506513
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.7295201940372555,
+        "y": 5.539164403593215
+      },
+      "prevControl": {
+        "x": 4.223252527067413,
+        "y": 6.326529871119891
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": -140.0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 180.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 270.0,
+    "maxAngularAcceleration": 600.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -179.82557275027028,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

src/main/java/com/team4099/utils/threads/PhoenixOdometryThread.java

@@ -1,4 +1,5 @@
-package com.team4099.utils.threads;// Copyright 2021-2023 FRC 6328
+package com.team4099.utils.threads;
+// Copyright 2021-2023 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or
@@ -12,6 +13,8 @@
 // GNU General Public License for more details.
 
 
+import com.team4099.robot2023.config.constants.DrivetrainConstants;
+
 /**
  * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
  *
@@ -66,13 +69,13 @@ public void run() {
             signalsLock.lock();
             try {
                 if (isCANFD && signals.length > 0) {
-                    com.ctre.phoenix6.BaseStatusSignal.waitForAll(2.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY, signals);
+                    com.ctre.phoenix6.BaseStatusSignal.waitForAll(2.0 / DrivetrainConstants.INSTANCE.getOdometryUpdateFrequency(), signals);
                 } else {
                     // "waitForAll" does not support blocking on multiple
                     // signals with a bus that is not CAN FD, regardless
                     // of Pro licensing. No reasoning for this behavior
                     // is provided by the documentation.
-                    Thread.sleep((long) (1000.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY));
+                    Thread.sleep((long) (1000.0 / DrivetrainConstants.INSTANCE.getOdometryUpdateFrequency()));
                     com.ctre.phoenix6.BaseStatusSignal.refreshAll(signals);
                 }
             } catch (InterruptedException e) {
@@ -82,13 +85,13 @@ public void run() {
             }
 
             // Save new data to queues
-            com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(true);
+            com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(true);
             try {
                 for (int i = 0; i < signals.length; i++) {
                     queues.get(i).offer(signals[i].getValueAsDouble());
                 }
             } finally {
-                com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(false);
+                com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(false);
             }
         }
     }

src/main/java/com/team4099/utils/threads/SparkMaxOdometryThread.java

@@ -1,4 +1,5 @@
-package com.team4099.utils.threads;// Copyright 2021-2023 FRC 6328
+package com.team4099.utils.threads;
+// Copyright 2021-2023 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or
@@ -12,6 +13,8 @@
 // GNU General Public License for more details.
 
 
+import com.team4099.robot2023.config.constants.DrivetrainConstants;
+
 /**
  * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
  *
@@ -35,29 +38,29 @@ public static com.team4099.utils.threads.SparkMaxOdometryThread getInstance() {
     private SparkMaxOdometryThread() {
         notifier = new edu.wpi.first.wpilibj.Notifier(this::periodic);
         notifier.setName("SparkMaxOdometryThread");
-        notifier.startPeriodic(1.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY );
+        notifier.startPeriodic(1.0 / DrivetrainConstants.INSTANCE.getOdometryUpdateFrequency());
     }
 
     public java.util.Queue<Double> registerSignal(java.util.function.DoubleSupplier signal) {
         java.util.Queue<Double> queue = new java.util.concurrent.ArrayBlockingQueue<>(100);
-        com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(true);
+        com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(true);
         try {
             signals.add(signal);
             queues.add(queue);
         } finally {
-            com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(false);
+            com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(false);
         }
         return queue;
     }
 
     private void periodic() {
-        com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(true);
+        com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(true);
         try {
             for (int i = 0; i < signals.size(); i++) {
                 queues.get(i).offer(signals.get(i).getAsDouble());
             }
         } finally {
-            com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.setOdometryLock(false);
+            com.team4099.robot2023.subsystems.drivetrain.Drivetrain.Companion.setOdometryLock(false);
         }
     }
 }