From 10cdf414e44dc44abd3829222db2493f61e86554 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Sat, 18 Jan 2025 10:12:10 -0600
Subject: [PATCH 01/71] Autos basics

---
 .../deploy/pathplanner/autos/New Auto.auto    | 19 +++++
 src/main/deploy/pathplanner/navgrid.json      |  1 +
 .../deploy/pathplanner/paths/Example 1.path   | 54 ++++++++++++
 ....1.1.json => PathplannerLib-2025.2.1.json} |  8 +-
 ...enix6-25.1.0.json => Phoenix6-25.2.0.json} | 84 +++++++++++++------
 ...Lib-2025.0.0.json => REVLib-2025.0.1.json} | 15 ++--
 ...ca-2025.0.0.json => Studica-2025.0.1.json} | 14 ++--
 7 files changed, 148 insertions(+), 47 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 src/main/deploy/pathplanner/paths/Example 1.path
 rename vendordeps/{PathplannerLib-2025.1.1.json => PathplannerLib-2025.2.1.json} (87%)
 rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.0.json} (85%)
 rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.1.json} (86%)
 rename vendordeps/{Studica-2025.0.0.json => Studica-2025.0.1.json} (89%)

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 0000000..0b46cdb
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example 1"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example 1.path b/src/main/deploy/pathplanner/paths/Example 1.path
new file mode 100644
index 0000000..9278a95
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example 1.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.6575,
+        "y": 3.673999999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6575000000000006,
+        "y": 3.673999999999999
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.1.1.json
rename to vendordeps/PathplannerLib-2025.2.1.json
index 6322388..71e25f3 100644
--- a/vendordeps/PathplannerLib-2025.1.1.json
+++ b/vendordeps/PathplannerLib-2025.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.1.1.json",
+    "fileName": "PathplannerLib-2025.2.1.json",
     "name": "PathplannerLib",
-    "version": "2025.1.1",
+    "version": "2025.2.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.1.1"
+            "version": "2025.2.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.1.1",
+            "version": "2025.2.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.0.json
similarity index 85%
rename from vendordeps/Phoenix6-25.1.0.json
rename to vendordeps/Phoenix6-25.2.0.json
index 473f6a8..38747fb 100644
--- a/vendordeps/Phoenix6-25.1.0.json
+++ b/vendordeps/Phoenix6-25.2.0.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.1.0.json",
+    "fileName": "Phoenix6-25.2.0.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.2.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.2.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.0",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.1.json
similarity index 86%
rename from vendordeps/REVLib-2025.0.0.json
rename to vendordeps/REVLib-2025.0.1.json
index cde6011..c998054 100644
--- a/vendordeps/REVLib-2025.0.0.json
+++ b/vendordeps/REVLib-2025.0.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.0.json",
+    "fileName": "REVLib-2025.0.1.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json
similarity index 89%
rename from vendordeps/Studica-2025.0.0.json
rename to vendordeps/Studica-2025.0.1.json
index ddb0e44..5010be0 100644
--- a/vendordeps/Studica-2025.0.0.json
+++ b/vendordeps/Studica-2025.0.1.json
@@ -1,13 +1,13 @@
 {
-    "fileName": "Studica-2025.0.0.json",
+    "fileName": "Studica-2025.0.1.json",
     "name": "Studica",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
     "frcYear": "2025",
     "mavenUrls": [
         "https://dev.studica.com/maven/release/2025/"
     ],
-    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
     "cppDependencies": [
         {
             "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
             "libName": "Studica",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         },
         {
             "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
             "libName": "StudicaDriver",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "javaDependencies": [
         {
             "artifactId": "Studica-java",
             "groupId": "com.studica.frc",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
@@ -65,7 +65,7 @@
                 "osxuniversal",
                 "windowsx86-64"
             ],
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ]
 }
\ No newline at end of file

From a6a3eb0441a6f3f36c582391411a10a5dbf5d6ea Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 20 Jan 2025 18:52:26 -0600
Subject: [PATCH 02/71] added boilerplate manipulator code

---
 src/main/java/frc/robot/Constants.java        | 75 ++++++++++++++++++-
 .../subsystems/AlgaeSubsystem/AlgaeArm.java   | 16 ++++
 .../AlgaeSubsystem/AlgaeIntake.java           | 16 ++++
 .../robot/subsystems/AlgaeSubsystem/Main.java | 19 +++++
 .../robot/subsystems/ClimberSubsystem.java    | 16 ++++
 .../subsystems/CoralSubsystem/CoralArm.java   | 20 +++++
 .../CoralSubsystem/CoralIntake.java           | 16 ++++
 .../robot/subsystems/CoralSubsystem/Main.java | 19 +++++
 .../robot/subsystems/ElevatorSubsystem.java   |  9 +--
 .../subsystems/NewElevatorSubsystem.java      | 24 ++++++
 10 files changed, 222 insertions(+), 8 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
 create mode 100644 src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
 create mode 100644 src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
 create mode 100644 src/main/java/frc/robot/subsystems/ClimberSubsystem.java
 create mode 100644 src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
 create mode 100644 src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
 create mode 100644 src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java
 create mode 100644 src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 639b994..340e001 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -10,6 +10,8 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -152,6 +154,70 @@ public static class CommonConstants {
     public static final boolean LOG_INTO_FILE_ENABLED = true;
   }
 
+  // TODO: ##################### PLACEHOLDERS #####################
+  public static class Climber {
+    public static final int MOTOR_PORT = 20;
+    public static final ProfiledPIDController PID = new ProfiledPIDController(
+      1,
+      0.0,
+      0.0, 
+      null
+    );
+  }
+
+  // TODO: ##################### PLACEHOLDERS #####################
+  public static class Coral {
+    public static class Pivot {
+      public static final int MOTOR_PORT = 14;
+      public static final ProfiledPIDController PID = new ProfiledPIDController(
+        1,
+        0.0,
+        0.0,
+        null
+      );
+    }
+
+    public static class Roll {
+      public static final int MOTOR_PORT = 15;
+      public static final ProfiledPIDController PID = new ProfiledPIDController(
+        1,
+        0.0,
+        0.0,
+        null
+      );
+    }
+    public static class Pitch {
+      public static final int MOTOR_PORT = 16;
+      public static final ProfiledPIDController PID = new ProfiledPIDController(
+        1,
+        0.0,
+        0.0,
+        null
+      );
+    }
+
+    public static class Intake {
+      public static final int MOTOR_PORT = 17;
+    }
+  }
+  
+  // TODO: ##################### PLACEHOLDERS #####################
+  public static class Algae {
+    public static final class Pivot {
+      public static final int MOTOR_PORT = 18;
+      public static final ProfiledPIDController PID = new ProfiledPIDController(
+        1,
+        0.0,
+        0.0,
+        null
+      );
+    }
+
+    public static final class Intake {
+      public static final int MOTOR_PORT = 19;
+    }
+  }
+
   public static class Elevator {
     public static final int elevatorOnePort = 10;
     public static final int elevatorTwoPort = 11;
@@ -175,13 +241,20 @@ public static class PID {
     // TODO: For the first testing, set these all to zero for safety reasons
     // Remind me to pad the top and bottom of the elevator with poodles to make sure
     // we don't damage it.
-    public static class Feedforward {
+    public static class FeedforwardConstants {
       public static double Ks = 0.0;
       public static double Kv = 4.0;
       public static double Ka = 0.03;
       public static double Kg = 0.1;
     }
 
+    public static ElevatorFeedforward feedForward = new ElevatorFeedforward(
+      FeedforwardConstants.Ks,
+      FeedforwardConstants.Kg,
+      FeedforwardConstants.Kv,
+      FeedforwardConstants.Ka
+    );
+
     public static class PhysicalParameters {
       public static double gearReduction = 9.0 / 2.0;
       public static double driveRadiusMeters = 0.0182;
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
new file mode 100644
index 0000000..a442633
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems.AlgaeSubsystem;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Algae;
+
+public class AlgaeArm extends SubsystemBase{
+    public final SparkMax pivotMotor = new SparkMax(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
+        
+    @Override
+    public void periodic() {
+        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
new file mode 100644
index 0000000..24da632
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems.AlgaeSubsystem;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Algae;
+
+public class AlgaeIntake extends SubsystemBase {
+    private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless);
+
+    @Override
+    public void periodic() {
+
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
new file mode 100644
index 0000000..e3ade48
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
@@ -0,0 +1,19 @@
+package frc.robot.subsystems.AlgaeSubsystem;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Main extends SubsystemBase{
+    
+    private final AlgaeArm arm;
+    private final AlgaeIntake intake;
+
+    public Main(AlgaeArm arm, AlgaeIntake intake) {
+        this.arm = arm;
+        this.intake = intake;
+    }
+
+    @Override
+    public void periodic() {
+        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
new file mode 100644
index 0000000..b6b46ff
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Climber;
+
+public class ClimberSubsystem extends SubsystemBase {
+    private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
+
+    @Override
+    public void periodic() {
+        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
new file mode 100644
index 0000000..46e2c9e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
@@ -0,0 +1,20 @@
+package frc.robot.subsystems.CoralSubsystem;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Coral;
+
+public class CoralArm extends SubsystemBase{
+    private final SparkMax pivotMotor = new SparkMax(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+
+    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+
+    @Override
+    public void periodic() {
+
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
new file mode 100644
index 0000000..3941f72
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems.CoralSubsystem;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Coral;
+
+public class CoralIntake extends SubsystemBase {
+    public final SparkMax intakeMotor = new SparkMax(Coral.Intake.MOTOR_PORT, MotorType.kBrushless);
+    
+    @Override
+    public void periodic() {
+        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java
new file mode 100644
index 0000000..dc539b1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java
@@ -0,0 +1,19 @@
+package frc.robot.subsystems.CoralSubsystem;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Main extends SubsystemBase {
+
+    private final CoralArm arm;
+    private final CoralIntake intake;
+
+    public Main(CoralArm arm, CoralIntake intake) {
+        this.arm = arm;
+        this.intake = intake;
+    }
+
+    @Override
+    public void periodic() {
+        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 26f9baa..91a6b43 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.Constants.Elevator;
 
 public class ElevatorSubsystem extends ProfiledPIDSubsystem {
     // NOTE: Elevator motor one has both the encoder used for positioning, and the
@@ -35,13 +36,7 @@ public class ElevatorSubsystem extends ProfiledPIDSubsystem {
             MotorType.kBrushless);
     private final SparkFlex elevatorMotorTwo = new SparkFlex(Constants.Elevator.elevatorTwoPort,
             MotorType.kBrushless);
-    private final ElevatorFeedforward feedForward = new ElevatorFeedforward(
-            Constants.Elevator.Feedforward.Ks,
-            Constants.Elevator.Feedforward.Kg,
-            Constants.Elevator.Feedforward.Kv,
-            Constants.Elevator.Feedforward.Ka
-
-    );
+    private final ElevatorFeedforward feedForward = Elevator.feedForward;
 
     private final SparkMaxConfig elevatorConfigOne = new SparkMaxConfig();
     private final SparkMaxConfig elevatorConfigTwo = new SparkMaxConfig();
diff --git a/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java
new file mode 100644
index 0000000..b6155eb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java
@@ -0,0 +1,24 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Elevator;
+
+public class NewElevatorSubsystem extends SubsystemBase {
+
+    private final SparkMax leaderMotor = new SparkMax(Elevator.elevatorOnePort, MotorType.kBrushless);
+    private final SparkMax followerMotor = new SparkMax(Elevator.elevatorTwoPort, MotorType.kBrushless);
+
+    private final SparkMaxConfig leaderConfig = new SparkMaxConfig();
+    private final SparkMaxConfig followerConfig = new SparkMaxConfig();
+
+    private final ElevatorFeedforward feedForward = Elevator.feedForward;
+
+    public NewElevatorSubsystem() {
+        
+    }
+}

From a70e3b10380d97d02553e4edf1f676d26d0c0998 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 22 Jan 2025 19:26:26 -0600
Subject: [PATCH 03/71] filled in elevatorsubsystem

renamed newelevatorsubsystem to elevatorsubsystem
---
 .../robot/subsystems/ElevatorSubsystem.java   | 262 +++++-------------
 .../subsystems/NewElevatorSubsystem.java      |  24 --
 2 files changed, 73 insertions(+), 213 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 91a6b43..e7a5af7 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -1,233 +1,117 @@
 package frc.robot.subsystems;
 
-import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkLimitSwitch;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.datalog.DoubleLogEntry;
-import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
-import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
-import frc.robot.Constants;
-import frc.robot.Robot;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Elevator;
 
-public class ElevatorSubsystem extends ProfiledPIDSubsystem {
-    // NOTE: Elevator motor one has both the encoder used for positioning, and the
-    // limit switch used for zeroing
-    private final SparkFlex elevatorMotorOne = new SparkFlex(Constants.Elevator.elevatorOnePort,
-            MotorType.kBrushless);
-    private final SparkFlex elevatorMotorTwo = new SparkFlex(Constants.Elevator.elevatorTwoPort,
-            MotorType.kBrushless);
-    private final ElevatorFeedforward feedForward = Elevator.feedForward;
-
-    private final SparkMaxConfig elevatorConfigOne = new SparkMaxConfig();
-    private final SparkMaxConfig elevatorConfigTwo = new SparkMaxConfig();
-
-    /*
-     * DCMotor gearbox,
-     * double gearing,
-     * double carriageMassKg,
-     * double drumRadiusMeters,
-     * double minHeightMeters,
-     * double maxHeightMeters,
-     * boolean simulateGravity,
-     * double startingHeightMeters,
-     * Matrix<N1, N1> measurementStdDevs
-     */
-    private boolean isZeroed = false;
-    private final RelativeEncoder elevatorEncoderOne;
-    private final RelativeEncoder elevatorEncoderTwo;
-    private final SparkLimitSwitch bottomLimit;
-
-    private final ElevatorSim simulation = new ElevatorSim(
-            Constants.Elevator.PhysicalParameters.simMotor,
-            Constants.Elevator.PhysicalParameters.gearReduction,
-            Constants.Elevator.PhysicalParameters.carriageMassKg,
-            Constants.Elevator.PhysicalParameters.driveRadiusMeters,
-            0.0,
-            Constants.Elevator.PhysicalParameters.elevatorHeightMeters,
-            true,
-            0.0,
-            0.001,
-            0.001
-    );
+public class ElevatorSubsystem extends SubsystemBase {
 
-    private DoubleLogEntry elevatorTargetP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/position");
-    private DoubleLogEntry elevatorTargetV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/velocity");
-    private DoubleLogEntry elevatorP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/position");
-    private DoubleLogEntry elevatorV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/velocity");
-    private DoubleLogEntry elevatorOneOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output1");
-    private DoubleLogEntry elevatorTwoOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output2");
-    private DoubleLogEntry elevatorOneCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current1");
-    private DoubleLogEntry elevatorTwoCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current2");
-
-    private Mechanism2d mechanism2D = new Mechanism2d(Constants.RobotConstants.robotLengthMeters,
-            Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters
-                    + Constants.Elevator.PhysicalParameters.elevatorHeightMeters
-                    + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0);
-    private MechanismRoot2d rootMechanism = mechanism2D.getRoot("climber",
-            Constants.RobotConstants.robotLengthMeters / 2.0
-                    + Constants.Elevator.PhysicalParameters.elevatorForwardsFromRobotCenterMeters,
-            Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters);
-    private MechanismLigament2d elevatorMechanism;
+    private final SparkMax leaderMotor = new SparkMax(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax followerMotor = new SparkMax(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless);
 
-    public ElevatorSubsystem() {
-        super(
-                new ProfiledPIDController(
-                        Constants.Elevator.PID.kP,
-                        Constants.Elevator.PID.kI,
-                        Constants.Elevator.PID.kD,
-                        new TrapezoidProfile.Constraints(
-                                Constants.Elevator.PID.MAX_VELOCITY,
-                                Constants.Elevator.PID.MAX_ACCELERATION)),
-                0.0);
-
-        this.getController().setTolerance(Units.inchesToMeters(0.5));
-
-        elevatorConfigOne
-            .idleMode(IdleMode.kBrake)
-            .inverted(Constants.Elevator.elevatorOneInverted);
-        elevatorConfigOne.encoder
-            .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter)
-            .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter);
-        elevatorConfigOne.limitSwitch.reverseLimitSwitchType(Constants.Elevator.bottomLimitMode);
+    private final SparkMaxConfig leaderConfig = new SparkMaxConfig();
+    private final SparkMaxConfig followerConfig = new SparkMaxConfig();
 
-        elevatorConfigTwo
-            .idleMode(IdleMode.kBrake)
-            .inverted(Constants.Elevator.elevatorTwoInverted);
-        elevatorConfigTwo.encoder
-            .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter)
-            .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter);
+    private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
+    private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
 
-        elevatorMotorOne.configure(elevatorConfigOne, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        elevatorMotorTwo.configure(elevatorConfigTwo, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        
-        elevatorEncoderOne = elevatorMotorOne.getEncoder();
-        elevatorEncoderTwo = elevatorMotorTwo.getEncoder();
+    private final SparkLimitSwitch bottomLimit = leaderMotor.getReverseLimitSwitch();
 
-        elevatorMotorOne.getEncoder().setPosition(0);
-        elevatorMotorTwo.getEncoder().setPosition(0);
 
-        bottomLimit = elevatorMotorOne.getReverseLimitSwitch();
+    private final ElevatorFeedforward feedForward = Elevator.FEEDFORWARD;
+    private final ProfiledPIDController elevatorPID = new ProfiledPIDController(
+        Elevator.PID.kP,
+        Elevator.PID.kI,
+        Elevator.PID.kD,
+        new TrapezoidProfile.Constraints(
+            Elevator.PID.MAX_VELOCITY, 
+            Elevator.PID.MAX_ACCELERATION
+        )
+    );
 
-        this.enable();
+    public ElevatorSubsystem() {
+        leaderConfig
+            .idleMode(IdleMode.kBrake)
+            .inverted(Elevator.Leader.INVERTED);  
+        leaderConfig.encoder
+            .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
+            .velocityConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER);
+            
+        followerConfig
+            .idleMode(IdleMode.kBrake)
+            .inverted(Elevator.Follower.INVERTED);
+        followerConfig.encoder
+            .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
+            .velocityConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER);
 
-        this.elevatorMechanism = rootMechanism.append(new MechanismLigament2d("elevator", 0.0, 90));
-    }
+        leaderMotor.configure(leaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
-    public void setPosition(double positionMeters) {
-        setGoal(MathUtil.clamp(positionMeters, 0.0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters));
+        leaderEncoder.setPosition(0);
+        followerEncoder.setPosition(0);
     }
 
-    public double getPosition() {
-        return getMeasurement();
-    }
 
-    public double getGoalPosition() {
-        return this.getController().getGoal().position;
-    }
+    private boolean isZeroed = false;
 
-    double lastVelocity = 0.0;
-    double lastTime = 0.0;
+    private double lastVelocity = 0.0;
+    private double lastTime = 0.0;
 
     @Override
-    public void useOutput(double output, TrapezoidProfile.State setpoint) {
-        double dv = setpoint.velocity - lastVelocity;
-        double dt = Timer.getFPGATimestamp() - lastTime;
-        lastTime = Timer.getFPGATimestamp();
-
-        lastVelocity = setpoint.velocity;
-
-        double ff = feedForward.calculate(setpoint.velocity, dv / dt);
-
-        if (isZeroed || Robot.isSimulation()) {
-            // Controller output voltage
-            double op = ff + output;
-
-            elevatorMotorOne.setVoltage(op);
-            elevatorMotorTwo.setVoltage(op);
+    public void periodic() {
+        // check if needs to be zeroed and is at zero
+        if (!isZeroed && bottomLimit.isPressed()) {
+            leaderEncoder.setPosition(0);
+            isZeroed = true;
+        }
 
-            elevatorTargetP.append(setpoint.position);
-            elevatorTargetV.append(setpoint.velocity);
+        // check if initial zero had been run
+        if (isZeroed) {
+            TrapezoidProfile.State setpoint = elevatorPID.getSetpoint();
+            double deltaVelocity = setpoint.velocity - lastVelocity;
+            double deltaTime = Timer.getFPGATimestamp() - lastTime;
 
-            SmartDashboard.putNumber("Elevator/Current Position", getMeasurement());
-            SmartDashboard.putNumber("Elevator/Current Velocity", elevatorEncoderOne.getVelocity());
+            lastTime = Timer.getFPGATimestamp();
+            lastVelocity = setpoint.velocity;
 
-            SmartDashboard.putNumber("Elevator/Target Position", setpoint.position);
-            SmartDashboard.putNumber("Elevator/Target Velocity", setpoint.velocity);
+            double output = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime)
+                + elevatorPID.calculate(leaderEncoder.getPosition());
 
-            if (Robot.isSimulation()) {
-                simulation.setInputVoltage(MathUtil.clamp((output + ff), -12.0, 12.0));
-            }
+            leaderMotor.setVoltage(output);
+            followerMotor.setVoltage(output);
         } else {
-            // Move down a little bit to zero
-            elevatorMotorOne.set(-0.05);
-            elevatorMotorTwo.set(-0.05);
+            // slowly move down to zero
+            leaderMotor.set(-0.05);
+            followerMotor.set(-0.05);
         }
     }
 
-    @Override
-    public double getMeasurement() {
-        return Robot.isSimulation() ? simulation.getPositionMeters() : elevatorEncoderOne.getPosition();
+    public void setPosition(double position) {
+        elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.elevatorHeightMeters));
     }
 
-    @Override
-    public void periodic() {
-        // TODO Auto-generated method stub
-
-        if ((bottomLimit.isPressed()) && (!isZeroed)) {
-            elevatorEncoderOne.setPosition(0.0);
-            setGoal(0.0);
-            isZeroed = true;
-        }
-
-        super.periodic();
-
-        elevatorP.append(getMeasurement());
-        elevatorV.append(elevatorEncoderOne.getVelocity());
-        elevatorOneOutput.append(elevatorMotorOne.getAppliedOutput());
-        elevatorTwoOutput.append(elevatorMotorTwo.getAppliedOutput());
-        elevatorOneCurrent.append(elevatorMotorOne.getOutputCurrent());
-        elevatorTwoCurrent.append(elevatorMotorTwo.getOutputCurrent());
-        this.elevatorMechanism
-                .setLength(getMeasurement() + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0);
-
-        // SmartDashboard.putNumber("Current Elevator Position", getMeasurement());
-        // SmartDashboard.putNumber("Goal Elevator Position",
-        // this.getController().getGoal().position);
-        SmartDashboard.putBoolean("Elevator Bottom Limit", bottomLimit.isPressed());
-        SmartDashboard.putData("Elevator Mechanism", mechanism2D);
+    public double getPosition() {
+        return leaderEncoder.getPosition();
     }
 
-    @Override
-    public void simulationPeriodic() {
-        // TODO Auto-generated method stub
-        super.simulationPeriodic();
-
-        simulation.update(0.020);
-
-        RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(simulation.getCurrentDrawAmps()));
+    public double getGoalPosition() {
+        return elevatorPID.getGoal().position;
     }
 
     public boolean isInPosition() {
-        return this.getController().atGoal();
+        return elevatorPID.atGoal();
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java
deleted file mode 100644
index b6155eb..0000000
--- a/src/main/java/frc/robot/subsystems/NewElevatorSubsystem.java
+++ /dev/null
@@ -1,24 +0,0 @@
-package frc.robot.subsystems;
-
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-
-import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.Elevator;
-
-public class NewElevatorSubsystem extends SubsystemBase {
-
-    private final SparkMax leaderMotor = new SparkMax(Elevator.elevatorOnePort, MotorType.kBrushless);
-    private final SparkMax followerMotor = new SparkMax(Elevator.elevatorTwoPort, MotorType.kBrushless);
-
-    private final SparkMaxConfig leaderConfig = new SparkMaxConfig();
-    private final SparkMaxConfig followerConfig = new SparkMaxConfig();
-
-    private final ElevatorFeedforward feedForward = Elevator.feedForward;
-
-    public NewElevatorSubsystem() {
-        
-    }
-}

From fd0f294dcb2c3ff9016f80c6cf32150b78f642e0 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 22 Jan 2025 20:27:59 -0600
Subject: [PATCH 04/71] partially filled out algae subsystem

---
 .../subsystems/AlgaeSubsystem/Algae.java      | 39 +++++++++++++++++++
 .../subsystems/AlgaeSubsystem/AlgaeArm.java   | 34 +++++++++++++++-
 .../AlgaeSubsystem/AlgaeIntake.java           | 20 ++++++++++
 3 files changed, 91 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java

diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
new file mode 100644
index 0000000..63a6810
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.AlgaeSubsystem;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Algae extends SubsystemBase{
+    
+    private final AlgaeArm arm;
+    private final AlgaeIntake intake;
+
+    public enum AlgaePresets {
+        STOW(10, 0),
+        FOO(80, 1);
+
+
+        double armAngle;
+        double intakePercentage;
+
+        private AlgaePresets(double armAngle, double intakePercentage) {
+            this.armAngle = armAngle;
+            this.intakePercentage = intakePercentage;
+        }
+    }
+
+    private AlgaePresets currentPreset = AlgaePresets.STOW;
+
+    public Algae(AlgaeArm arm, AlgaeIntake intake) {
+        this.arm = arm;
+        this.intake = intake;
+    }
+
+    public void setAlgaePreset(AlgaePresets preset) {
+        if (preset != currentPreset) {
+            arm.setGoalDegrees(preset.armAngle);
+            intake.setOutputPercentage(preset.intakePercentage);
+
+            currentPreset = preset;
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
index a442633..9bf158e 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
@@ -1,16 +1,46 @@
 package frc.robot.subsystems.AlgaeSubsystem;
 
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
 public class AlgaeArm extends SubsystemBase{
     public final SparkMax pivotMotor = new SparkMax(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
-        
+    public final SparkMaxConfig pivotConfig = new SparkMaxConfig();
+    public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
+
+    public final ProfiledPIDController pivotPID = Algae.Pivot.PID;
+
+    public AlgaeArm() {
+        pivotConfig.idleMode(IdleMode.kBrake);
+        pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
     @Override
     public void periodic() {
-        
+        double output = pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble());
+
+        pivotMotor.set(output);
+    }
+
+    public void setGoalDegrees(double goal) {
+        pivotPID.setGoal(MathUtil.clamp(goal, Algae.Pivot.RETRACTED_LIMIT_DEGREES, Algae.Pivot.EXTENDED_LIMIT_DEGREES));
+    }
+
+    public double getGoalDegrees() {
+        return pivotPID.getGoal().position;
+    }
+
+    public double getPositionDegrees() {
+        return pivotEncoder.getAbsolutePosition().getValueAsDouble();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
index 24da632..01b73e7 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
@@ -3,14 +3,34 @@
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
 public class AlgaeIntake extends SubsystemBase {
     private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless);
 
+    private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
+        Algae.Intake.POSITIVE_RATE_LIMIT,
+        Algae.Intake.NEGATIVE_RATE_LIMIT, 
+        0
+    );
+
+    private double outputPercentage = 0.0;
+
     @Override
     public void periodic() {
+        double output = intakeProfile.calculate(outputPercentage);
+
+        intakeMotor.set(output);
+    }
+
+    public void setOutputPercentage(double outputPercentage) {
+        this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1);
+    }
 
+    public double getOutputPercentage() {
+        return outputPercentage;
     }
 }

From e827c1060928777a4d17848bc159138e9f051e7f Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 23 Jan 2025 19:04:18 -0600
Subject: [PATCH 05/71] partially filled in coral subsystem

---
 src/main/java/frc/robot/Constants.java        | 40 +++++++---
 .../robot/subsystems/AlgaeSubsystem/Main.java | 19 -----
 .../subsystems/CoralSubsystem/Coral.java      | 50 +++++++++++++
 .../subsystems/CoralSubsystem/CoralArm.java   | 73 ++++++++++++++++++-
 .../CoralElevator.java}                       | 16 ++--
 .../CoralSubsystem/CoralIntake.java           | 36 ++++++++-
 .../robot/subsystems/CoralSubsystem/Main.java | 19 -----
 7 files changed, 194 insertions(+), 59 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
 create mode 100644 src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
 rename src/main/java/frc/robot/subsystems/{ElevatorSubsystem.java => CoralSubsystem/CoralElevator.java} (95%)
 delete mode 100644 src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 340e001..7ed1b97 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -12,6 +12,7 @@
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -169,6 +170,10 @@ public static class Climber {
   public static class Coral {
     public static class Pivot {
       public static final int MOTOR_PORT = 14;
+      public static final int ENCODER_PORT = 28;
+
+      public static final double MAXIMUM_ANGLE = 80;
+
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
         0.0,
@@ -188,6 +193,9 @@ public static class Roll {
     }
     public static class Pitch {
       public static final int MOTOR_PORT = 16;
+
+      public static final double MAXIMUM_ANGLE = 45;
+
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
         0.0,
@@ -198,37 +206,51 @@ public static class Pitch {
 
     public static class Intake {
       public static final int MOTOR_PORT = 17;
+
+      public static final double POSITIVE_RATE_LIMIT = 5.0;
+      public static final double NEGATIVE_RATE_LIMIT = 5.0;
     }
   }
   
   // TODO: ##################### PLACEHOLDERS #####################
-  public static class Algae {
+  public static final class Algae {
     public static final class Pivot {
       public static final int MOTOR_PORT = 18;
+      public static final int ENCODER_PORT = 27;
+
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
         0.0,
         0.0,
         null
       );
+
+      public static final double RETRACTED_LIMIT_DEGREES = 10.0;
+      public static final double EXTENDED_LIMIT_DEGREES = 90.0;
     }
 
     public static final class Intake {
       public static final int MOTOR_PORT = 19;
+
+      public static final double POSITIVE_RATE_LIMIT = 5.0;
+      public static final double NEGATIVE_RATE_LIMIT = -5.0;
     }
   }
 
-  public static class Elevator {
-    public static final int elevatorOnePort = 10;
-    public static final int elevatorTwoPort = 11;
+  public static final class Elevator {
+    public static final class Leader {
+      public static final int MOTOR_PORT = 10;
+      public static final boolean INVERTED = true;
+    }
 
-    public static boolean elevatorOneInverted = true;
-    public static boolean elevatorTwoInverted = false;
+    public static final class Follower {
+      public static final int MOTOR_PORT = 11;
+      public static final boolean INVERTED = false;
+    }
 
     public static Type bottomLimitMode = Type.kNormallyOpen;
 
-
-    public static double motorTurnsPerMeter = 39.44;
+    public static double MOTOR_REVOLUTIONS_PER_METER = 39.44;
 
     public static class PID {
       public static double kP = 20.0; // 9.0;
@@ -248,7 +270,7 @@ public static class FeedforwardConstants {
       public static double Kg = 0.1;
     }
 
-    public static ElevatorFeedforward feedForward = new ElevatorFeedforward(
+    public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward(
       FeedforwardConstants.Ks,
       FeedforwardConstants.Kg,
       FeedforwardConstants.Kv,
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
deleted file mode 100644
index e3ade48..0000000
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Main.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package frc.robot.subsystems.AlgaeSubsystem;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Main extends SubsystemBase{
-    
-    private final AlgaeArm arm;
-    private final AlgaeIntake intake;
-
-    public Main(AlgaeArm arm, AlgaeIntake intake) {
-        this.arm = arm;
-        this.intake = intake;
-    }
-
-    @Override
-    public void periodic() {
-        
-    }
-}
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
new file mode 100644
index 0000000..faa0fb4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -0,0 +1,50 @@
+package frc.robot.subsystems.CoralSubsystem;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Coral extends SubsystemBase {
+
+    private final CoralArm arm;
+    private final CoralIntake intake;
+    private final CoralElevator elevator;
+
+    public enum CoralPresets {
+        LEVEL_1(1.0, 20.0, 0.0, 0.0),
+        LEVEL_2(2.0, 20.0, 0.0, 0.0),
+        LEVEL_3(3.0, 20.0, 0.0, 0.0),
+        LEVEL_4(4.0, 20.0, 0.0, 0.0),
+        INTAKE(1.0, 110.0, 0.0, 0.0),
+        STOW(1.0, 0.0, 0.0, 0.0);
+
+        double elevatorHeight;
+        double pivotAngle;
+        double rollAngle;
+        double pitchAngle;
+
+        private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
+            this.elevatorHeight = elevatorHeight;
+            this.pivotAngle = pivotAngle;
+            this.rollAngle = rollAngle;
+            this.pitchAngle = pitchAngle;
+        }
+    }
+
+    private CoralPresets currentPreset = CoralPresets.STOW;
+
+    public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
+        this.arm = arm;
+        this.intake = intake;
+        this.elevator = elevator;
+    }
+
+    public void setCoralPreset(CoralPresets preset) {
+        if (preset != currentPreset) {
+            elevator.setGoalPosition(preset.elevatorHeight);
+            arm.setPivotGoalDegrees(preset.pivotAngle);
+            arm.setRollGoalDegrees(preset.rollAngle);
+            arm.setPitchGoalDegress(preset.pitchAngle);
+
+            currentPreset = preset;
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
index 46e2c9e..461b91e 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
@@ -1,20 +1,91 @@
 package frc.robot.subsystems.CoralSubsystem;
 
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase{
     private final SparkMax pivotMotor = new SparkMax(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-
     private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
 
+    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+    private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
+    private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
+
+    private final SparkMaxConfig pivotConfig = new SparkMaxConfig();
+    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+
+    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+    private final ProfiledPIDController rollPID = Coral.Roll.PID;
+    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+    
+
+    public CoralArm() {
+        pivotConfig.idleMode(IdleMode.kBrake);
+        rollConfig.idleMode(IdleMode.kBrake);
+        pitchConfig.idleMode(IdleMode.kBrake);
+
+        pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        rollMotor.configure(rollConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        pitchMotor.configure(pitchConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        rollPID.enableContinuousInput(-180, 180);
+    }
+
     @Override
     public void periodic() {
+        pivotMotor.set(pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble()));
+        rollMotor.set(rollPID.calculate(rollEncoder.getPosition()));
+        pitchMotor.set(pitchPID.calculate(pitchEncoder.getPosition()));
+    }
+
+    public void setPivotGoalDegrees(double goal) {
+        pivotPID.setGoal(MathUtil.clamp(goal, Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE));
+    }
+
+    public void setRollGoalDegrees(double goal) {
+        rollPID.setGoal(goal);
+    }
 
+    public void setPitchGoalDegress(double goal) {
+        pitchPID.setGoal(MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE, -1 * Coral.Pitch.MAXIMUM_ANGLE));
     }
+
+    public double getPivotGoalDegress() {
+        return pivotPID.getGoal().position;
+    }
+
+    public double getRollGoalDegrees() {
+        return rollPID.getGoal().position;
+    }
+
+    public double getPitchGoalDegrees() {
+        return pitchPID.getGoal().position;
+    }
+
+    public double getPivotPositionDegrees() {
+        return pivotEncoder.getAbsolutePosition().getValueAsDouble();
+    }
+
+    public double getRollPositionDegrees() {
+        return rollEncoder.getPosition();
+    }
+
+    public double getPitchPositionDegrees() {
+        return pitchEncoder.getPosition();
+    }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
similarity index 95%
rename from src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
rename to src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
index e7a5af7..ce9c5e8 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems.CoralSubsystem;
 
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -17,7 +17,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Elevator;
 
-public class ElevatorSubsystem extends SubsystemBase {
+public class CoralElevator extends SubsystemBase {
 
     private final SparkMax leaderMotor = new SparkMax(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax followerMotor = new SparkMax(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless);
@@ -42,7 +42,7 @@ public class ElevatorSubsystem extends SubsystemBase {
         )
     );
 
-    public ElevatorSubsystem() {
+    public CoralElevator() {
         leaderConfig
             .idleMode(IdleMode.kBrake)
             .inverted(Elevator.Leader.INVERTED);  
@@ -99,18 +99,18 @@ public void periodic() {
         }
     }
 
-    public void setPosition(double position) {
+    public void setGoalPosition(double position) {
         elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.elevatorHeightMeters));
     }
+    
+    public double getGoalPosition() {
+        return elevatorPID.getGoal().position;
+    }
 
     public double getPosition() {
         return leaderEncoder.getPosition();
     }
 
-    public double getGoalPosition() {
-        return elevatorPID.getGoal().position;
-    }
-
     public boolean isInPosition() {
         return elevatorPID.atGoal();
     }
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
index 3941f72..c16cab3 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
@@ -1,16 +1,46 @@
 package frc.robot.subsystems.CoralSubsystem;
 
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Coral;
 
 public class CoralIntake extends SubsystemBase {
-    public final SparkMax intakeMotor = new SparkMax(Coral.Intake.MOTOR_PORT, MotorType.kBrushless);
-    
+    private final SparkMax intakeMotor = new SparkMax(Coral.Intake.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMaxConfig intakeConfig = new SparkMaxConfig();
+
+    private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
+        Coral.Intake.POSITIVE_RATE_LIMIT,
+        Coral.Intake.NEGATIVE_RATE_LIMIT,
+        0
+    );
+
+    public CoralIntake() {
+        intakeConfig.idleMode(IdleMode.kBrake);
+        intakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    private double outputPercentage = 0.0;
+
     @Override
     public void periodic() {
-        
+        double output = intakeProfile.calculate(outputPercentage);
+
+        intakeMotor.set(output);
+    }
+
+    public void setOutputPercentage(double outputPercentage) {
+        this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1);
+    }
+
+    public double getOutputPercentage() {
+        return outputPercentage;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java
deleted file mode 100644
index dc539b1..0000000
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Main.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package frc.robot.subsystems.CoralSubsystem;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Main extends SubsystemBase {
-
-    private final CoralArm arm;
-    private final CoralIntake intake;
-
-    public Main(CoralArm arm, CoralIntake intake) {
-        this.arm = arm;
-        this.intake = intake;
-    }
-
-    @Override
-    public void periodic() {
-        
-    }
-}

From 4f2f61c6c9e246b0c277f3af148223d203f41f73 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 23 Jan 2025 19:31:36 -0600
Subject: [PATCH 06/71] basic climber things

---
 .../robot/subsystems/ClimberSubsystem.java    | 24 ++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index b6b46ff..5f4d7db 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -1,16 +1,38 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Climber;
 
 public class ClimberSubsystem extends SubsystemBase {
     private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMaxConfig climberConfig = new SparkMaxConfig();
+
+    public ClimberSubsystem() {
+        climberConfig.idleMode(IdleMode.kBrake);
+
+        climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    private double output = 0.0;
 
     @Override
     public void periodic() {
-        
+        climberMotor.set(output);
+    }
+
+    // TODO: limit the output somehow
+    public void setOutput(double output) {
+        this.output = output;
+    }
+
+    public double getOutput() {
+        return output;
     }
 }

From f9757fea9ec6642d69db10eaaf1cb36894c85351 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Tue, 28 Jan 2025 08:53:13 -0600
Subject: [PATCH 07/71] Fixed FWERB code

---
 .../deploy/pathplanner/autos/New Auto.auto    | 14 ++-
 .../deploy/pathplanner/autos/Testing.auto     | 19 ++++
 .../paths/{Example 1.path => New Path.path}   | 16 ++--
 .../deploy/pathplanner/paths/route 1.path     | 95 +++++++++++++++++++
 .../deploy/pathplanner/paths/route 2.path     | 54 +++++++++++
 src/main/java/frc/robot/Constants.java        | 15 ++-
 vendordeps/AdvantageKit.json                  | 11 ++-
 ...enix6-25.2.0.json => Phoenix6-25.2.1.json} | 58 +++++------
 ...Lib-2025.0.1.json => REVLib-2025.0.2.json} | 12 +--
 9 files changed, 240 insertions(+), 54 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Testing.auto
 rename src/main/deploy/pathplanner/paths/{Example 1.path => New Path.path} (76%)
 create mode 100644 src/main/deploy/pathplanner/paths/route 1.path
 create mode 100644 src/main/deploy/pathplanner/paths/route 2.path
 rename vendordeps/{Phoenix6-25.2.0.json => Phoenix6-25.2.1.json} (92%)
 rename vendordeps/{REVLib-2025.0.1.json => REVLib-2025.0.2.json} (90%)

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
index 0b46cdb..d96865d 100644
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -7,7 +7,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example 1"
+            "pathName": "New Path"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "route 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "route 2"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Testing.auto b/src/main/deploy/pathplanner/autos/Testing.auto
new file mode 100644
index 0000000..268147b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Testing.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example 1.path b/src/main/deploy/pathplanner/paths/New Path.path
similarity index 76%
rename from src/main/deploy/pathplanner/paths/Example 1.path
rename to src/main/deploy/pathplanner/paths/New Path.path
index 9278a95..9934f81 100644
--- a/src/main/deploy/pathplanner/paths/Example 1.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.6575,
-        "y": 3.673999999999999
+        "x": 7.108709016393442,
+        "y": 1.2618340163934432
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.6575000000000006,
-        "y": 3.673999999999999
+        "x": 6.653176229508197,
+        "y": 1.2258709016393436
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 5.358504098360656,
+        "y": 1.2618340163934432
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 5.913831967213116,
+        "y": 1.3667008196721315
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
new file mode 100644
index 0000000..e04cf82
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/route 1.path	
@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.03483606557377,
+        "y": 1.6454405737704905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.3155737704918025,
+        "y": 2.472592213114752
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.9919057377049176,
+        "y": 2.7602971311475404
+      },
+      "prevControl": {
+        "x": 4.406172988804652,
+        "y": 2.7268884818653034
+      },
+      "nextControl": {
+        "x": 3.248668032786885,
+        "y": 2.820235655737705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3785860655737705,
+        "y": 0.8782274590163933
+      },
+      "prevControl": {
+        "x": 2.3136270491803277,
+        "y": 0.7583504098360652
+      },
+      "nextControl": {
+        "x": 1.1306156494023096,
+        "y": 0.9100185380127345
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6802254098360656,
+        "y": 2.8921618852459017
+      },
+      "prevControl": {
+        "x": 3.464446721311475,
+        "y": 2.62843237704918
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9092363054778103,
+      "rotationDegrees": 59.99999999999999
+    },
+    {
+      "waypointRelativePos": 1.87,
+      "rotationDegrees": 55.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -33.690067525979806
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/route 2.path b/src/main/deploy/pathplanner/paths/route 2.path
new file mode 100644
index 0000000..571c9e5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/route 2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6322745901639335,
+        "y": 2.8561987704918033
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.35655737704918,
+        "y": 2.5804815573770488
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4385245901639343,
+        "y": 0.9261782786885238
+      },
+      "prevControl": {
+        "x": 1.7142418032786886,
+        "y": 1.3337602459016387
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 53.97262661489648
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.743562836470616
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 639b994..a0d2c4f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
@@ -60,9 +61,9 @@ public static Alliance getAlliance() {
 
   public static class SwerveModuleConstants {
     public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
-    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d);
+    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d); // 6.75:1
     // This is for L2 modules with 16T pinions
-    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d) * (16.f / 14.f);
+    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d);
 
     public static final double DRIVE_ROTATION_TO_METER = DRIVE_GEAR_RATIO * Math.PI * WHEEL_DIAMETER;
     public static final double STEER_ROTATION_TO_RADIANS = STEERING_GEAR_RATIO * Math.PI * 2d;
@@ -122,7 +123,7 @@ public static class DriveConstants {
     public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec
 
     // TODO: ############## REPLACE PLACEHOLDERS ##############
-    public static final double MAX_MODULE_CURRENT = 10;
+    public static final double MAX_MODULE_CURRENT = 100;
 
     public static final double TRACK_WIDTH = Units.inchesToMeters(19.75);
     public static final double WHEEL_BASE = Units.inchesToMeters(19.75);
@@ -212,8 +213,12 @@ public static final class PathPlannerConstants {
         SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
         DCMotor.getKrakenX60(1),
         DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
-        4
-      )
+        1
+      ),
+      new Translation2d(-0.5, 0.5),
+      new Translation2d(0.5, 0.5),
+      new Translation2d(-0.5, -0.5),
+      new Translation2d(0.5, -0.5)
     );
   }
 
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 1fa7c03..03df051 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.0.0",
+    "version": "4.1.0",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,21 +12,22 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "4.0.0"
+            "version": "4.1.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "4.0.0",
+            "version": "4.1.0",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [
                 "linuxathena",
-                "windowsx86-64",
                 "linuxx86-64",
-                "osxuniversal"
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
             ]
         }
     ],
diff --git a/vendordeps/Phoenix6-25.2.0.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 92%
rename from vendordeps/Phoenix6-25.2.0.json
rename to vendordeps/Phoenix6-25.2.1.json
index 38747fb..1397da1 100644
--- a/vendordeps/Phoenix6-25.2.0.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.0.json",
+    "fileName": "Phoenix6-25.2.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.0",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.0"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -354,7 +354,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -370,7 +370,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json
similarity index 90%
rename from vendordeps/REVLib-2025.0.1.json
rename to vendordeps/REVLib-2025.0.2.json
index c998054..c29aefa 100644
--- a/vendordeps/REVLib-2025.0.1.json
+++ b/vendordeps/REVLib-2025.0.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.1.json",
+    "fileName": "REVLib-2025.0.2.json",
     "name": "REVLib",
-    "version": "2025.0.1",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.1"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -53,7 +53,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,

From e187d182632bff5cc3a652a48aa757065f621ac6 Mon Sep 17 00:00:00 2001
From: michael-m-2983 <116770284+michael-m-2983@users.noreply.github.com>
Date: Tue, 28 Jan 2025 12:56:40 -0600
Subject: [PATCH 08/71] Fixed FWERB code (#3)

Co-authored-by: Yunju607 <yj207lee@gmail.com>
---
 .../deploy/pathplanner/autos/New Auto.auto    | 14 ++-
 .../deploy/pathplanner/autos/Testing.auto     | 19 ++++
 .../paths/{Example 1.path => New Path.path}   | 16 ++--
 .../deploy/pathplanner/paths/route 1.path     | 95 +++++++++++++++++++
 .../deploy/pathplanner/paths/route 2.path     | 54 +++++++++++
 src/main/java/frc/robot/Constants.java        | 15 ++-
 vendordeps/AdvantageKit.json                  | 11 ++-
 ...enix6-25.2.0.json => Phoenix6-25.2.1.json} | 58 +++++------
 ...Lib-2025.0.1.json => REVLib-2025.0.2.json} | 12 +--
 9 files changed, 240 insertions(+), 54 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Testing.auto
 rename src/main/deploy/pathplanner/paths/{Example 1.path => New Path.path} (76%)
 create mode 100644 src/main/deploy/pathplanner/paths/route 1.path
 create mode 100644 src/main/deploy/pathplanner/paths/route 2.path
 rename vendordeps/{Phoenix6-25.2.0.json => Phoenix6-25.2.1.json} (92%)
 rename vendordeps/{REVLib-2025.0.1.json => REVLib-2025.0.2.json} (90%)

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
index 0b46cdb..d96865d 100644
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -7,7 +7,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example 1"
+            "pathName": "New Path"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "route 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "route 2"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Testing.auto b/src/main/deploy/pathplanner/autos/Testing.auto
new file mode 100644
index 0000000..268147b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Testing.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example 1.path b/src/main/deploy/pathplanner/paths/New Path.path
similarity index 76%
rename from src/main/deploy/pathplanner/paths/Example 1.path
rename to src/main/deploy/pathplanner/paths/New Path.path
index 9278a95..9934f81 100644
--- a/src/main/deploy/pathplanner/paths/Example 1.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.6575,
-        "y": 3.673999999999999
+        "x": 7.108709016393442,
+        "y": 1.2618340163934432
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.6575000000000006,
-        "y": 3.673999999999999
+        "x": 6.653176229508197,
+        "y": 1.2258709016393436
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 5.358504098360656,
+        "y": 1.2618340163934432
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 5.913831967213116,
+        "y": 1.3667008196721315
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
new file mode 100644
index 0000000..e04cf82
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/route 1.path	
@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.03483606557377,
+        "y": 1.6454405737704905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.3155737704918025,
+        "y": 2.472592213114752
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.9919057377049176,
+        "y": 2.7602971311475404
+      },
+      "prevControl": {
+        "x": 4.406172988804652,
+        "y": 2.7268884818653034
+      },
+      "nextControl": {
+        "x": 3.248668032786885,
+        "y": 2.820235655737705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3785860655737705,
+        "y": 0.8782274590163933
+      },
+      "prevControl": {
+        "x": 2.3136270491803277,
+        "y": 0.7583504098360652
+      },
+      "nextControl": {
+        "x": 1.1306156494023096,
+        "y": 0.9100185380127345
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6802254098360656,
+        "y": 2.8921618852459017
+      },
+      "prevControl": {
+        "x": 3.464446721311475,
+        "y": 2.62843237704918
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9092363054778103,
+      "rotationDegrees": 59.99999999999999
+    },
+    {
+      "waypointRelativePos": 1.87,
+      "rotationDegrees": 55.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -33.690067525979806
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/route 2.path b/src/main/deploy/pathplanner/paths/route 2.path
new file mode 100644
index 0000000..571c9e5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/route 2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6322745901639335,
+        "y": 2.8561987704918033
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.35655737704918,
+        "y": 2.5804815573770488
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4385245901639343,
+        "y": 0.9261782786885238
+      },
+      "prevControl": {
+        "x": 1.7142418032786886,
+        "y": 1.3337602459016387
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 53.97262661489648
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.743562836470616
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 639b994..a0d2c4f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
@@ -60,9 +61,9 @@ public static Alliance getAlliance() {
 
   public static class SwerveModuleConstants {
     public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
-    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d);
+    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d); // 6.75:1
     // This is for L2 modules with 16T pinions
-    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d) * (16.f / 14.f);
+    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d);
 
     public static final double DRIVE_ROTATION_TO_METER = DRIVE_GEAR_RATIO * Math.PI * WHEEL_DIAMETER;
     public static final double STEER_ROTATION_TO_RADIANS = STEERING_GEAR_RATIO * Math.PI * 2d;
@@ -122,7 +123,7 @@ public static class DriveConstants {
     public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec
 
     // TODO: ############## REPLACE PLACEHOLDERS ##############
-    public static final double MAX_MODULE_CURRENT = 10;
+    public static final double MAX_MODULE_CURRENT = 100;
 
     public static final double TRACK_WIDTH = Units.inchesToMeters(19.75);
     public static final double WHEEL_BASE = Units.inchesToMeters(19.75);
@@ -212,8 +213,12 @@ public static final class PathPlannerConstants {
         SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
         DCMotor.getKrakenX60(1),
         DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
-        4
-      )
+        1
+      ),
+      new Translation2d(-0.5, 0.5),
+      new Translation2d(0.5, 0.5),
+      new Translation2d(-0.5, -0.5),
+      new Translation2d(0.5, -0.5)
     );
   }
 
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 1fa7c03..03df051 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.0.0",
+    "version": "4.1.0",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,21 +12,22 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "4.0.0"
+            "version": "4.1.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "4.0.0",
+            "version": "4.1.0",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [
                 "linuxathena",
-                "windowsx86-64",
                 "linuxx86-64",
-                "osxuniversal"
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
             ]
         }
     ],
diff --git a/vendordeps/Phoenix6-25.2.0.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 92%
rename from vendordeps/Phoenix6-25.2.0.json
rename to vendordeps/Phoenix6-25.2.1.json
index 38747fb..1397da1 100644
--- a/vendordeps/Phoenix6-25.2.0.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.0.json",
+    "fileName": "Phoenix6-25.2.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.0",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.0"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -354,7 +354,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -370,7 +370,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json
similarity index 90%
rename from vendordeps/REVLib-2025.0.1.json
rename to vendordeps/REVLib-2025.0.2.json
index c998054..c29aefa 100644
--- a/vendordeps/REVLib-2025.0.1.json
+++ b/vendordeps/REVLib-2025.0.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.1.json",
+    "fileName": "REVLib-2025.0.2.json",
     "name": "REVLib",
-    "version": "2025.0.1",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.1"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -53,7 +53,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.1",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,

From 028e1f4858a0924c31b623487cf68692bf6dd769 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Tue, 28 Jan 2025 14:50:55 -0600
Subject: [PATCH 09/71] setting.json and updates

---
 .../deploy/pathplanner/paths/route 1.path     |  4 +--
 src/main/deploy/pathplanner/settings.json     | 32 +++++++++++++++++++
 ....2.1.json => PathplannerLib-2025.2.2.json} |  8 ++---
 3 files changed, 38 insertions(+), 6 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/settings.json
 rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%)

diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
index e04cf82..b05e8de 100644
--- a/src/main/deploy/pathplanner/paths/route 1.path	
+++ b/src/main/deploy/pathplanner/paths/route 1.path	
@@ -40,8 +40,8 @@
         "y": 0.7583504098360652
       },
       "nextControl": {
-        "x": 1.1306156494023096,
-        "y": 0.9100185380127345
+        "x": 1.1306156494023079,
+        "y": 0.9100185380127347
       },
       "isLocked": false,
       "linkedName": null
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..5bf438c
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.1.json
rename to vendordeps/PathplannerLib-2025.2.2.json
index 71e25f3..a5bf9ee 100644
--- a/vendordeps/PathplannerLib-2025.2.1.json
+++ b/vendordeps/PathplannerLib-2025.2.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.1.json",
+    "fileName": "PathplannerLib-2025.2.2.json",
     "name": "PathplannerLib",
-    "version": "2025.2.1",
+    "version": "2025.2.2",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.1"
+            "version": "2025.2.2"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.1",
+            "version": "2025.2.2",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

From b83279575ebfc4565cb5b50a69fa4a28b8f24d60 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 18:19:16 -0600
Subject: [PATCH 10/71] changed elevator to zero by motor feedback

---
 .../robot/subsystems/CoralSubsystem/CoralElevator.java   | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
index ce9c5e8..ce60520 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
@@ -28,8 +28,6 @@ public class CoralElevator extends SubsystemBase {
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
 
-    private final SparkLimitSwitch bottomLimit = leaderMotor.getReverseLimitSwitch();
-
 
     private final ElevatorFeedforward feedForward = Elevator.FEEDFORWARD;
     private final ProfiledPIDController elevatorPID = new ProfiledPIDController(
@@ -73,7 +71,8 @@ public CoralElevator() {
     @Override
     public void periodic() {
         // check if needs to be zeroed and is at zero
-        if (!isZeroed && bottomLimit.isPressed()) {
+                                // TODO: ######################### PLACEHOLDERS AGAIN #########################
+        if (!isZeroed && (leaderMotor.getOutputCurrent() > 2 || followerMotor.getOutputCurrent() > 2)) {
             leaderEncoder.setPosition(0);
             isZeroed = true;
         }
@@ -114,4 +113,8 @@ public double getPosition() {
     public boolean isInPosition() {
         return elevatorPID.atGoal();
     }
+
+    public void zeroElevator() {
+        isZeroed = false;
+    }
 }

From 1c23ca87564869a7375b4d25ff8bf26a6c1d3b91 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 18:28:29 -0600
Subject: [PATCH 11/71] corrected some motors to sparkflex

---
 .../robot/subsystems/AlgaeSubsystem/AlgaeArm.java   |  3 ++-
 .../robot/subsystems/CoralSubsystem/CoralArm.java   |  3 ++-
 .../subsystems/CoralSubsystem/CoralElevator.java    | 13 ++++++-------
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
index 9bf158e..658d343 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.AlgaeSubsystem;
 
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -14,7 +15,7 @@
 import frc.robot.Constants.Algae;
 
 public class AlgaeArm extends SubsystemBase{
-    public final SparkMax pivotMotor = new SparkMax(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    public final SparkFlex pivotMotor = new SparkFlex(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
     public final SparkMaxConfig pivotConfig = new SparkMaxConfig();
     public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
 
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
index 461b91e..c8e4e51 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -15,7 +16,7 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase{
-    private final SparkMax pivotMotor = new SparkMax(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
 
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
index ce60520..be25ef0 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
@@ -4,10 +4,9 @@
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkLimitSwitch;
-import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -19,11 +18,11 @@
 
 public class CoralElevator extends SubsystemBase {
 
-    private final SparkMax leaderMotor = new SparkMax(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax followerMotor = new SparkMax(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkFlex leaderMotor = new SparkFlex(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkFlex followerMotor = new SparkFlex(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless);
 
-    private final SparkMaxConfig leaderConfig = new SparkMaxConfig();
-    private final SparkMaxConfig followerConfig = new SparkMaxConfig();
+    private final SparkFlexConfig leaderConfig = new SparkFlexConfig();
+    private final SparkFlexConfig followerConfig = new SparkFlexConfig();
 
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();

From c1eaf559fd1be0143befe52e778adcb2c5fefe5d Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 18:47:23 -0600
Subject: [PATCH 12/71] changed coral intake to talonfx

---
 .../robot/subsystems/CoralSubsystem/CoralIntake.java   | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
index c16cab3..ceb8f84 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
@@ -1,5 +1,8 @@
 package frc.robot.subsystems.CoralSubsystem;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -13,8 +16,8 @@
 import frc.robot.Constants.Coral;
 
 public class CoralIntake extends SubsystemBase {
-    private final SparkMax intakeMotor = new SparkMax(Coral.Intake.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMaxConfig intakeConfig = new SparkMaxConfig();
+    private final TalonFX intakeMotor = new TalonFX(Coral.Intake.MOTOR_PORT);
+
 
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
         Coral.Intake.POSITIVE_RATE_LIMIT,
@@ -23,8 +26,7 @@ public class CoralIntake extends SubsystemBase {
     );
 
     public CoralIntake() {
-        intakeConfig.idleMode(IdleMode.kBrake);
-        intakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        intakeMotor.setNeutralMode(NeutralModeValue.Brake);
     }
 
     private double outputPercentage = 0.0;

From bd1648114229f85234dbb6d11e975495aa5adcde Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Wed, 29 Jan 2025 19:26:53 -0600
Subject: [PATCH 13/71] Updates 2 :)

---
 .../deploy/pathplanner/autos/New Auto.auto    |  2 +-
 .../deploy/pathplanner/paths/New Path.path    |  4 +-
 src/main/deploy/pathplanner/paths/Path 3.path | 75 +++++++++++++++++++
 .../deploy/pathplanner/paths/route 1.path     |  5 --
 src/main/java/frc/robot/Constants.java        |  4 +-
 src/main/java/frc/robot/RobotContainer.java   | 22 ++++--
 6 files changed, 94 insertions(+), 18 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/Path 3.path

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
index d96865d..4944055 100644
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -13,7 +13,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "route 1"
+            "pathName": "Path 3"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 9934f81..63e13e5 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -20,8 +20,8 @@
         "y": 1.2618340163934432
       },
       "prevControl": {
-        "x": 5.913831967213116,
-        "y": 1.3667008196721315
+        "x": 5.9219262295081965,
+        "y": 1.2498463114754088
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/Path 3.path b/src/main/deploy/pathplanner/paths/Path 3.path
new file mode 100644
index 0000000..3fea5ff
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Path 3.path	
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.154713114754099,
+        "y": 1.273821721311475
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.471413934426229,
+        "y": 2.1129610655737703
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.800102459016393,
+        "y": 2.71234631147541
+      },
+      "prevControl": {
+        "x": 4.040483445926598,
+        "y": 2.643666029501063
+      },
+      "nextControl": {
+        "x": 3.5062341001529425,
+        "y": 2.7963086997221134
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5703893442622954,
+        "y": 0.722387295081968
+      },
+      "prevControl": {
+        "x": 1.8461065573770497,
+        "y": 1.0460553278688542
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8804478208716437,
+      "rotationDegrees": 59.99999999999999
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 53.20990143837071
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
index 194b950..b05e8de 100644
--- a/src/main/deploy/pathplanner/paths/route 1.path	
+++ b/src/main/deploy/pathplanner/paths/route 1.path	
@@ -40,13 +40,8 @@
         "y": 0.7583504098360652
       },
       "nextControl": {
-<<<<<<< HEAD
         "x": 1.1306156494023079,
         "y": 0.9100185380127347
-=======
-        "x": 1.1306156494023096,
-        "y": 0.9100185380127345
->>>>>>> e187d182632bff5cc3a652a48aa757065f621ac6
       },
       "isLocked": false,
       "linkedName": null
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a0d2c4f..32766a1 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -42,8 +42,8 @@ public static class RobotConstants {
     public static final double robotLengthMeters = Units.inchesToMeters(25.0);
     
       // TODO: ############## REPLACE PLACEHOLDERS ##############
-      public static final double TOTAL_MASS_KG = 10;
-      public static final double MOMENT_OF_INERTIA = 1;
+      public static final double TOTAL_MASS_KG = 60;
+      public static final double MOMENT_OF_INERTIA = 5;
   }
 
   public static final class FieldConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0bbe4cc..2f5f022 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,22 +4,28 @@
 
 package frc.robot;
 
-import frc.robot.Constants.*;
-import frc.robot.commands.*;
-import frc.robot.commands.ElevatorCommand.ElevatorPresets;
+import java.util.function.DoubleSupplier;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import frc.robot.subsystems.*;
-
-import java.util.function.DoubleSupplier;
+import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.*;
-import edu.wpi.first.wpilibj2.command.button.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.ControllerConstants;
+import frc.robot.commands.DriveCommand;
+import frc.robot.commands.ElevatorCommand;
+import frc.robot.commands.ElevatorCommand.ElevatorPresets;
+import frc.robot.commands.ElevatorFollowCommand;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since

From fab3931847116b4b52c24dcdc4cf91eb49a1ee8b Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 19:53:06 -0600
Subject: [PATCH 14/71] updated coralarm logic

---
 src/main/java/frc/robot/Constants.java        |  9 +++
 .../subsystems/CoralSubsystem/CoralArm.java   | 78 ++++++++++++++++---
 2 files changed, 77 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7ed1b97..1ead702 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -10,8 +10,10 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -180,6 +182,13 @@ public static class Pivot {
         0.0,
         null
       );
+
+      public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
+        1,
+        1,
+        1,
+        1
+      );
     }
 
     public static class Roll {
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
index c8e4e51..9738bbe 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
@@ -11,7 +11,10 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ControlAffinePlantInversionFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Coral;
 
@@ -30,10 +33,15 @@ public class CoralArm extends SubsystemBase{
 
 
     private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
     private final ProfiledPIDController rollPID = Coral.Roll.PID;
     private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
     
 
+    private double pivotGoal = 0.0;
+    private double rollGoal = 0.0;
+    private double pitchGoal = 0.0;
+
     public CoralArm() {
         pivotConfig.idleMode(IdleMode.kBrake);
         rollConfig.idleMode(IdleMode.kBrake);
@@ -48,33 +56,83 @@ public CoralArm() {
 
     @Override
     public void periodic() {
-        pivotMotor.set(pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble()));
+        // if entering the frame border 
+        // && wrist is at neutral
+        if (
+            (pivotPID.getGoal().position != pivotGoal)
+            && (rollPID.atGoal() && pitchPID.atGoal())
+        ) {
+            // allow arm to enter frame border
+            pivotPID.setGoal(pivotGoal);
+        } else if ( // if exiting the frame border && has exited the frame border
+            ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+            && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) 
+        ) {
+            // set roll and pitch back to their goals
+            rollPID.setGoal(rollGoal);
+            pitchPID.setGoal(pitchGoal);
+        }
+
+        // run the motors
+        pivotMotor.set(
+            pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble())
+            + pivotFeedforward.calculate(Units.degreesToRadians(pivotPID.getGoal().position), 0.0)
+        );
         rollMotor.set(rollPID.calculate(rollEncoder.getPosition()));
         pitchMotor.set(pitchPID.calculate(pitchEncoder.getPosition()));
-    }
+    } 
 
+    // this should be relative to straight upwards.
+    // i.e. 0 should be straight vertical,
+    // 20 would be to the right, -20 to the left
     public void setPivotGoalDegrees(double goal) {
-        pivotPID.setGoal(MathUtil.clamp(goal, Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE));
+        pivotGoal = MathUtil.clamp(goal, Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE);
+        // if passing through the frame border
+        if (
+            ((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0)) 
+            && (Math.abs(this.getPivotPositionDegrees()) < Coral.Pivot.FRAME_BORDER_ANGLE)
+        ) {
+            // set the pivot to stop at the frame border to allow the wrist to move neutral
+            pivotPID.setGoal(
+                Coral.Pivot.FRAME_BORDER_ANGLE
+                * (pivotGoal < 0 ? -1 : 1)
+            );
+            rollPID.setGoal(0);
+            pitchPID.setGoal(0);
+        } else {
+            pivotPID.setGoal(pivotGoal);
+        }
     }
 
+    // similarly, this ranges from -180 to 180
     public void setRollGoalDegrees(double goal) {
-        rollPID.setGoal(goal);
+        rollGoal = goal;
+        // if outside of frame border
+        if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
+            // start moving to goal
+            rollPID.setGoal(rollGoal);
+        }
     }
 
-    public void setPitchGoalDegress(double goal) {
-        pitchPID.setGoal(MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE, -1 * Coral.Pitch.MAXIMUM_ANGLE));
+    public void setPitchGoalDegrees(double goal) {
+        pitchGoal = MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE, -1 * Coral.Pitch.MAXIMUM_ANGLE);
+        // if outside the frame border
+        if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
+            // start moving to goal
+            pitchPID.setGoal(pitchGoal);
+        }
     }
 
-    public double getPivotGoalDegress() {
-        return pivotPID.getGoal().position;
+    public double getPivotGoalDegrees() {
+        return pivotGoal;
     }
 
     public double getRollGoalDegrees() {
-        return rollPID.getGoal().position;
+        return rollGoal;
     }
 
     public double getPitchGoalDegrees() {
-        return pitchPID.getGoal().position;
+        return pitchGoal;
     }
 
     public double getPivotPositionDegrees() {

From 7b1f37ea3d90812877faa2c7518530378934e6d3 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 19:54:39 -0600
Subject: [PATCH 15/71] typo

---
 src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
index faa0fb4..5bcf454 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -42,7 +42,7 @@ public void setCoralPreset(CoralPresets preset) {
             elevator.setGoalPosition(preset.elevatorHeight);
             arm.setPivotGoalDegrees(preset.pivotAngle);
             arm.setRollGoalDegrees(preset.rollAngle);
-            arm.setPitchGoalDegress(preset.pitchAngle);
+            arm.setPitchGoalDegrees(preset.pitchAngle);
 
             currentPreset = preset;
         }

From f4ffbfad29b6e298e3703a77d6d10f08db565d88 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 20:01:24 -0600
Subject: [PATCH 16/71] add feedforward to algaearm

---
 src/main/java/frc/robot/Constants.java                    | 6 ++++++
 .../frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java     | 8 +++++++-
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1ead702..1366811 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -175,6 +175,7 @@ public static class Pivot {
       public static final int ENCODER_PORT = 28;
 
       public static final double MAXIMUM_ANGLE = 80;
+      public static final double FRAME_BORDER_ANGLE = 20;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
@@ -233,6 +234,11 @@ public static final class Pivot {
         0.0,
         null
       );
+      public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
+        1,
+        1,
+        1
+      );
 
       public static final double RETRACTED_LIMIT_DEGREES = 10.0;
       public static final double EXTENDED_LIMIT_DEGREES = 90.0;
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
index 658d343..2e62a23 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
@@ -10,7 +10,9 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
@@ -20,6 +22,7 @@ public class AlgaeArm extends SubsystemBase{
     public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
 
     public final ProfiledPIDController pivotPID = Algae.Pivot.PID;
+    public final ArmFeedforward pivotFeedforward = Algae.Pivot.FEEDFORWARD;
 
     public AlgaeArm() {
         pivotConfig.idleMode(IdleMode.kBrake);
@@ -28,7 +31,10 @@ public AlgaeArm() {
 
     @Override
     public void periodic() {
-        double output = pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble());
+        double output = pivotPID.calculate(
+            pivotEncoder.getAbsolutePosition().getValueAsDouble()
+            + pivotFeedforward.calculate(Units.degreesToRadians(this.getGoalDegrees()), 0.0)
+        );
 
         pivotMotor.set(output);
     }

From 6b2f585216801a870647f5e1b33c5f1f63a84355 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 20:16:20 -0600
Subject: [PATCH 17/71] coral position mirroring

---
 .../subsystems/CoralSubsystem/Coral.java      | 30 ++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
index 5bcf454..8708d78 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -29,7 +29,20 @@ private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle,
         }
     }
 
+    public enum MirrorPresets {
+        RIGHT(false),
+        LEFT(true),
+        STARBOARD(false),
+        PORT(true);
+
+        boolean isMirrored;
+        MirrorPresets(boolean isMirrored) {
+            this.isMirrored = isMirrored;
+        }
+    } 
+
     private CoralPresets currentPreset = CoralPresets.STOW;
+    MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
 
     public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
         this.arm = arm;
@@ -40,11 +53,26 @@ public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
     public void setCoralPreset(CoralPresets preset) {
         if (preset != currentPreset) {
             elevator.setGoalPosition(preset.elevatorHeight);
-            arm.setPivotGoalDegrees(preset.pivotAngle);
+            arm.setPivotGoalDegrees(
+                preset.pivotAngle 
+                * (mirrorSetting.isMirrored ? -1 : 1)
+            );
             arm.setRollGoalDegrees(preset.rollAngle);
             arm.setPitchGoalDegrees(preset.pitchAngle);
 
             currentPreset = preset;
         }
     }
+
+    public void mirrorArm() {
+        if (mirrorSetting.isMirrored) {
+            mirrorSetting = MirrorPresets.RIGHT;
+        } else {
+            mirrorSetting = MirrorPresets.LEFT;
+        }
+    }
+
+    public void mirorArm(MirrorPresets preset) {
+        mirrorSetting = preset;
+    }
 }

From 956fb65e4eb74f6dfb9caa2d2973485367a1cf1e Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 20:40:50 -0600
Subject: [PATCH 18/71] add algaeintake beambreak and update intake logic

---
 src/main/java/frc/robot/Constants.java        |  1 +
 .../subsystems/AlgaeSubsystem/Algae.java      | 31 ++++++++++++++-----
 .../AlgaeSubsystem/AlgaeIntake.java           | 12 +++++--
 3 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1366811..56a5f62 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -246,6 +246,7 @@ public static final class Pivot {
 
     public static final class Intake {
       public static final int MOTOR_PORT = 19;
+      public static final int BEAMBREAK_PORT = 0;
 
       public static final double POSITIVE_RATE_LIMIT = 5.0;
       public static final double NEGATIVE_RATE_LIMIT = -5.0;
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
index 63a6810..ded0daf 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
@@ -8,20 +8,30 @@ public class Algae extends SubsystemBase{
     private final AlgaeIntake intake;
 
     public enum AlgaePresets {
-        STOW(10, 0),
-        FOO(80, 1);
-
+        STOW(10),
+        FOO(80);
 
         double armAngle;
-        double intakePercentage;
 
-        private AlgaePresets(double armAngle, double intakePercentage) {
+        private AlgaePresets(double armAngle) {
             this.armAngle = armAngle;
-            this.intakePercentage = intakePercentage;
         }
     }
 
+    public enum AlgaeIntakePresets {
+        INTAKING(1),
+        PURGE(-1),
+        STOP(0);
+
+        double outputPercentage;
+
+        AlgaeIntakePresets(double outputPercentage) {
+            this.outputPercentage = outputPercentage;
+        } 
+    }
+
     private AlgaePresets currentPreset = AlgaePresets.STOW;
+    private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP;
 
     public Algae(AlgaeArm arm, AlgaeIntake intake) {
         this.arm = arm;
@@ -31,9 +41,14 @@ public Algae(AlgaeArm arm, AlgaeIntake intake) {
     public void setAlgaePreset(AlgaePresets preset) {
         if (preset != currentPreset) {
             arm.setGoalDegrees(preset.armAngle);
-            intake.setOutputPercentage(preset.intakePercentage);
-
             currentPreset = preset;
         }
     }
+
+    public void setAlgaeIntakePreset(AlgaeIntakePresets preset) {
+        if (preset != currentIntakePreset) {
+            intake.setOutputPercentage(preset.outputPercentage);
+            currentIntakePreset = preset;
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
index 01b73e7..f66b56f 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
@@ -5,12 +5,15 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
 public class AlgaeIntake extends SubsystemBase {
     private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless);
 
+    private final DigitalInput intakeBeambreak = new DigitalInput(Algae.Intake.BEAMBREAK_PORT);
+
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
         Algae.Intake.POSITIVE_RATE_LIMIT,
         Algae.Intake.NEGATIVE_RATE_LIMIT, 
@@ -21,9 +24,14 @@ public class AlgaeIntake extends SubsystemBase {
 
     @Override
     public void periodic() {
-        double output = intakeProfile.calculate(outputPercentage);
+        // if no algae
+        if (intakeBeambreak.get()) {
+            double output = intakeProfile.calculate(outputPercentage);
+            intakeMotor.set(output);
+        } else {
+            intakeMotor.set(0.05);
+        }
 
-        intakeMotor.set(output);
     }
 
     public void setOutputPercentage(double outputPercentage) {

From 89d1472342fcb6eedc81b2d08ce2cc7385fbb6cf Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 29 Jan 2025 20:59:49 -0600
Subject: [PATCH 19/71] updated coralintake logic

---
 .../subsystems/CoralSubsystem/Coral.java      | 25 ++++++++++++++++---
 .../CoralSubsystem/CoralIntake.java           | 13 ++++++++--
 2 files changed, 33 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
index 8708d78..e45b83a 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -36,13 +36,25 @@ public enum MirrorPresets {
         PORT(true);
 
         boolean isMirrored;
-        MirrorPresets(boolean isMirrored) {
+        private MirrorPresets(boolean isMirrored) {
             this.isMirrored = isMirrored;
         }
     } 
 
+    public enum CoralIntakePresets {
+        INTAKING(1),
+        PURGE(-1),
+        STOP(0);
+
+        double intakePercentage;
+        private CoralIntakePresets(double intakePercentage) {
+            this.intakePercentage = intakePercentage;
+        }
+    }
+
     private CoralPresets currentPreset = CoralPresets.STOW;
-    MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
+    private MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
+    private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP;
 
     public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
         this.arm = arm;
@@ -72,7 +84,14 @@ public void mirrorArm() {
         }
     }
 
-    public void mirorArm(MirrorPresets preset) {
+    public void mirrorArm(MirrorPresets preset) {
         mirrorSetting = preset;
     }
+
+    public void setCoralIntakePreset(CoralIntakePresets preset) {
+        if (preset != currentIntakePreset) {
+            intake.setOutputPercentage(preset.intakePercentage);
+            currentIntakePreset = preset;
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
index ceb8f84..64f8d40 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
@@ -1,8 +1,10 @@
 package frc.robot.subsystems.CoralSubsystem;
 
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -18,6 +20,7 @@
 public class CoralIntake extends SubsystemBase {
     private final TalonFX intakeMotor = new TalonFX(Coral.Intake.MOTOR_PORT);
 
+    private final StatusSignal<ReverseLimitValue> intakeBeambreak = intakeMotor.getReverseLimit();
 
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
         Coral.Intake.POSITIVE_RATE_LIMIT,
@@ -33,9 +36,15 @@ public CoralIntake() {
 
     @Override
     public void periodic() {
-        double output = intakeProfile.calculate(outputPercentage);
+        // if no coral 
+        if (intakeBeambreak.getValue().value == 1) {
+            double output = intakeProfile.calculate(outputPercentage);
+            intakeMotor.set(output);
+        } else {
+            // hold
+            intakeMotor.set(Math.min(0.05, outputPercentage));
+        }
 
-        intakeMotor.set(output);
     }
 
     public void setOutputPercentage(double outputPercentage) {

From 6f7249fdd4c93a443ec5434e2ef4b9b41065d999 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 30 Jan 2025 19:01:27 -0600
Subject: [PATCH 20/71] custom coral arm stuff i guess

---
 .../subsystems/CoralSubsystem/Coral.java      | 47 +++++++++++++++++--
 1 file changed, 44 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
index e45b83a..e5f92e1 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -14,7 +14,9 @@ public enum CoralPresets {
         LEVEL_3(3.0, 20.0, 0.0, 0.0),
         LEVEL_4(4.0, 20.0, 0.0, 0.0),
         INTAKE(1.0, 110.0, 0.0, 0.0),
-        STOW(1.0, 0.0, 0.0, 0.0);
+        STOW(1.0, 0.0, 0.0, 0.0),
+
+        CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 
         double elevatorHeight;
         double pivotAngle;
@@ -44,7 +46,9 @@ private MirrorPresets(boolean isMirrored) {
     public enum CoralIntakePresets {
         INTAKING(1),
         PURGE(-1),
-        STOP(0);
+        STOP(0),
+
+        CUSTOM(Double.NaN);
 
         double intakePercentage;
         private CoralIntakePresets(double intakePercentage) {
@@ -63,7 +67,9 @@ public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
     }
 
     public void setCoralPreset(CoralPresets preset) {
-        if (preset != currentPreset) {
+        if (preset == CoralPresets.CUSTOM) {
+            // uhhh i don't now how to throw an exception and i don't feel like figuring it out
+        } else if (preset != currentPreset) {
             elevator.setGoalPosition(preset.elevatorHeight);
             arm.setPivotGoalDegrees(
                 preset.pivotAngle 
@@ -76,6 +82,35 @@ public void setCoralPreset(CoralPresets preset) {
         }
     }
 
+    public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
+        currentPreset = CoralPresets.CUSTOM;
+
+        elevator.setGoalPosition(elevatorHeight);
+        arm.setPivotGoalDegrees(pivotAngle);
+        arm.setRollGoalDegrees(rollAngle);
+        arm.setPitchGoalDegrees(pitchAngle);
+    }
+
+    public void setCustomElevatorPosition(double elevatorHeight) {
+        currentPreset = CoralPresets.CUSTOM;
+        elevator.setGoalPosition(elevatorHeight);
+    }
+
+    public void setCustomPivotPosition(double pivotAngle) {
+        currentPreset = CoralPresets.CUSTOM;
+        arm.setPivotGoalDegrees(pivotAngle);
+    }
+
+    public void setCustomRollPosition(double rollAngle) {
+        currentPreset = CoralPresets.CUSTOM;
+        arm.setRollGoalDegrees(rollAngle);
+    }
+
+    public void setCustomPitchPosition(double pitchAngle) {
+        currentPreset = CoralPresets.CUSTOM;
+        arm.setPitchGoalDegrees(pitchAngle);
+    }
+
     public void mirrorArm() {
         if (mirrorSetting.isMirrored) {
             mirrorSetting = MirrorPresets.RIGHT;
@@ -94,4 +129,10 @@ public void setCoralIntakePreset(CoralIntakePresets preset) {
             currentIntakePreset = preset;
         }
     }
+
+    public void setCustomIntakePercent(double percentage) {
+        currentIntakePreset = CoralIntakePresets.CUSTOM;
+        intake.setOutputPercentage(percentage);
+    }
 }
+ 
\ No newline at end of file

From 84513d4da8fcc5b487c9ce1f7ce2f48c4ca6752c Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 30 Jan 2025 19:49:59 -0600
Subject: [PATCH 21/71] coral virtual representation

---
 .../subsystems/CoralSubsystem/Coral.java      | 22 +++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
index e5f92e1..099031b 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
@@ -1,5 +1,9 @@
 package frc.robot.subsystems.CoralSubsystem;
 
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Coral extends SubsystemBase {
@@ -8,6 +12,15 @@ public class Coral extends SubsystemBase {
     private final CoralIntake intake;
     private final CoralElevator elevator;
 
+    private final Mechanism2d coralMechanism = new Mechanism2d(2,3);
+    private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.5, 0);
+    private final MechanismLigament2d elevatorMechanism = rootMechanism.append(
+        new MechanismLigament2d("Elevator", 1, 0)
+    );
+    private final MechanismLigament2d pivotMechanism = elevatorMechanism.append(
+        new MechanismLigament2d("Coral", 1, 0)
+    );
+
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0),
         LEVEL_2(2.0, 20.0, 0.0, 0.0),
@@ -56,6 +69,15 @@ private CoralIntakePresets(double intakePercentage) {
         }
     }
 
+    @Override
+    public void periodic() {
+        // i have no idea what any of the getPositions output
+        elevatorMechanism.setLength(elevator.getPosition());
+        pivotMechanism.setAngle(arm.getPivotPositionDegrees());
+
+        SmartDashboard.putData("Coral Mechanism", coralMechanism);
+    }
+
     private CoralPresets currentPreset = CoralPresets.STOW;
     private MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
     private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP;

From dba16722918f58d70be717b191756acf4eaf9265 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Sat, 1 Feb 2025 10:29:10 -0600
Subject: [PATCH 22/71] Auto Works

---
 .../deploy/pathplanner/autos/Testing.auto     |  6 ++
 .../deploy/pathplanner/paths/New Path.path    |  6 +-
 .../deploy/pathplanner/paths/route 1.path     |  8 +--
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   |  4 +-
 .../frc/robot/subsystems/SwerveSubsystem.java | 56 +++++++++++++++----
 6 files changed, 61 insertions(+), 21 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Testing.auto b/src/main/deploy/pathplanner/autos/Testing.auto
index 268147b..f9fe73c 100644
--- a/src/main/deploy/pathplanner/autos/Testing.auto
+++ b/src/main/deploy/pathplanner/autos/Testing.auto
@@ -9,6 +9,12 @@
           "data": {
             "pathName": "New Path"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Testing"
+          }
         }
       ]
     }
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 63e13e5..8c7148e 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 5.358504098360656,
+        "x": 6.257581967213115,
         "y": 1.2618340163934432
       },
       "prevControl": {
-        "x": 5.9219262295081965,
+        "x": 6.821004098360655,
         "y": 1.2498463114754088
       },
       "nextControl": null,
@@ -41,7 +41,7 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 0.0
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
index b05e8de..6c871fb 100644
--- a/src/main/deploy/pathplanner/paths/route 1.path	
+++ b/src/main/deploy/pathplanner/paths/route 1.path	
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.03483606557377,
-        "y": 1.6454405737704905
+        "x": 5.658196721311475,
+        "y": 0.8782274590163933
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.3155737704918025,
-        "y": 2.472592213114752
+        "x": 5.082786885245901,
+        "y": 1.7653176229508185
       },
       "isLocked": false,
       "linkedName": null
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 32766a1..822bf3d 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -42,7 +42,7 @@ public static class RobotConstants {
     public static final double robotLengthMeters = Units.inchesToMeters(25.0);
     
       // TODO: ############## REPLACE PLACEHOLDERS ##############
-      public static final double TOTAL_MASS_KG = 60;
+      public static final double TOTAL_MASS_KG = 49; // 107lbs
       public static final double MOMENT_OF_INERTIA = 5;
   }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2f5f022..d658112 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.ControllerConstants;
@@ -104,13 +103,14 @@ public RobotContainer() {
         SmartDashboard.putData("Auto Chooser", autoChooser);
 
         swerveDriveSubsystem.setDefaultCommand(normalDrive);
+        NamedCommands.registerCommand("Testing", elevatorToTop);
     }
 
     // Command shootAction =
     // Command alignAction = ; // Self-deadlines
     // Command spoolAction =
     // Command intakeAction = ;
-
+    
     private ElevatorCommand elevatorToTop = new ElevatorCommand(elevator, ElevatorPresets.TOP, 0.0);
     private ElevatorCommand elevatorToMiddle = new ElevatorCommand(elevator, ElevatorPresets.MIDDLE, 0.0);
     private ElevatorCommand elevatorToStow = new ElevatorCommand(elevator, ElevatorPresets.STOW, 0.0);
diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index 45338f4..1db0243 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -2,28 +2,36 @@
 
 import java.util.List;
 
-import com.studica.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.path.PathPoint;
+import com.studica.frc.AHRS;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.kinematics.*;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -32,9 +40,13 @@
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.PathPlannerConstants;
+import frc.robot.Constants.PoseConstants;
+import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.LimelightHelpers;
 import frc.robot.Robot;
-import frc.robot.Constants.*;
 
 public class SwerveSubsystem extends SubsystemBase {
     SwerveModule frontLeft = new SwerveModule(SwerveModuleConstants.FL_STEER_ID, SwerveModuleConstants.FL_DRIVE_ID,
@@ -106,13 +118,19 @@ public SwerveSubsystem() {
         // zeroHeading()
 
         // --------- Path Planner Init ---------- \\
-
+                RobotConfig config = null;
+        try{
+        config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+        // Handle exception as needed
+        e.printStackTrace();
+        } 
         AutoBuilder.configure(
                 this::getPose, // Robot pose supplier
                 this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
                 this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
-                PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER,
+                //(speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+                /*PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, // todo -> check above method^^^
                 PathPlannerConstants.ROBOT_CONFIG,
                 () -> {
                     // Boolean supplier that controls when the path will be mirrored for the red
@@ -124,7 +142,23 @@ public SwerveSubsystem() {
                         return alliance.get() == DriverStation.Alliance.Red;
                     }
 
-                    return false;
+                    return false; */
+                (speeds, feedforwards) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+                    new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                    new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+            ),
+            config, // The robot configuration
+            () -> {
+              // Boolean supplier that controls when the path will be mirrored for the red alliance
+              // This will flip the path being followed to the red side of the field.
+              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+              var alliance = DriverStation.getAlliance();
+              if (alliance.isPresent()) {
+                return alliance.get() == DriverStation.Alliance.Red;
+              }
+              return false;
 
                 },
                 this // Reference to this subsystem to set requirements
@@ -153,13 +187,13 @@ public void periodic() {
         // TODO: Test
         // WARNING: REMOVE IF USING TAG FOLLOW!!!
         // updateVisionOdometry();
-
+/* 
         if (DriverStation.isTeleopEnabled()) {
             updateMegaTagOdometry();
         } else {
             updateVisionOdometry();
         }
-
+*/
         odometry.update(getRotation2d(), getModulePositions());
         // if (DriverStation.getAlliance().isPresent()) {
         // switch (DriverStation.getAlliance().get()) {

From 6c257198f5c07204bc4cacbee9e19f32722625a5 Mon Sep 17 00:00:00 2001
From: michael-m-2983 <116770284+michael-m-2983@users.noreply.github.com>
Date: Sat, 1 Feb 2025 10:33:07 -0600
Subject: [PATCH 23/71] Improve code readability

---
 .../frc/robot/commands/ElevatorCommand.java   | 71 ++++++++++++++-----
 .../robot/commands/ElevatorFollowCommand.java | 38 +++++++---
 .../{AlgaeSubsystem => algae}/Algae.java      | 53 +++++++-------
 .../{AlgaeSubsystem => algae}/AlgaeArm.java   | 17 +++--
 .../AlgaeIntake.java                          |  2 +-
 .../{CoralSubsystem => coral}/Coral.java      |  2 +-
 .../{CoralSubsystem => coral}/CoralArm.java   |  2 +-
 .../CoralElevator.java                        |  2 +-
 .../CoralIntake.java                          |  2 +-
 9 files changed, 126 insertions(+), 63 deletions(-)
 rename src/main/java/frc/robot/subsystems/{AlgaeSubsystem => algae}/Algae.java (85%)
 rename src/main/java/frc/robot/subsystems/{AlgaeSubsystem => algae}/AlgaeArm.java (73%)
 rename src/main/java/frc/robot/subsystems/{AlgaeSubsystem => algae}/AlgaeIntake.java (96%)
 rename src/main/java/frc/robot/subsystems/{CoralSubsystem => coral}/Coral.java (99%)
 rename src/main/java/frc/robot/subsystems/{CoralSubsystem => coral}/CoralArm.java (99%)
 rename src/main/java/frc/robot/subsystems/{CoralSubsystem => coral}/CoralElevator.java (98%)
 rename src/main/java/frc/robot/subsystems/{CoralSubsystem => coral}/CoralIntake.java (97%)

diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java
index 4018b4f..b739557 100644
--- a/src/main/java/frc/robot/commands/ElevatorCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorCommand.java
@@ -6,38 +6,48 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class ElevatorCommand extends Command {
-    private final ElevatorSubsystem elevatorSub;
-
-    public enum ElevatorPresets {
-        STOW(0.0),
-        MIDDLE(Constants.Elevator.PhysicalParameters.elevatorHeightMeters / 2),
-        TOP(Constants.Elevator.PhysicalParameters.elevatorHeightMeters);
-
-        private ElevatorPresets(double pos_meters) {
-            this.position_m = pos_meters;
-        }
+import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorHeightMeters;
 
-        private double position_m;
-    }
+/**
+ * This command tells the elevator to go to a preset.
+ */
+public class ElevatorCommand extends Command {
+    private final ElevatorSubsystem elevatorSub; // A reference to the elevator subsystem
 
-    private ElevatorPresets target = ElevatorPresets.STOW;
-    private double offset;
+    private final ElevatorPreset target; // The current selected target
+    private final double offset; // An offset for the current target
 
-    public ElevatorCommand(ElevatorSubsystem elevatorSub, ElevatorPresets targetPosition, double targetOffset) {
-        this.elevatorSub = elevatorSub;
+    /**
+     * Create a new instance of the command.
+     * 
+     * @param elevatorSystem The elevator subsystem
+     * @param targetPosition The target position - one of 'STOW', 'MIDDLE' or 'TOP'. See the {@link ElevatorCommand.ElevatorPresets} enum for more info.
+     * @param offset An offset for the target position
+     */
+    public ElevatorCommand(ElevatorSubsystem elevatorSystem, ElevatorPreset targetPosition, double targetOffset) {
+        this.elevatorSub = elevatorSystem;
         this.target = targetPosition;
         this.offset = targetOffset;
+
         addRequirements(elevatorSub);
     }
 
     @Override
     public void initialize() {
-        double tgt = target.position_m + offset;
-        elevatorSub.setGoal(MathUtil.clamp(tgt, 0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters));
+        double tgt = target.getPosition() + offset; // The actual target position, in meters
+        
+        // Set the goal of the elevator subsystem after clamping it to a reasonable value.
+        elevatorSub.setGoal(MathUtil.clamp(tgt, 0, elevatorHeightMeters));
+
+        // Put it on smartdashboard
         SmartDashboard.putString("Elevator Command", target.toString());
     }
 
+    /**
+     * Check if the elevator is in its goal position.
+     * 
+     * @return If the elevator is in the current 'goal' position
+     */
     @Override
     public boolean isFinished() {
         return elevatorSub.isInPosition();
@@ -48,4 +58,27 @@ public void end(boolean interrupted) {
         // NOTE: Don't disable (so it position-holds with feedforward)
         // elevatorSub.disable();
     }
+
+    /**
+     * Various presets for the elevator
+     */
+    public enum ElevatorPreset {
+        STOW(0.0),
+        MIDDLE(elevatorHeightMeters / 2),
+        TOP(elevatorHeightMeters);
+
+        // The position of the preset in meters
+        private final double position;
+
+        private ElevatorPresets(double pos_meters) {
+            this.position = pos_meters;
+        }
+
+        /**
+         * Returns the position of the elevator preset in meters
+         */
+        public double getPosition() {
+            return this.position;
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java b/src/main/java/frc/robot/commands/ElevatorFollowCommand.java
index 03a2aa1..ce109ca 100644
--- a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorFollowCommand.java
@@ -11,13 +11,31 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 
+import static frc.robot.Constants.Elevator.PID.MAX_ACCELERATION;
+import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorHeightMeters;
+
 public class ElevatorFollowCommand extends Command {
+    // A reference to the elevator subsystem
     private final ElevatorSubsystem elevatorSub;
+
+    private DoubleSupplier target;
+
+    // See the {@link execute()} method
     private double lastTarget = 0;
     private double lastTime = 0;
-    private DoubleSupplier target;
-    private SlewRateLimiter limiter = new SlewRateLimiter(Constants.Elevator.PID.MAX_ACCELERATION / 10.0);
+    
+    // A <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/filters/slew-rate-limiter.html">slew rate limiter</a> that limits the acceleration. Essentially, it caps the maximum rate-of-change
+    private SlewRateLimiter limiter = new SlewRateLimiter(MAX_ACCELERATION / 10.0);
 
+    /**
+     * This command makes the elevator follow a target
+     * 
+     * The target is specified via a DoubleSupplier.
+     * 
+     * Usage Example:
+     *  Command c = new ElevatorFollowCommand(elevatorSubsystem, () => Math.sin(++i % 50));
+     *  // Do something with the command
+     */
     public ElevatorFollowCommand(ElevatorSubsystem elevatorSub, DoubleSupplier target) {
         this.elevatorSub = elevatorSub;
         this.target = target;
@@ -36,15 +54,19 @@ public boolean isFinished() {
 
     @Override
     public void execute() {
-        double tgt = target.getAsDouble();
-        double dX = tgt - lastTarget;
-        double dt = Timer.getFPGATimestamp() - lastTime;
-        lastTime = Timer.getFPGATimestamp();
-        lastTarget = tgt;
+        double tgt = target.getAsDouble(); // The goal
+        double dX = tgt - lastTarget; // How far we need to go
+        double dt = Timer.getFPGATimestamp() - lastTime; // delta time
+
+        // Set the elevator goal to the target value
         elevatorSub.setGoal(new TrapezoidProfile.State(
                 MathUtil.clamp(limiter.calculate(tgt), 0,
-                        Constants.Elevator.PhysicalParameters.elevatorHeightMeters),
+                        elevatorHeightMeters),
                 0.0));
+
+        // Update values for the next dt and dX calculations
+        lastTime = Timer.getFPGATimestamp();
+        lastTarget = tgt;
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java b/src/main/java/frc/robot/subsystems/algae/Algae.java
similarity index 85%
rename from src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
rename to src/main/java/frc/robot/subsystems/algae/Algae.java
index ded0daf..a7cae9b 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/Algae.java
+++ b/src/main/java/frc/robot/subsystems/algae/Algae.java
@@ -1,38 +1,18 @@
-package frc.robot.subsystems.AlgaeSubsystem;
+package frc.robot.subsystems.algae;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Algae extends SubsystemBase{
+public class Algae extends SubsystemBase {
     
     private final AlgaeArm arm;
     private final AlgaeIntake intake;
 
-    public enum AlgaePresets {
-        STOW(10),
-        FOO(80);
-
-        double armAngle;
-
-        private AlgaePresets(double armAngle) {
-            this.armAngle = armAngle;
-        }
-    }
-
-    public enum AlgaeIntakePresets {
-        INTAKING(1),
-        PURGE(-1),
-        STOP(0);
-
-        double outputPercentage;
-
-        AlgaeIntakePresets(double outputPercentage) {
-            this.outputPercentage = outputPercentage;
-        } 
-    }
-
     private AlgaePresets currentPreset = AlgaePresets.STOW;
     private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP;
 
+    /**
+     * Creates a new Algae subsystem
+     */
     public Algae(AlgaeArm arm, AlgaeIntake intake) {
         this.arm = arm;
         this.intake = intake;
@@ -51,4 +31,27 @@ public void setAlgaeIntakePreset(AlgaeIntakePresets preset) {
             currentIntakePreset = preset;
         }
     }
+
+    public enum AlgaePresets {
+        STOW(10),
+        FOO(80);
+
+        public double armAngle;
+
+        private AlgaePresets(double armAngle) {
+            this.armAngle = armAngle;
+        }
+    }
+
+    public enum AlgaeIntakePresets {
+        INTAKING(1),
+        PURGE(-1),
+        STOP(0);
+
+        public double outputPercentage;
+
+        AlgaeIntakePresets(double outputPercentage) {
+            this.outputPercentage = outputPercentage;
+        } 
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
similarity index 73%
rename from src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
rename to src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index 2e62a23..018cf1d 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.AlgaeSubsystem;
+package frc.robot.subsystems.algae;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkFlex;
@@ -16,13 +16,18 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
-public class AlgaeArm extends SubsystemBase{
-    public final SparkFlex pivotMotor = new SparkFlex(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
+import static frc.robot.Constants.Algae.Pivot.MOTOR_PORT;
+import static frc.robot.Constants.Algae.Pivot.ENCODER_PORT;
+import static frc.robot.Constants.Algae.Pivot.PID;
+import static frc.robot.Constants.Algae.Pivot.FEEDFORWARD;
+
+public class AlgaeArm extends SubsystemBase {
+    public final SparkFlex pivotMotor = new SparkFlex(MOTOR_PORT, MotorType.kBrushless);
     public final SparkMaxConfig pivotConfig = new SparkMaxConfig();
-    public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
+    public final CANcoder pivotEncoder = new CANcoder(ENCODER_PORT);
 
-    public final ProfiledPIDController pivotPID = Algae.Pivot.PID;
-    public final ArmFeedforward pivotFeedforward = Algae.Pivot.FEEDFORWARD;
+    public final ProfiledPIDController pivotPID = PID;
+    public final ArmFeedforward pivotFeedforward = FEEDFORWARD;
 
     public AlgaeArm() {
         pivotConfig.idleMode(IdleMode.kBrake);
diff --git a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
similarity index 96%
rename from src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
rename to src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index f66b56f..6bb864d 100644
--- a/src/main/java/frc/robot/subsystems/AlgaeSubsystem/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.AlgaeSubsystem;
+package frc.robot.subsystems.algae;
 
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java b/src/main/java/frc/robot/subsystems/coral/Coral.java
similarity index 99%
rename from src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
rename to src/main/java/frc/robot/subsystems/coral/Coral.java
index 099031b..765b720 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/Coral.java
+++ b/src/main/java/frc/robot/subsystems/coral/Coral.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.CoralSubsystem;
+package frc.robot.subsystems.coral;
 
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
similarity index 99%
rename from src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
rename to src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 9738bbe..5f3cca4 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.CoralSubsystem;
+package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.AbsoluteEncoder;
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
similarity index 98%
rename from src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
rename to src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index be25ef0..f93468f 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.CoralSubsystem;
+package frc.robot.subsystems.coral;
 
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
diff --git a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
similarity index 97%
rename from src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
rename to src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 64f8d40..0754021 100644
--- a/src/main/java/frc/robot/subsystems/CoralSubsystem/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.CoralSubsystem;
+package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;

From 31e78203cd937e8da0d5add4971e15a7224271f0 Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Sat, 1 Feb 2025 10:59:01 -0600
Subject: [PATCH 24/71] readded vision

---
 src/main/java/frc/robot/subsystems/SwerveSubsystem.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index 1db0243..bd05ab5 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -187,13 +187,13 @@ public void periodic() {
         // TODO: Test
         // WARNING: REMOVE IF USING TAG FOLLOW!!!
         // updateVisionOdometry();
-/* 
+ 
         if (DriverStation.isTeleopEnabled()) {
             updateMegaTagOdometry();
         } else {
             updateVisionOdometry();
         }
-*/
+
         odometry.update(getRotation2d(), getModulePositions());
         // if (DriverStation.getAlliance().isPresent()) {
         // switch (DriverStation.getAlliance().get()) {

From 18b8283927454cf307d43f29724f2a05f6f071ca Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Sat, 1 Feb 2025 15:23:28 -0600
Subject: [PATCH 25/71] renamed subsystem files

i think this should work
---
 src/main/java/frc/robot/subsystems/algae/AlgaeArm.java      | 2 +-
 src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java   | 2 +-
 .../subsystems/algae/{Algae.java => AlgaeSubsystem.java}    | 6 +++---
 src/main/java/frc/robot/subsystems/coral/CoralArm.java      | 2 +-
 src/main/java/frc/robot/subsystems/coral/CoralElevator.java | 2 +-
 src/main/java/frc/robot/subsystems/coral/CoralIntake.java   | 2 +-
 .../subsystems/coral/{Coral.java => CoralSubsystem.java}    | 6 +++---
 7 files changed, 11 insertions(+), 11 deletions(-)
 rename src/main/java/frc/robot/subsystems/algae/{Algae.java => AlgaeSubsystem.java} (89%)
 rename src/main/java/frc/robot/subsystems/coral/{Coral.java => CoralSubsystem.java} (96%)

diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index 018cf1d..abde8e1 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.algae;
+package frc.robot.subsystems.Algae;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkFlex;
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index 6bb864d..753dc23 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.algae;
+package frc.robot.subsystems.Algae;
 
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
diff --git a/src/main/java/frc/robot/subsystems/algae/Algae.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
similarity index 89%
rename from src/main/java/frc/robot/subsystems/algae/Algae.java
rename to src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
index a7cae9b..d1af935 100644
--- a/src/main/java/frc/robot/subsystems/algae/Algae.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
@@ -1,8 +1,8 @@
-package frc.robot.subsystems.algae;
+package frc.robot.subsystems.Algae;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Algae extends SubsystemBase {
+public class AlgaeSubsystem extends SubsystemBase {
     
     private final AlgaeArm arm;
     private final AlgaeIntake intake;
@@ -13,7 +13,7 @@ public class Algae extends SubsystemBase {
     /**
      * Creates a new Algae subsystem
      */
-    public Algae(AlgaeArm arm, AlgaeIntake intake) {
+    public AlgaeSubsystem(AlgaeArm arm, AlgaeIntake intake) {
         this.arm = arm;
         this.intake = intake;
     }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 5f3cca4..2a66f7d 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.coral;
+package frc.robot.subsystems.Coral;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.AbsoluteEncoder;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index f93468f..88c0275 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.coral;
+package frc.robot.subsystems.Coral;
 
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 0754021..e69c9bc 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.coral;
+package frc.robot.subsystems.Coral;
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
diff --git a/src/main/java/frc/robot/subsystems/coral/Coral.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
similarity index 96%
rename from src/main/java/frc/robot/subsystems/coral/Coral.java
rename to src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 765b720..db7fc47 100644
--- a/src/main/java/frc/robot/subsystems/coral/Coral.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.coral;
+package frc.robot.subsystems.Coral;
 
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Coral extends SubsystemBase {
+public class CoralSubsystem extends SubsystemBase {
 
     private final CoralArm arm;
     private final CoralIntake intake;
@@ -82,7 +82,7 @@ public void periodic() {
     private MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
     private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP;
 
-    public Coral(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
+    public CoralSubsystem(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
         this.arm = arm;
         this.intake = intake;
         this.elevator = elevator;

From e4e169f2033ac7a962432dbeba027857c4639c29 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 2 Feb 2025 16:04:31 -0600
Subject: [PATCH 26/71] Update/Add theoretical manipulator constants. Fix
 building on windows.

---
 build.gradle                                  |   2 +-
 src/main/java/frc/robot/Constants.java        |  92 ++++++++----
 src/main/java/frc/robot/RobotContainer.java   |  63 ++++----
 .../frc/robot/commands/ElevatorCommand.java   | 138 +++++++++---------
 .../robot/commands/ElevatorFollowCommand.java |  77 ----------
 .../frc/robot/subsystems/algae/AlgaeArm.java  |   2 +-
 .../robot/subsystems/algae/AlgaeIntake.java   |   2 +-
 .../subsystems/algae/AlgaeSubsystem.java      |   2 +-
 .../frc/robot/subsystems/coral/CoralArm.java  |   2 +-
 .../robot/subsystems/coral/CoralElevator.java |   4 +-
 .../robot/subsystems/coral/CoralIntake.java   |   2 +-
 .../subsystems/coral/CoralSubsystem.java      |   2 +-
 12 files changed, 172 insertions(+), 216 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/ElevatorFollowCommand.java

diff --git a/build.gradle b/build.gradle
index 9d8ae14..c6fea4f 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
 }
 
 java {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 56a5f62..7484fa1 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -175,8 +175,9 @@ public static class Pivot {
       public static final int ENCODER_PORT = 28;
 
       public static final double MAXIMUM_ANGLE = 80;
-      public static final double FRAME_BORDER_ANGLE = 20;
+      public static final double FRAME_BORDER_ANGLE = 30;
 
+      // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
         0.0,
@@ -184,12 +185,20 @@ public static class Pivot {
         null
       );
 
+      // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
-        1,
-        1,
-        1,
-        1
+        0.0,
+        1.04,// V
+        1.62,// V*s/rad
+        0.05// V*s^2/rad
       );
+      public static class PhysicalConstants {
+        public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
+        public static final double NET_REDUCTION = 96.0;
+        public static final double MASS_KG = 4.8;
+        public static final double ARM_LEN_M = 0.51;
+        public static final double MOI = 0.2875548495; // Kg*m^2
+      }
     }
 
     public static class Roll {
@@ -200,11 +209,19 @@ public static class Roll {
         0.0,
         null
       );
+      public static final double MAXIMUM_ANGLE = 90;
+      public static class PhysicalConstants {
+        public static DCMotor MOTOR = DCMotor.getNeo550(1);
+        public static final double NET_REDUCTION = 45.0;
+        public static final double MASS_KG = 2.85; // Includes a coral
+        public static final double ARM_LEN_M = 0.083;
+        public static final double MOI = 0.0403605447; // Kg*m^2
+      }
     }
     public static class Pitch {
       public static final int MOTOR_PORT = 16;
 
-      public static final double MAXIMUM_ANGLE = 45;
+      public static final double MAXIMUM_ANGLE = 115;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
         1,
@@ -212,6 +229,14 @@ public static class Pitch {
         0.0,
         null
       );
+
+      public static class PhysicalConstants {
+        public static DCMotor MOTOR = DCMotor.getNeo550(1);
+        public static final double NET_REDUCTION = 92.85714286; // Yeah this is cursed
+        public static final double MASS_KG = 2.16; // Includes a coral
+        public static final double ARM_LEN_M = 0.101;
+        public static final double MOI = 0.0200055915; // Kg*m^2
+      }
     }
 
     public static class Intake {
@@ -219,6 +244,9 @@ public static class Intake {
 
       public static final double POSITIVE_RATE_LIMIT = 5.0;
       public static final double NEGATIVE_RATE_LIMIT = 5.0;
+
+      public static final double IN_OUT_CURRENT_LIMIT = 40.0; // Stator limit
+      public static final double HOLD_CURRENT_LIMIT = 5.0; // Stator, TODO: Test this!
     }
   }
   
@@ -242,11 +270,19 @@ public static final class Pivot {
 
       public static final double RETRACTED_LIMIT_DEGREES = 10.0;
       public static final double EXTENDED_LIMIT_DEGREES = 90.0;
+
+      public static class PhysicalConstants {
+        public static DCMotor MOTOR = DCMotor.getNeoVortex(1);
+        public static final double NET_REDUCTION = 42.66666667; // Yeah this is cursed
+        public static final double MASS_KG = 3.18;
+        public static final double ARM_LEN_M = 0.2349863728;
+        public static final double MOI = 0.1114866914; // Kg*m^2
+      }
     }
 
     public static final class Intake {
       public static final int MOTOR_PORT = 19;
-      public static final int BEAMBREAK_PORT = 0;
+      public static final int BEAMBREAK_PORT = 0; // NOTE: Beambreak will *probably* be a rockwell proximity sensor wired into the SPARK max
 
       public static final double POSITIVE_RATE_LIMIT = 5.0;
       public static final double NEGATIVE_RATE_LIMIT = -5.0;
@@ -264,26 +300,24 @@ public static final class Follower {
       public static final boolean INVERTED = false;
     }
 
-    public static Type bottomLimitMode = Type.kNormallyOpen;
-
-    public static double MOTOR_REVOLUTIONS_PER_METER = 39.44;
+    public static double MOTOR_REVOLUTIONS_PER_METER = 32.81;
 
+    // TODO: Tune! Use FWERB for now
     public static class PID {
-      public static double kP = 20.0; // 9.0;
+      public static double kP = 20.0;
       public static double kI = 0.0;
-      public static double kD = 0.5; // 4.0;
-      public static double MAX_VELOCITY = 2.8;
-      public static double MAX_ACCELERATION = 18.0;
+      public static double kD = 0.5;
+      public static double MAX_VELOCITY = 3.20;
+      // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if the elevator can make or exceed this
+      public static double MAX_ACCELERATION = 20.0;
     }
 
-    // TODO: For the first testing, set these all to zero for safety reasons
-    // Remind me to pad the top and bottom of the elevator with poodles to make sure
-    // we don't damage it.
+    // TODO: PAD THE ELEVATOR!!!!!!!
     public static class FeedforwardConstants {
       public static double Ks = 0.0;
-      public static double Kv = 4.0;
-      public static double Ka = 0.03;
-      public static double Kg = 0.1;
+      public static double Kv = 3.5;
+      public static double Ka = 0.08;
+      public static double Kg = 0.75; // TODO: Check this!!!
     }
 
     public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward(
@@ -294,14 +328,16 @@ public static class FeedforwardConstants {
     );
 
     public static class PhysicalParameters {
-      public static double gearReduction = 9.0 / 2.0;
-      public static double driveRadiusMeters = 0.0182;
-      public static double carriageMassKg = 1.5;
-      public static double elevatorHeightMeters = Units.inchesToMeters(50.0);
-      public static double elevatorBottomFromFloorMeters = Units.inchesToMeters(12.0);
-      public static double elevatorCarriageHeightMeters = Units.inchesToMeters(6.0);
-      public static double elevatorForwardsFromRobotCenterMeters = Units.inchesToMeters(25.0 / 2);
-      public static DCMotor simMotor = DCMotor.getNeoVortex(2);
+      public static double gearReduction = 5.0 / 2.0;
+      public static double driveRadiusMeters = 0.0121;
+      public static double carriageMassKg = 8.77; // NOTE: This includes the weight "reduction" due to CF spring counterbalance!
+      public static double elevatorTravelMeters = Units.inchesToMeters(59.5);
+      public static double elevatorBottomFromFloorMeters = Units.inchesToMeters(3.0); // Relative to bottom of stage 2
+      public static double elevatorCarriageHeightMeters = Units.inchesToMeters(8.0); // Bottom to top of stage 2
+      public static double coralArmPivotVerticalOffset = Units.inchesToMeters(6.0); // From bottom of stage 2 to coral arm pivot axis
+      public static double coralArmPivotHorizontalOffset = Units.inchesToMeters(7.5); // From bottom of stage 2 to coral arm pivot axis
+      public static double elevatorForwardsFromRobotCenterMeters = Units.inchesToMeters(1); // To mid-plane of elevator
+      public static DCMotor MOTOR = DCMotor.getNeoVortex(2);
     }
   }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0bbe4cc..1986ecd 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,10 +6,13 @@
 
 import frc.robot.Constants.*;
 import frc.robot.commands.*;
-import frc.robot.commands.ElevatorCommand.ElevatorPresets;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
+import frc.robot.subsystems.coral.CoralArm;
+import frc.robot.subsystems.coral.CoralElevator;
+import frc.robot.subsystems.coral.CoralIntake;
+import frc.robot.subsystems.coral.CoralSubsystem;
 
 import java.util.function.DoubleSupplier;
 
@@ -48,7 +51,10 @@ public class RobotContainer {
     private final UsbCamera intakeCam = CameraServer.startAutomaticCapture();
     private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID());
 
-    private final ElevatorSubsystem elevator = new ElevatorSubsystem();
+    private final CoralArm coralArm = new CoralArm();
+    private final CoralIntake coralIntake = new CoralIntake();
+    private final CoralElevator coralElevator = new CoralElevator();
+    private final CoralSubsystem coralSub = new CoralSubsystem(coralArm, coralIntake, coralElevator);
 
     /*
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -100,15 +106,6 @@ public RobotContainer() {
         swerveDriveSubsystem.setDefaultCommand(normalDrive);
     }
 
-    // Command shootAction =
-    // Command alignAction = ; // Self-deadlines
-    // Command spoolAction =
-    // Command intakeAction = ;
-
-    private ElevatorCommand elevatorToTop = new ElevatorCommand(elevator, ElevatorPresets.TOP, 0.0);
-    private ElevatorCommand elevatorToMiddle = new ElevatorCommand(elevator, ElevatorPresets.MIDDLE, 0.0);
-    private ElevatorCommand elevatorToStow = new ElevatorCommand(elevator, ElevatorPresets.STOW, 0.0);
-
     /**
      * Use this method to define your trigger->command mappings. Triggers can be
      * created via the
@@ -124,28 +121,28 @@ public RobotContainer() {
      * joysticks}.
      */
     private void configureBindings() {
-        operatorXbox.a()
-                .onTrue(elevatorToStow);
-        operatorXbox.x()
-                .onTrue(elevatorToMiddle);
-        operatorXbox.y()
-                .onTrue(elevatorToTop);
-
-        operatorXbox.b().whileTrue(new ElevatorFollowCommand(elevator, new DoubleSupplier() {
-            @Override
-            public double getAsDouble() {
-                return (operatorXbox.getLeftY() * -0.5 + 0.5)
-                        * Constants.Elevator.PhysicalParameters.elevatorHeightMeters;
-            }
-        }));
-
-        operatorXbox.povUp().debounce(0.02).onTrue(new InstantCommand(() -> {
-            elevator.setPosition(elevator.getGoalPosition() + 0.1);
-        }));
-
-        operatorXbox.povDown().debounce(0.02).onTrue(new InstantCommand(() -> {
-            elevator.setPosition(elevator.getGoalPosition() - 0.1);
-        }));
+        // operatorXbox.a()
+        //         .onTrue(elevatorToStow);
+        // operatorXbox.x()
+        //         .onTrue(elevatorToMiddle);
+        // operatorXbox.y()
+        //         .onTrue(elevatorToTop);
+
+        // operatorXbox.b().whileTrue(new ElevatorFollowCommand(elevator, new DoubleSupplier() {
+        //     @Override
+        //     public double getAsDouble() {
+        //         return (operatorXbox.getLeftY() * -0.5 + 0.5)
+        //                 * Constants.Elevator.PhysicalParameters.elevatorTravelMeters;
+        //     }
+        // }));
+
+        // operatorXbox.povUp().debounce(0.02).onTrue(new InstantCommand(() -> {
+        //     elevator.setPosition(elevator.getGoalPosition() + 0.1);
+        // }));
+
+        // operatorXbox.povDown().debounce(0.02).onTrue(new InstantCommand(() -> {
+        //     elevator.setPosition(elevator.getGoalPosition() - 0.1);
+        // }));
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java
index b739557..c933f2d 100644
--- a/src/main/java/frc/robot/commands/ElevatorCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorCommand.java
@@ -1,84 +1,84 @@
-package frc.robot.commands;
+// package frc.robot.commands;
 
-import frc.robot.Constants;
-import frc.robot.subsystems.ElevatorSubsystem;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
+// import frc.robot.Constants;
+// import frc.robot.subsystems.ElevatorSubsystem;
+// import edu.wpi.first.math.MathUtil;
+// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+// import edu.wpi.first.wpilibj2.command.Command;
 
-import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorHeightMeters;
+// import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorTravelMeters;
 
-/**
- * This command tells the elevator to go to a preset.
- */
-public class ElevatorCommand extends Command {
-    private final ElevatorSubsystem elevatorSub; // A reference to the elevator subsystem
+// /**
+//  * This command tells the elevator to go to a preset.
+//  */
+// public class ElevatorCommand extends Command {
+//     private final ElevatorSubsystem elevatorSub; // A reference to the elevator subsystem
 
-    private final ElevatorPreset target; // The current selected target
-    private final double offset; // An offset for the current target
+//     private final ElevatorPreset target; // The current selected target
+//     private final double offset; // An offset for the current target
 
-    /**
-     * Create a new instance of the command.
-     * 
-     * @param elevatorSystem The elevator subsystem
-     * @param targetPosition The target position - one of 'STOW', 'MIDDLE' or 'TOP'. See the {@link ElevatorCommand.ElevatorPresets} enum for more info.
-     * @param offset An offset for the target position
-     */
-    public ElevatorCommand(ElevatorSubsystem elevatorSystem, ElevatorPreset targetPosition, double targetOffset) {
-        this.elevatorSub = elevatorSystem;
-        this.target = targetPosition;
-        this.offset = targetOffset;
+//     /**
+//      * Create a new instance of the command.
+//      * 
+//      * @param elevatorSystem The elevator subsystem
+//      * @param targetPosition The target position - one of 'STOW', 'MIDDLE' or 'TOP'. See the {@link ElevatorCommand.ElevatorPresets} enum for more info.
+//      * @param offset An offset for the target position
+//      */
+//     public ElevatorCommand(ElevatorSubsystem elevatorSystem, ElevatorPreset targetPosition, double targetOffset) {
+//         this.elevatorSub = elevatorSystem;
+//         this.target = targetPosition;
+//         this.offset = targetOffset;
 
-        addRequirements(elevatorSub);
-    }
+//         addRequirements(elevatorSub);
+//     }
 
-    @Override
-    public void initialize() {
-        double tgt = target.getPosition() + offset; // The actual target position, in meters
+//     @Override
+//     public void initialize() {
+//         double tgt = target.getPosition() + offset; // The actual target position, in meters
         
-        // Set the goal of the elevator subsystem after clamping it to a reasonable value.
-        elevatorSub.setGoal(MathUtil.clamp(tgt, 0, elevatorHeightMeters));
+//         // Set the goal of the elevator subsystem after clamping it to a reasonable value.
+//         elevatorSub.setGoal(MathUtil.clamp(tgt, 0, elevatorTravelMeters));
 
-        // Put it on smartdashboard
-        SmartDashboard.putString("Elevator Command", target.toString());
-    }
+//         // Put it on smartdashboard
+//         SmartDashboard.putString("Elevator Command", target.toString());
+//     }
 
-    /**
-     * Check if the elevator is in its goal position.
-     * 
-     * @return If the elevator is in the current 'goal' position
-     */
-    @Override
-    public boolean isFinished() {
-        return elevatorSub.isInPosition();
-    }
+//     /**
+//      * Check if the elevator is in its goal position.
+//      * 
+//      * @return If the elevator is in the current 'goal' position
+//      */
+//     @Override
+//     public boolean isFinished() {
+//         return elevatorSub.isInPosition();
+//     }
 
-    @Override
-    public void end(boolean interrupted) {
-        // NOTE: Don't disable (so it position-holds with feedforward)
-        // elevatorSub.disable();
-    }
+//     @Override
+//     public void end(boolean interrupted) {
+//         // NOTE: Don't disable (so it position-holds with feedforward)
+//         // elevatorSub.disable();
+//     }
 
-    /**
-     * Various presets for the elevator
-     */
-    public enum ElevatorPreset {
-        STOW(0.0),
-        MIDDLE(elevatorHeightMeters / 2),
-        TOP(elevatorHeightMeters);
+//     /**
+//      * Various presets for the elevator
+//      */
+//     public enum ElevatorPreset {
+//         STOW(0.0),
+//         MIDDLE(elevatorTravelMeters / 2),
+//         TOP(elevatorTravelMeters);
 
-        // The position of the preset in meters
-        private final double position;
+//         // The position of the preset in meters
+//         private final double position;
 
-        private ElevatorPresets(double pos_meters) {
-            this.position = pos_meters;
-        }
+//         private ElevatorPreset(double pos_meters) {
+//             this.position = pos_meters;
+//         }
 
-        /**
-         * Returns the position of the elevator preset in meters
-         */
-        public double getPosition() {
-            return this.position;
-        }
-    }
-}
+//         /**
+//          * Returns the position of the elevator preset in meters
+//          */
+//         public double getPosition() {
+//             return this.position;
+//         }
+//     }
+// }
diff --git a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java b/src/main/java/frc/robot/commands/ElevatorFollowCommand.java
deleted file mode 100644
index ce109ca..0000000
--- a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java
+++ /dev/null
@@ -1,77 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.Constants;
-import frc.robot.subsystems.ElevatorSubsystem;
-
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import static frc.robot.Constants.Elevator.PID.MAX_ACCELERATION;
-import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorHeightMeters;
-
-public class ElevatorFollowCommand extends Command {
-    // A reference to the elevator subsystem
-    private final ElevatorSubsystem elevatorSub;
-
-    private DoubleSupplier target;
-
-    // See the {@link execute()} method
-    private double lastTarget = 0;
-    private double lastTime = 0;
-    
-    // A <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/filters/slew-rate-limiter.html">slew rate limiter</a> that limits the acceleration. Essentially, it caps the maximum rate-of-change
-    private SlewRateLimiter limiter = new SlewRateLimiter(MAX_ACCELERATION / 10.0);
-
-    /**
-     * This command makes the elevator follow a target
-     * 
-     * The target is specified via a DoubleSupplier.
-     * 
-     * Usage Example:
-     *  Command c = new ElevatorFollowCommand(elevatorSubsystem, () => Math.sin(++i % 50));
-     *  // Do something with the command
-     */
-    public ElevatorFollowCommand(ElevatorSubsystem elevatorSub, DoubleSupplier target) {
-        this.elevatorSub = elevatorSub;
-        this.target = target;
-        addRequirements(elevatorSub);
-    }
-
-    @Override
-    public void initialize() {
-        elevatorSub.enable();
-    }
-
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    @Override
-    public void execute() {
-        double tgt = target.getAsDouble(); // The goal
-        double dX = tgt - lastTarget; // How far we need to go
-        double dt = Timer.getFPGATimestamp() - lastTime; // delta time
-
-        // Set the elevator goal to the target value
-        elevatorSub.setGoal(new TrapezoidProfile.State(
-                MathUtil.clamp(limiter.calculate(tgt), 0,
-                        elevatorHeightMeters),
-                0.0));
-
-        // Update values for the next dt and dX calculations
-        lastTime = Timer.getFPGATimestamp();
-        lastTarget = tgt;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        // NOTE: Don't disable (so it position-holds with feedforward)
-        // elevatorSub.disable();
-    }
-}
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index abde8e1..018cf1d 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Algae;
+package frc.robot.subsystems.algae;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkFlex;
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index 753dc23..6bb864d 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Algae;
+package frc.robot.subsystems.algae;
 
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
index d1af935..06e7cef 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Algae;
+package frc.robot.subsystems.algae;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 2a66f7d..5f3cca4 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Coral;
+package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.AbsoluteEncoder;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index 88c0275..d01a948 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Coral;
+package frc.robot.subsystems.coral;
 
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -98,7 +98,7 @@ public void periodic() {
     }
 
     public void setGoalPosition(double position) {
-        elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.elevatorHeightMeters));
+        elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.elevatorTravelMeters));
     }
     
     public double getGoalPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index e69c9bc..0754021 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Coral;
+package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index db7fc47..79122f4 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.Coral;
+package frc.robot.subsystems.coral;
 
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;

From de743a16f34d162fd5a3f2cc125baa973eb4e8cc Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 2 Feb 2025 17:32:43 -0600
Subject: [PATCH 27/71] Add theoretical presets

---
 .../subsystems/coral/CoralSubsystem.java      | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 79122f4..31b884b 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -22,19 +22,19 @@ public class CoralSubsystem extends SubsystemBase {
     );
 
     public enum CoralPresets {
-        LEVEL_1(1.0, 20.0, 0.0, 0.0),
-        LEVEL_2(2.0, 20.0, 0.0, 0.0),
-        LEVEL_3(3.0, 20.0, 0.0, 0.0),
-        LEVEL_4(4.0, 20.0, 0.0, 0.0),
-        INTAKE(1.0, 110.0, 0.0, 0.0),
-        STOW(1.0, 0.0, 0.0, 0.0),
+        LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
+        LEVEL_2(0.237, 19.032, -90, 105.968),
+        LEVEL_3(0.640, 19.032, -90, 105.968),
+        LEVEL_4(1.342, 23.238, -90, 111.762),
+        INTAKE(0.0, 9.559, -90, 50.44),
+        STOW(0.0, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 
-        double elevatorHeight;
-        double pivotAngle;
-        double rollAngle;
-        double pitchAngle;
+        double elevatorHeight; // Elevator height (relative to bottom of elevator/fully retracted)
+        double pivotAngle; // Looking at the robot from the FRONT (algae intake side), positive to the right, and negative to the left (positive=CW)
+        double rollAngle; // Wrist 1 angle, degrees from pointing at the bumpers on the CORAL ARM side of the robot. positive=CCW
+        double pitchAngle; // Wrist 2 angle, degrees from pointing straight up (max: 115deg)
 
         private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
             this.elevatorHeight = elevatorHeight;

From 233a47686cd69d4abf459e79369099d838dc6a17 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 3 Feb 2025 19:26:24 -0600
Subject: [PATCH 28/71] add coral wrist roll restrictions

---
 src/main/java/frc/robot/subsystems/coral/CoralArm.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 5f3cca4..bcf9b59 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -106,7 +106,7 @@ public void setPivotGoalDegrees(double goal) {
 
     // similarly, this ranges from -180 to 180
     public void setRollGoalDegrees(double goal) {
-        rollGoal = goal;
+        rollGoal = MathUtil.clamp(goal, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
         // if outside of frame border
         if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
             // start moving to goal
@@ -115,7 +115,7 @@ public void setRollGoalDegrees(double goal) {
     }
 
     public void setPitchGoalDegrees(double goal) {
-        pitchGoal = MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE, -1 * Coral.Pitch.MAXIMUM_ANGLE);
+        pitchGoal = MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
         // if outside the frame border
         if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
             // start moving to goal

From ee5a1e336e8aaa4396edc476d3fdbff2219442c3 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 3 Feb 2025 19:36:58 -0600
Subject: [PATCH 29/71] update coral subsystem setter getters

---
 .../subsystems/coral/CoralSubsystem.java      | 20 +++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 31b884b..3ddda1b 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -104,6 +104,18 @@ public void setCoralPreset(CoralPresets preset) {
         }
     }
 
+    public double getPivotGoalDegrees() {
+        return arm.getPivotGoalDegrees();
+    }
+
+    public double getRollGoalDegrees() {
+        return arm.getRollGoalDegrees();
+    }
+
+    public double getPitchGoalDegrees() {
+        return arm.getPitchGoalDegrees();
+    }
+
     public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
         currentPreset = CoralPresets.CUSTOM;
 
@@ -113,22 +125,22 @@ public void setCustomPosition(double elevatorHeight, double pivotAngle, double r
         arm.setPitchGoalDegrees(pitchAngle);
     }
 
-    public void setCustomElevatorPosition(double elevatorHeight) {
+    public void setCustomElevatorMeters(double elevatorHeight) {
         currentPreset = CoralPresets.CUSTOM;
         elevator.setGoalPosition(elevatorHeight);
     }
 
-    public void setCustomPivotPosition(double pivotAngle) {
+    public void setCustomPivotDegrees(double pivotAngle) {
         currentPreset = CoralPresets.CUSTOM;
         arm.setPivotGoalDegrees(pivotAngle);
     }
 
-    public void setCustomRollPosition(double rollAngle) {
+    public void setCustomRollDegrees(double rollAngle) {
         currentPreset = CoralPresets.CUSTOM;
         arm.setRollGoalDegrees(rollAngle);
     }
 
-    public void setCustomPitchPosition(double pitchAngle) {
+    public void setCustomPitchDegrees(double pitchAngle) {
         currentPreset = CoralPresets.CUSTOM;
         arm.setPitchGoalDegrees(pitchAngle);
     }

From 8b01a79c2e660ff436a5dbea4ef3ddcd8b234f86 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 3 Feb 2025 20:49:47 -0600
Subject: [PATCH 30/71] put everything in robotcontainers and added bindings

some reorganization too
---
 src/main/java/frc/robot/RobotContainer.java   | 156 +++++++++++-------
 .../commands/ActivateCoralIntakeCommand.java  |  41 +++++
 .../commands/PurgeCoralIntakeCommand.java     |  23 +++
 .../frc/robot/commands/ShootAlgaeCommand.java |  29 ++++
 .../robot/commands/WristFollowCommand.java    |  24 +++
 .../robot/subsystems/algae/AlgaeIntake.java   |   7 +-
 .../subsystems/algae/AlgaeSubsystem.java      |  55 +++---
 .../robot/subsystems/coral/CoralIntake.java   |   4 +
 .../subsystems/coral/CoralSubsystem.java      |  19 +--
 9 files changed, 258 insertions(+), 100 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java
 create mode 100644 src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java
 create mode 100644 src/main/java/frc/robot/commands/ShootAlgaeCommand.java
 create mode 100644 src/main/java/frc/robot/commands/WristFollowCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1986ecd..9254d15 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -9,11 +9,18 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
+import frc.robot.subsystems.algae.AlgaeArm;
+import frc.robot.subsystems.algae.AlgaeIntake;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets;
 import frc.robot.subsystems.coral.CoralArm;
 import frc.robot.subsystems.coral.CoralElevator;
 import frc.robot.subsystems.coral.CoralIntake;
 import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
@@ -51,10 +58,14 @@ public class RobotContainer {
     private final UsbCamera intakeCam = CameraServer.startAutomaticCapture();
     private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID());
 
-    private final CoralArm coralArm = new CoralArm();
-    private final CoralIntake coralIntake = new CoralIntake();
-    private final CoralElevator coralElevator = new CoralElevator();
-    private final CoralSubsystem coralSub = new CoralSubsystem(coralArm, coralIntake, coralElevator);
+    private final CoralSubsystem coralSubsystem = new CoralSubsystem();
+    private CoralPresets currentCoralPreset = CoralPresets.STOW;
+
+    private final AlgaeSubsystem algaeSubsystem = new AlgaeSubsystem();
+
+    private final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
+
+
 
     /*
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -66,40 +77,6 @@ public RobotContainer() {
         DataLogManager.logNetworkTables(true);
         DataLogManager.start();
 
-        // NamedCommands.registerCommand("NoNote", new (
-        // new WaitForCommand(1.0),
-        // new WaitUntilCommand(new BooleanSupplier() {
-        // @Override
-        // public boolean getAsBoolean() {
-        // return !intake.containsNote().getAsBoolean();
-        // }
-        // })
-        // ));
-
-        /*
-         * NamedCommands.registerCommand("Shoot Close", new SequentialCommandGroup(
-         * new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}),
-         * new WaitCommand(2),
-         * new AlignNoteCommand(intake, shooter),
-         * new PrepNoteCommand(shooter, intake),
-         * new PrepShooterCommand(intake, shooter, 0.8),
-         * new ShootCommand(shooter, intake)
-         * ));
-         * NamedCommands.registerCommand("Pickup", new SequentialCommandGroup(
-         * new InstantCommand(() -> {arm.setArmPreset(Presets.INTAKE);}),
-         * new IntakeCommand(intake)));
-         */
-        // Test Auto Week 0
-        // return new SequentialCommandGroup(
-        // new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}),
-        // new WaitCommand(2),
-        // new AlignNoteCommand(intake, shooter),
-        // new PrepNoteCommand(shooter, intake),
-        // new PrepShooterCommand(intake, shooter, 0.8),
-        // new InstantCommand(() -> System.out.println("HELLLLLOOO")),
-        // new ShootCommand(shooter, intake)
-        // );
-
         autoChooser = AutoBuilder.buildAutoChooser();
         SmartDashboard.putData("Auto Chooser", autoChooser);
 
@@ -121,28 +98,87 @@ public RobotContainer() {
      * joysticks}.
      */
     private void configureBindings() {
-        // operatorXbox.a()
-        //         .onTrue(elevatorToStow);
-        // operatorXbox.x()
-        //         .onTrue(elevatorToMiddle);
-        // operatorXbox.y()
-        //         .onTrue(elevatorToTop);
-
-        // operatorXbox.b().whileTrue(new ElevatorFollowCommand(elevator, new DoubleSupplier() {
-        //     @Override
-        //     public double getAsDouble() {
-        //         return (operatorXbox.getLeftY() * -0.5 + 0.5)
-        //                 * Constants.Elevator.PhysicalParameters.elevatorTravelMeters;
-        //     }
-        // }));
-
-        // operatorXbox.povUp().debounce(0.02).onTrue(new InstantCommand(() -> {
-        //     elevator.setPosition(elevator.getGoalPosition() + 0.1);
-        // }));
-
-        // operatorXbox.povDown().debounce(0.02).onTrue(new InstantCommand(() -> {
-        //     elevator.setPosition(elevator.getGoalPosition() - 0.1);
-        // }));
+        /*
+         * operator
+        */
+        // low algae
+        operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
+            algaeSubsystem.setAlgaePreset(AlgaePresets.FLOOR);
+        }));
+        // high algae
+        operatorXbox.rightBumper().onTrue(new InstantCommand(() -> {
+            algaeSubsystem.setAlgaePreset(AlgaePresets.STOW);
+        }));
+
+        // L1 
+        operatorXbox.a().onTrue(new InstantCommand(() -> {
+            currentCoralPreset = CoralPresets.LEVEL_1;
+        }));
+        // L2
+        operatorXbox.x().onTrue(new InstantCommand(() -> {
+            currentCoralPreset = CoralPresets.LEVEL_2;
+        }));
+        // L3
+        operatorXbox.y().onTrue(new InstantCommand(() -> {
+            currentCoralPreset = CoralPresets.LEVEL_3;
+        }));
+        // L4
+        operatorXbox.b().onTrue(new InstantCommand(() -> {
+            currentCoralPreset = CoralPresets.LEVEL_4;
+        }));
+        // go to preset position
+        operatorXbox.rightTrigger().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPreset(currentCoralPreset);
+        }));
+        // wrist adjustment
+        operatorXbox.rightStick().and(new BooleanSupplier() {
+            // deadzone
+            @Override
+            public boolean getAsBoolean() {
+                return Math.sqrt(Math.pow(operatorXbox.getRightX(),2) + Math.pow(operatorXbox.getRightY(),2)) > 0.25; 
+            }
+        }).whileTrue(new WristFollowCommand(coralSubsystem, operatorXbox));
+        // score / intake coral
+        operatorXbox.rightBumper().whileTrue(new ActivateCoralIntakeCommand(coralSubsystem));
+        // purge coral
+        operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
+        /*
+         * driver
+        */
+        // stop the climber
+        driverXbox.x().onTrue(new InstantCommand(() -> {
+            climberSubsystem.setOutput(0);
+        }));
+        // move the climber
+        driverXbox.y().and(new BooleanSupplier() {
+            private boolean deployed = true;
+            @Override
+            public boolean getAsBoolean() {
+                deployed = !(deployed);
+                return deployed;
+            }
+        }).onFalse(new InstantCommand(() -> {
+            climberSubsystem.setOutput(1);
+        })).onTrue(new InstantCommand(() -> {
+            climberSubsystem.setOutput(-1);
+        }));
+        // something something maintainence
+        driverXbox.button(6).onTrue(new SequentialCommandGroup(null));
+        // set field orientation
+        driverXbox.button(7).onTrue(new InstantCommand(() -> {
+            swerveDriveSubsystem.setHeading(0);
+        }));
+
+        /* 
+         * coop
+        */
+        // algae floor / shoot
+        driverXbox.leftBumper().and(new BooleanSupplier() {
+            @Override
+            public boolean getAsBoolean() {
+                return false;
+            }
+        }).onTrue(new In);
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java b/src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java
new file mode 100644
index 0000000..afd06d2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java
@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
+
+public class ActivateCoralIntakeCommand extends Command {
+
+    private final CoralSubsystem subsystem;
+    private final boolean scoring;
+   
+    public ActivateCoralIntakeCommand(CoralSubsystem subsystem) {
+        this.subsystem = subsystem;
+        this.scoring = subsystem.isHolding();
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+        // score if holding, intake if not
+        if (scoring) {
+            subsystem.setCoralIntakePreset(CoralIntakePresets.SCORE);
+        } else {
+            subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE);
+        }   
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (scoring) {
+            subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+        } else {
+            // if intaking but failed
+            if (!subsystem.isHolding()) {
+                subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+            }
+            // else continue to hold the piece
+        }
+    }
+
+}
diff --git a/src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java b/src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java
new file mode 100644
index 0000000..3571a89
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java
@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
+
+public class PurgeCoralIntakeCommand extends Command {
+    private final CoralSubsystem subsystem;
+
+    public PurgeCoralIntakeCommand(CoralSubsystem subsystem) {
+        this.subsystem = subsystem;
+    }
+
+    @Override
+    public void initialize() {
+        subsystem.setCoralIntakePreset(CoralIntakePresets.PURGE);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+    }
+}
diff --git a/src/main/java/frc/robot/commands/ShootAlgaeCommand.java b/src/main/java/frc/robot/commands/ShootAlgaeCommand.java
new file mode 100644
index 0000000..3f14f87
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ShootAlgaeCommand.java
@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets;
+
+public class ShootAlgaeCommand extends Command {
+    private final AlgaeSubsystem subsystem;
+
+    public ShootAlgaeCommand(AlgaeSubsystem subsystem) {
+        this.subsystem = subsystem;
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+        subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.SHOOT);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !subsystem.isHolding();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/WristFollowCommand.java b/src/main/java/frc/robot/commands/WristFollowCommand.java
new file mode 100644
index 0000000..219fe7d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/WristFollowCommand.java
@@ -0,0 +1,24 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.coral.CoralSubsystem;
+
+public class WristFollowCommand extends Command {
+   
+    private final CoralSubsystem subsystem;
+    private final CommandXboxController operator;
+    
+    public WristFollowCommand(CoralSubsystem subsystem, CommandXboxController operator) {
+        this.subsystem = subsystem;
+        this.operator = operator;
+
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void execute() {
+        subsystem.setCustomPitchDegrees((operator.getRightY() * 10) + subsystem.getPitchGoalDegrees());
+        subsystem.setCustomRollDegrees((operator.getRightX() * 10) + subsystem.getRollGoalDegrees());
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index 6bb864d..d97c8e2 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -29,7 +29,8 @@ public void periodic() {
             double output = intakeProfile.calculate(outputPercentage);
             intakeMotor.set(output);
         } else {
-            intakeMotor.set(0.05);
+            // hold
+            intakeMotor.set(Math.min(0.05, outputPercentage));
         }
 
     }
@@ -41,4 +42,8 @@ public void setOutputPercentage(double outputPercentage) {
     public double getOutputPercentage() {
         return outputPercentage;
     }
+
+    public boolean isHolding() {
+        return intakeBeambreak.get();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
index 06e7cef..00dd63b 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java
@@ -4,37 +4,9 @@
 
 public class AlgaeSubsystem extends SubsystemBase {
     
-    private final AlgaeArm arm;
-    private final AlgaeIntake intake;
-
-    private AlgaePresets currentPreset = AlgaePresets.STOW;
-    private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP;
-
-    /**
-     * Creates a new Algae subsystem
-     */
-    public AlgaeSubsystem(AlgaeArm arm, AlgaeIntake intake) {
-        this.arm = arm;
-        this.intake = intake;
-    }
-
-    public void setAlgaePreset(AlgaePresets preset) {
-        if (preset != currentPreset) {
-            arm.setGoalDegrees(preset.armAngle);
-            currentPreset = preset;
-        }
-    }
-
-    public void setAlgaeIntakePreset(AlgaeIntakePresets preset) {
-        if (preset != currentIntakePreset) {
-            intake.setOutputPercentage(preset.outputPercentage);
-            currentIntakePreset = preset;
-        }
-    }
-
     public enum AlgaePresets {
         STOW(10),
-        FOO(80);
+        FLOOR(90);
 
         public double armAngle;
 
@@ -46,6 +18,7 @@ private AlgaePresets(double armAngle) {
     public enum AlgaeIntakePresets {
         INTAKING(1),
         PURGE(-1),
+        SHOOT(-1),
         STOP(0);
 
         public double outputPercentage;
@@ -53,5 +26,29 @@ public enum AlgaeIntakePresets {
         AlgaeIntakePresets(double outputPercentage) {
             this.outputPercentage = outputPercentage;
         } 
+    }   
+    private final AlgaeArm arm = new AlgaeArm();
+    private final AlgaeIntake intake = new AlgaeIntake();
+
+    private AlgaePresets currentPreset = AlgaePresets.STOW;
+    private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP;
+
+
+    public void setAlgaePreset(AlgaePresets preset) {
+        if (preset != currentPreset) {
+            arm.setGoalDegrees(preset.armAngle);
+            currentPreset = preset;
+        }
+    }
+
+    public void setAlgaeIntakePreset(AlgaeIntakePresets preset) {
+        if (preset != currentIntakePreset) {
+            intake.setOutputPercentage(preset.outputPercentage);
+            currentIntakePreset = preset;
+        }
+    }
+
+    public boolean isHolding() {
+        return intake.isHolding();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 0754021..0a25cd9 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -54,4 +54,8 @@ public void setOutputPercentage(double outputPercentage) {
     public double getOutputPercentage() {
         return outputPercentage;
     }
+
+    public boolean isHolding() {
+        return intakeBeambreak.getValue().value == 0;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 3ddda1b..d694fcf 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -8,9 +8,9 @@
 
 public class CoralSubsystem extends SubsystemBase {
 
-    private final CoralArm arm;
-    private final CoralIntake intake;
-    private final CoralElevator elevator;
+    private final CoralArm arm = new CoralArm();
+    private final CoralIntake intake = new CoralIntake();
+    private final CoralElevator elevator = new CoralElevator();
 
     private final Mechanism2d coralMechanism = new Mechanism2d(2,3);
     private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.5, 0);
@@ -57,8 +57,9 @@ private MirrorPresets(boolean isMirrored) {
     } 
 
     public enum CoralIntakePresets {
-        INTAKING(1),
+        INTAKE(1),
         PURGE(-1),
+        SCORE(-1),
         STOP(0),
 
         CUSTOM(Double.NaN);
@@ -82,12 +83,6 @@ public void periodic() {
     private MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
     private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP;
 
-    public CoralSubsystem(CoralArm arm, CoralIntake intake, CoralElevator elevator) {
-        this.arm = arm;
-        this.intake = intake;
-        this.elevator = elevator;
-    }
-
     public void setCoralPreset(CoralPresets preset) {
         if (preset == CoralPresets.CUSTOM) {
             // uhhh i don't now how to throw an exception and i don't feel like figuring it out
@@ -116,6 +111,10 @@ public double getPitchGoalDegrees() {
         return arm.getPitchGoalDegrees();
     }
 
+    public boolean isHolding() {
+        return intake.isHolding();
+    }
+
     public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
         currentPreset = CoralPresets.CUSTOM;
 

From 59b725747c598ce59e0781c435dbc09cc488580d Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 3 Feb 2025 20:54:52 -0600
Subject: [PATCH 31/71] reorganized commands

---
 src/main/java/frc/robot/RobotContainer.java     | 17 +++++++----------
 .../commands/{ => algae}/ShootAlgaeCommand.java |  2 +-
 .../{ => coral}/ActivateCoralIntakeCommand.java |  2 +-
 .../CoralWristFollowCommand.java}               |  6 +++---
 .../{ => coral}/PurgeCoralIntakeCommand.java    |  2 +-
 5 files changed, 13 insertions(+), 16 deletions(-)
 rename src/main/java/frc/robot/commands/{ => algae}/ShootAlgaeCommand.java (95%)
 rename src/main/java/frc/robot/commands/{ => coral}/ActivateCoralIntakeCommand.java (97%)
 rename src/main/java/frc/robot/commands/{WristFollowCommand.java => coral/CoralWristFollowCommand.java} (77%)
 rename src/main/java/frc/robot/commands/{ => coral}/PurgeCoralIntakeCommand.java (94%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9254d15..4e97cb0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,22 +6,19 @@
 
 import frc.robot.Constants.*;
 import frc.robot.commands.*;
+import frc.robot.commands.algae.ShootAlgaeCommand;
+import frc.robot.commands.coral.ActivateCoralIntakeCommand;
+import frc.robot.commands.coral.CoralWristFollowCommand;
+import frc.robot.commands.coral.PurgeCoralIntakeCommand;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
-import frc.robot.subsystems.algae.AlgaeArm;
-import frc.robot.subsystems.algae.AlgaeIntake;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets;
-import frc.robot.subsystems.coral.CoralArm;
-import frc.robot.subsystems.coral.CoralElevator;
-import frc.robot.subsystems.coral.CoralIntake;
 import frc.robot.subsystems.coral.CoralSubsystem;
-import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
 import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
@@ -137,7 +134,7 @@ private void configureBindings() {
             public boolean getAsBoolean() {
                 return Math.sqrt(Math.pow(operatorXbox.getRightX(),2) + Math.pow(operatorXbox.getRightY(),2)) > 0.25; 
             }
-        }).whileTrue(new WristFollowCommand(coralSubsystem, operatorXbox));
+        }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
         // score / intake coral
         operatorXbox.rightBumper().whileTrue(new ActivateCoralIntakeCommand(coralSubsystem));
         // purge coral
@@ -176,9 +173,9 @@ public boolean getAsBoolean() {
         driverXbox.leftBumper().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
-                return false;
+                return algaeSubsystem.isHolding();
             }
-        }).onTrue(new In);
+        }).whileTrue(new ShootAlgaeCommand(algaeSubsystem));
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/ShootAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java
similarity index 95%
rename from src/main/java/frc/robot/commands/ShootAlgaeCommand.java
rename to src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java
index 3f14f87..f46b5df 100644
--- a/src/main/java/frc/robot/commands/ShootAlgaeCommand.java
+++ b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java
@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.commands.algae;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
diff --git a/src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java b/src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
similarity index 97%
rename from src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java
rename to src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
index afd06d2..f8c3ef4 100644
--- a/src/main/java/frc/robot/commands/ActivateCoralIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.commands.coral;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.coral.CoralSubsystem;
diff --git a/src/main/java/frc/robot/commands/WristFollowCommand.java b/src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java
similarity index 77%
rename from src/main/java/frc/robot/commands/WristFollowCommand.java
rename to src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java
index 219fe7d..216069c 100644
--- a/src/main/java/frc/robot/commands/WristFollowCommand.java
+++ b/src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java
@@ -1,15 +1,15 @@
-package frc.robot.commands;
+package frc.robot.commands.coral;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.coral.CoralSubsystem;
 
-public class WristFollowCommand extends Command {
+public class CoralWristFollowCommand extends Command {
    
     private final CoralSubsystem subsystem;
     private final CommandXboxController operator;
     
-    public WristFollowCommand(CoralSubsystem subsystem, CommandXboxController operator) {
+    public CoralWristFollowCommand(CoralSubsystem subsystem, CommandXboxController operator) {
         this.subsystem = subsystem;
         this.operator = operator;
 
diff --git a/src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java
similarity index 94%
rename from src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java
rename to src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java
index 3571a89..030008b 100644
--- a/src/main/java/frc/robot/commands/PurgeCoralIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java
@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.commands.coral;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.coral.CoralSubsystem;

From b042baaf476033eb3095e34a6e08b29df52ff75b Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Wed, 5 Feb 2025 20:28:11 -0600
Subject: [PATCH 32/71] coral simulation init

untested as usual
---
 src/main/java/frc/robot/Constants.java        | 40 +++++---
 .../frc/robot/subsystems/coral/CoralArm.java  | 93 ++++++++++++++++++-
 .../robot/subsystems/coral/CoralElevator.java | 57 +++++++++++-
 .../robot/subsystems/coral/CoralIntake.java   | 44 ++++++---
 4 files changed, 200 insertions(+), 34 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7484fa1..0d9ac7d 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -196,8 +196,9 @@ public static class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
         public static final double NET_REDUCTION = 96.0;
         public static final double MASS_KG = 4.8;
-        public static final double ARM_LEN_M = 0.51;
+        public static final double ARM_LENGTH_METERS = 0.51;
         public static final double MOI = 0.2875548495; // Kg*m^2
+        public static final double GEARING = 1; // TODO: this is the only placholder
       }
     }
 
@@ -210,12 +211,14 @@ public static class Roll {
         null
       );
       public static final double MAXIMUM_ANGLE = 90;
+
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
         public static final double NET_REDUCTION = 45.0;
         public static final double MASS_KG = 2.85; // Includes a coral
-        public static final double ARM_LEN_M = 0.083;
+        public static final double ARM_LENGTH_METERS = 0.083;
         public static final double MOI = 0.0403605447; // Kg*m^2
+        public static final double GEARING = 1; // TODO: this is the only placholder
       }
     }
     public static class Pitch {
@@ -234,8 +237,9 @@ public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
         public static final double NET_REDUCTION = 92.85714286; // Yeah this is cursed
         public static final double MASS_KG = 2.16; // Includes a coral
-        public static final double ARM_LEN_M = 0.101;
+        public static final double ARM_LENGTH_METERS = 0.101;
         public static final double MOI = 0.0200055915; // Kg*m^2
+        public static final double GEARING = 1; // TODO: this is the only placeholder
       }
     }
 
@@ -247,6 +251,12 @@ public static class Intake {
 
       public static final double IN_OUT_CURRENT_LIMIT = 40.0; // Stator limit
       public static final double HOLD_CURRENT_LIMIT = 5.0; // Stator, TODO: Test this!
+      // TODO: ################### PLACHOLDERS ###################
+      public static final class PhysicalConstants {
+        public static final DCMotor MOTOR = DCMotor.getFalcon500(1);
+        public static final double MOI = 0.1; // J*KG / M^2
+        public static final double GEARING = 1;
+      }
     }
   }
   
@@ -289,6 +299,7 @@ public static final class Intake {
     }
   }
 
+  // See https://cad.onshape.com/documents/fa9a0365dfdf7e376f93f1b4/w/36bfb0cc9de95ef5933791e3/e/700ba3cf920578fe61d3ec24
   public static final class Elevator {
     public static final class Leader {
       public static final int MOTOR_PORT = 10;
@@ -328,16 +339,19 @@ public static class FeedforwardConstants {
     );
 
     public static class PhysicalParameters {
-      public static double gearReduction = 5.0 / 2.0;
-      public static double driveRadiusMeters = 0.0121;
-      public static double carriageMassKg = 8.77; // NOTE: This includes the weight "reduction" due to CF spring counterbalance!
-      public static double elevatorTravelMeters = Units.inchesToMeters(59.5);
-      public static double elevatorBottomFromFloorMeters = Units.inchesToMeters(3.0); // Relative to bottom of stage 2
-      public static double elevatorCarriageHeightMeters = Units.inchesToMeters(8.0); // Bottom to top of stage 2
-      public static double coralArmPivotVerticalOffset = Units.inchesToMeters(6.0); // From bottom of stage 2 to coral arm pivot axis
-      public static double coralArmPivotHorizontalOffset = Units.inchesToMeters(7.5); // From bottom of stage 2 to coral arm pivot axis
-      public static double elevatorForwardsFromRobotCenterMeters = Units.inchesToMeters(1); // To mid-plane of elevator
-      public static DCMotor MOTOR = DCMotor.getNeoVortex(2);
+      public static final double GEARING = 5.0 / 2.0;
+      public static final double DRIVE_RADIUS_METERS = 0.0121;
+      public static final double CARRIAGE_MASS_KG = 8.77; // NOTE: This includes the weight "reduction" due to CF spring counterbalance!
+
+      public static final double MAX_TRAVEL = Units.inchesToMeters(59.5);
+      public static final double BOTTOM_TO_FLOOR = Units.inchesToMeters(3.0); // Relative to bottom of stage 2
+      public static final double CARRIAGE_HEIGHT = Units.inchesToMeters(8.0); // Bottom to top of stage 2
+      
+      public static final double CORAL_PIVOT_VERTICAL_OFFSET = Units.inchesToMeters(6.0); // From bottom of stage 2 to coral arm pivot axis
+      public static final double CORAL_PIVOT_HORIZONTAL_OFFSET = Units.inchesToMeters(7.5); // From bottom of stage 2 to coral arm pivot axis
+      public static final double ELEVATOR_FORWARDS_OFFSET = Units.inchesToMeters(1); // To mid-plane of elevator
+
+      public static final DCMotor MOTOR = DCMotor.getNeoVortex(2);
     }
   }
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index bcf9b59..74d7132 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,7 +1,11 @@
 package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.sim.CANcoderSimState;
 import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.sim.SparkAbsoluteEncoderSim;
+import com.revrobotics.sim.SparkFlexSim;
+import com.revrobotics.sim.SparkMaxSim;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -15,23 +19,67 @@
 import edu.wpi.first.math.controller.ControlAffinePlantInversionFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Unit;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase{
     private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+    // sim motors
+    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
 
     private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
     private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
     private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
+    // sim encoders
+    private final CANcoderSimState simPivotEncoder = pivotEncoder.getSimState();
+    private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
+    private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
+
+    // physics simulations
+    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+        Coral.Pivot.PhysicalConstants.MOTOR,
+        Coral.Pivot.PhysicalConstants.GEARING,
+        Coral.Pivot.PhysicalConstants.MOI,
+        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE * -1),
+        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE),
+        true,
+        0
+    );
+    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+        Coral.Roll.PhysicalConstants.MOTOR,
+        Coral.Roll.PhysicalConstants.GEARING,
+        Coral.Roll.PhysicalConstants.MOI,
+        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE * -1),
+        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE),
+        false,
+        0
+    );
+    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+        Coral.Pitch.PhysicalConstants.MOTOR,
+        Coral.Pitch.PhysicalConstants.GEARING,
+        Coral.Pitch.PhysicalConstants.MOI,
+        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE * -1),
+        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE),
+        false,
+        0
+    );
 
     private final SparkMaxConfig pivotConfig = new SparkMaxConfig();
     private final SparkMaxConfig rollConfig = new SparkMaxConfig();
     private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
 
-
     private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
     private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
     private final ProfiledPIDController rollPID = Coral.Roll.PID;
@@ -80,7 +128,36 @@ public void periodic() {
         );
         rollMotor.set(rollPID.calculate(rollEncoder.getPosition()));
         pitchMotor.set(pitchPID.calculate(pitchEncoder.getPosition()));
-    } 
+    }
+    
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+        simRollPhysics.setInput(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+        simPitchPhysics.setInput(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+
+        simPivotPhysics.update(0.02);
+        simRollPhysics.update(0.02);
+        simPitchPhysics.update(0.02);
+
+        // update sim objects
+        simPivotMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+        simRollMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+        simPitchMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+    }
 
     // this should be relative to straight upwards.
     // i.e. 0 should be straight vertical,
@@ -136,15 +213,21 @@ public double getPitchGoalDegrees() {
     }
 
     public double getPivotPositionDegrees() {
-        return pivotEncoder.getAbsolutePosition().getValueAsDouble();
+        return Robot.isReal() 
+            ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
+            : Units.radiansToDegrees(simPivotPhysics.getAngleRads());
     }
 
     public double getRollPositionDegrees() {
-        return rollEncoder.getPosition();
+        return Robot.isReal()
+            ? rollEncoder.getPosition()
+            : simRollEncoder.getPosition();
     }
 
     public double getPitchPositionDegrees() {
-        return pitchEncoder.getPosition();
+        return Robot.isReal() 
+            ? pitchEncoder.getPosition()
+            : simPitchEncoder.getPosition();
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index d01a948..306b4b8 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -4,6 +4,8 @@
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.sim.SparkFlexSim;
+import com.revrobotics.sim.SparkRelativeEncoderSim;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkFlexConfig;
@@ -11,21 +13,43 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.Constants.Elevator;
 
 public class CoralElevator extends SubsystemBase {
 
     private final SparkFlex leaderMotor = new SparkFlex(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
     private final SparkFlex followerMotor = new SparkFlex(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless);
-
-    private final SparkFlexConfig leaderConfig = new SparkFlexConfig();
-    private final SparkFlexConfig followerConfig = new SparkFlexConfig();
+    // sim motors
+    private final SparkFlexSim simLeaderMotor = new SparkFlexSim(leaderMotor, DCMotor.getNeoVortex(1));
+    private final SparkFlexSim simFollowerMotor = new SparkFlexSim(followerMotor, DCMotor.getNeoVortex(1));
 
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
+    // sim encoders
+    private final SparkRelativeEncoderSim simLeaderEncoder = new SparkRelativeEncoderSim(leaderMotor);
+    private final SparkRelativeEncoderSim simFollowerEncoder = new SparkRelativeEncoderSim(followerMotor);
+
+    // physics simulations
+    private final ElevatorSim simElevator = new ElevatorSim(
+        Elevator.PhysicalParameters.MOTOR,
+        Elevator.PhysicalParameters.GEARING,
+        Elevator.PhysicalParameters.CARRIAGE_MASS_KG,
+        Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
+        0,
+        Elevator.PhysicalParameters.MAX_TRAVEL,
+        true,
+        0
+    );
+
+    private final SparkFlexConfig leaderConfig = new SparkFlexConfig();
+    private final SparkFlexConfig followerConfig = new SparkFlexConfig();
 
 
     private final ElevatorFeedforward feedForward = Elevator.FEEDFORWARD;
@@ -97,8 +121,29 @@ public void periodic() {
         }
     }
 
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simElevator.setInput(
+            (leaderMotor.getAppliedOutput() + followerMotor.getAppliedOutput()) * RoboRioSim.getVInVoltage()
+        );
+        simElevator.update(0.02);
+
+        // update sim objects
+        simLeaderMotor.iterate(
+            simElevator.getVelocityMetersPerSecond() / Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+        simFollowerMotor.iterate(
+            simElevator.getVelocityMetersPerSecond() / Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+    }
+
     public void setGoalPosition(double position) {
-        elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.elevatorTravelMeters));
+        elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.MAX_TRAVEL));
     }
     
     public double getGoalPosition() {
@@ -106,7 +151,9 @@ public double getGoalPosition() {
     }
 
     public double getPosition() {
-        return leaderEncoder.getPosition();
+        return Robot.isReal() 
+            ? leaderEncoder.getPosition()
+            : simLeaderEncoder.getPosition();
     }
 
     public boolean isInPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 0a25cd9..20616cc 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,27 +1,35 @@
 package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitValue;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.sim.TalonFXSimState;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Coral;
 
 public class CoralIntake extends SubsystemBase {
     private final TalonFX intakeMotor = new TalonFX(Coral.Intake.MOTOR_PORT);
+    // sim motor
+    private TalonFXSimState simIntakeMotor;
 
-    private final StatusSignal<ReverseLimitValue> intakeBeambreak = intakeMotor.getReverseLimit();
-
+    // physics simulation
+    private final FlywheelSim simIntakePhysics = new FlywheelSim(
+        LinearSystemId.createFlywheelSystem(
+            Coral.Intake.PhysicalConstants.MOTOR,
+            Coral.Intake.PhysicalConstants.MOI,
+            Coral.Intake.PhysicalConstants.GEARING
+        ),
+        Coral.Intake.PhysicalConstants.MOTOR
+    );
+ 
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
         Coral.Intake.POSITIVE_RATE_LIMIT,
         Coral.Intake.NEGATIVE_RATE_LIMIT,
@@ -37,7 +45,7 @@ public CoralIntake() {
     @Override
     public void periodic() {
         // if no coral 
-        if (intakeBeambreak.getValue().value == 1) {
+        if (this.isHolding()) {
             double output = intakeProfile.calculate(outputPercentage);
             intakeMotor.set(output);
         } else {
@@ -47,6 +55,20 @@ public void periodic() {
 
     }
 
+    @Override
+    public void simulationPeriodic() {
+        simIntakeMotor = intakeMotor.getSimState();
+        simIntakeMotor.setSupplyVoltage(RoboRioSim.getVInVoltage());
+        
+        // update physics
+        simIntakePhysics.setInput(simIntakeMotor.getMotorVoltage() * RoboRioSim.getVInVoltage());
+        simIntakePhysics.update(0.02);
+
+        // update sim objects
+        //                              rpm to rps
+        simIntakeMotor.setRotorVelocity(simIntakePhysics.getAngularVelocityRPM() / 60);
+    }
+
     public void setOutputPercentage(double outputPercentage) {
         this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1);
     }
@@ -56,6 +78,6 @@ public double getOutputPercentage() {
     }
 
     public boolean isHolding() {
-        return intakeBeambreak.getValue().value == 0;
+        return intakeMotor.getReverseLimit().getValue().value == 0;
     }
 }

From 7c17fa908fd9147a30cd9413e23df7d857aee5ad Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 6 Feb 2025 18:30:18 -0600
Subject: [PATCH 33/71] refactored coral scoring/intaking command

---
 src/main/java/frc/robot/RobotContainer.java   | 18 ++++++--
 .../coral/ActivateCoralIntakeCommand.java     | 41 -------------------
 .../commands/coral/IntakeCoralCommand.java    | 33 +++++++++++++++
 .../commands/coral/ScoreCoralCommand.java     | 31 ++++++++++++++
 4 files changed, 79 insertions(+), 44 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
 create mode 100644 src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
 create mode 100644 src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4e97cb0..5336e2b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,9 +7,10 @@
 import frc.robot.Constants.*;
 import frc.robot.commands.*;
 import frc.robot.commands.algae.ShootAlgaeCommand;
-import frc.robot.commands.coral.ActivateCoralIntakeCommand;
 import frc.robot.commands.coral.CoralWristFollowCommand;
+import frc.robot.commands.coral.IntakeCoralCommand;
 import frc.robot.commands.coral.PurgeCoralIntakeCommand;
+import frc.robot.commands.coral.ScoreCoralCommand;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
@@ -136,7 +137,18 @@ public boolean getAsBoolean() {
             }
         }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
         // score / intake coral
-        operatorXbox.rightBumper().whileTrue(new ActivateCoralIntakeCommand(coralSubsystem));
+        operatorXbox.rightBumper().and(new BooleanSupplier() {
+            @Override
+            public boolean getAsBoolean() {
+                return coralSubsystem.isHolding();
+            }
+        }).whileTrue(new ScoreCoralCommand(coralSubsystem));
+        operatorXbox.rightBumper().and(new BooleanSupplier() {
+            @Override
+            public boolean getAsBoolean() {
+                return !coralSubsystem.isHolding();
+            }
+        }).whileTrue(new IntakeCoralCommand(coralSubsystem));
         // purge coral
         operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
         /*
@@ -159,7 +171,7 @@ public boolean getAsBoolean() {
         })).onTrue(new InstantCommand(() -> {
             climberSubsystem.setOutput(-1);
         }));
-        // something something maintainence
+        // TODO: something something maintainence
         driverXbox.button(6).onTrue(new SequentialCommandGroup(null));
         // set field orientation
         driverXbox.button(7).onTrue(new InstantCommand(() -> {
diff --git a/src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java b/src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
deleted file mode 100644
index f8c3ef4..0000000
--- a/src/main/java/frc/robot/commands/coral/ActivateCoralIntakeCommand.java
+++ /dev/null
@@ -1,41 +0,0 @@
-package frc.robot.commands.coral;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.coral.CoralSubsystem;
-import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
-
-public class ActivateCoralIntakeCommand extends Command {
-
-    private final CoralSubsystem subsystem;
-    private final boolean scoring;
-   
-    public ActivateCoralIntakeCommand(CoralSubsystem subsystem) {
-        this.subsystem = subsystem;
-        this.scoring = subsystem.isHolding();
-        addRequirements(subsystem);
-    }
-
-    @Override
-    public void initialize() {
-        // score if holding, intake if not
-        if (scoring) {
-            subsystem.setCoralIntakePreset(CoralIntakePresets.SCORE);
-        } else {
-            subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE);
-        }   
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        if (scoring) {
-            subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
-        } else {
-            // if intaking but failed
-            if (!subsystem.isHolding()) {
-                subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
-            }
-            // else continue to hold the piece
-        }
-    }
-
-}
diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
new file mode 100644
index 0000000..db01362
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
@@ -0,0 +1,33 @@
+package frc.robot.commands.coral;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
+
+public class IntakeCoralCommand extends Command{
+    private final CoralSubsystem subsystem;
+
+    public IntakeCoralCommand(CoralSubsystem subsystem) {
+        this.subsystem = subsystem;
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+        subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // if intaking failed
+        if (!subsystem.isHolding()) {
+            subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+        }
+        // else continue to hold the piece
+    }
+
+    @Override
+    public boolean isFinished() {
+        return subsystem.isHolding();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
new file mode 100644
index 0000000..ffcbb44
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
@@ -0,0 +1,31 @@
+package frc.robot.commands.coral;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
+
+public class ScoreCoralCommand extends Command{
+
+    private final CoralSubsystem subsystem;
+   
+    public ScoreCoralCommand(CoralSubsystem subsystem) {
+        this.subsystem = subsystem;
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+        subsystem.setCoralIntakePreset(CoralIntakePresets.SCORE);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !subsystem.isHolding();
+    }
+
+}

From 83c298158a79ba7d832d9d42a0cf43378f12cf6a Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 6 Feb 2025 19:01:03 -0600
Subject: [PATCH 34/71] algae simulation init

---
 src/main/java/frc/robot/Constants.java        | 11 +++-
 .../frc/robot/subsystems/algae/AlgaeArm.java  | 54 +++++++++++++++----
 .../robot/subsystems/algae/AlgaeIntake.java   | 29 ++++++++++
 3 files changed, 81 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 0d9ac7d..b3e319a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -282,10 +282,11 @@ public static final class Pivot {
       public static final double EXTENDED_LIMIT_DEGREES = 90.0;
 
       public static class PhysicalConstants {
-        public static DCMotor MOTOR = DCMotor.getNeoVortex(1);
+        public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
+        public static final double GEARING = 1; // TODO: this is the only placeholder
         public static final double NET_REDUCTION = 42.66666667; // Yeah this is cursed
         public static final double MASS_KG = 3.18;
-        public static final double ARM_LEN_M = 0.2349863728;
+        public static final double ARM_LENGTH_METERS = 0.2349863728;
         public static final double MOI = 0.1114866914; // Kg*m^2
       }
     }
@@ -296,6 +297,12 @@ public static final class Intake {
 
       public static final double POSITIVE_RATE_LIMIT = 5.0;
       public static final double NEGATIVE_RATE_LIMIT = -5.0;
+
+      public static final class PhysicalConstants {
+        public static final DCMotor MOTOR = DCMotor.getNeo550(1);
+        public static final double MOI = 0.1; // J*KG / M^2
+        public static final double GEARING = 1;
+      }
     }
   }
 
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index 018cf1d..d5d540e 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -1,6 +1,8 @@
 package frc.robot.subsystems.algae;
 
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+import com.revrobotics.sim.SparkFlexSim;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -13,21 +15,35 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.Constants.Algae;
 
-import static frc.robot.Constants.Algae.Pivot.MOTOR_PORT;
-import static frc.robot.Constants.Algae.Pivot.ENCODER_PORT;
-import static frc.robot.Constants.Algae.Pivot.PID;
-import static frc.robot.Constants.Algae.Pivot.FEEDFORWARD;
-
 public class AlgaeArm extends SubsystemBase {
-    public final SparkFlex pivotMotor = new SparkFlex(MOTOR_PORT, MotorType.kBrushless);
-    public final SparkMaxConfig pivotConfig = new SparkMaxConfig();
-    public final CANcoder pivotEncoder = new CANcoder(ENCODER_PORT);
+    private final SparkFlex pivotMotor = new SparkFlex(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    // sim motor
+    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Algae.Pivot.PhysicalConstants.MOTOR);
+    
+    public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
+
+    // physics simulation
+    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+        Algae.Pivot.PhysicalConstants.MOTOR,
+        Algae.Pivot.PhysicalConstants.GEARING,
+        Algae.Pivot.PhysicalConstants.MOI,
+        Algae.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+        Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES),
+        Units.degreesToRadians(Algae.Pivot.EXTENDED_LIMIT_DEGREES),
+        true,
+        Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES)
+    );
 
-    public final ProfiledPIDController pivotPID = PID;
-    public final ArmFeedforward pivotFeedforward = FEEDFORWARD;
+    private final SparkMaxConfig pivotConfig = new SparkMaxConfig();
+
+    private final ProfiledPIDController pivotPID = Algae.Pivot.PID;
+    private final ArmFeedforward pivotFeedforward = Algae.Pivot.FEEDFORWARD;
 
     public AlgaeArm() {
         pivotConfig.idleMode(IdleMode.kBrake);
@@ -44,6 +60,20 @@ public void periodic() {
         pivotMotor.set(output);
     }
 
+    @Override
+    public void simulationPeriodic() {
+        // update physics 
+        simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+        simPivotPhysics.update(0.02);
+
+        // update sim objects
+        simPivotMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+    }
+
     public void setGoalDegrees(double goal) {
         pivotPID.setGoal(MathUtil.clamp(goal, Algae.Pivot.RETRACTED_LIMIT_DEGREES, Algae.Pivot.EXTENDED_LIMIT_DEGREES));
     }
@@ -53,6 +83,8 @@ public double getGoalDegrees() {
     }
 
     public double getPositionDegrees() {
-        return pivotEncoder.getAbsolutePosition().getValueAsDouble();
+        return Robot.isReal() 
+            ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
+            : Units.radiansToDegrees(simPivotPhysics.getAngleRads());
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index d97c8e2..dfde316 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -1,16 +1,32 @@
 package frc.robot.subsystems.algae;
 
+import com.revrobotics.sim.SparkMaxSim;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
 
 public class AlgaeIntake extends SubsystemBase {
     private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless);
+    // sim motor
+    private final SparkMaxSim simIntakeMotor = new SparkMaxSim(intakeMotor, Algae.Intake.PhysicalConstants.MOTOR);
+
+    // physics simulation
+    private final FlywheelSim simIntakePhysics = new FlywheelSim(
+        LinearSystemId.createFlywheelSystem(
+            Algae.Intake.PhysicalConstants.MOTOR,
+            Algae.Intake.PhysicalConstants.MOI,
+            Algae.Intake.PhysicalConstants.GEARING
+        ),
+        Algae.Intake.PhysicalConstants.MOTOR
+    );
 
     private final DigitalInput intakeBeambreak = new DigitalInput(Algae.Intake.BEAMBREAK_PORT);
 
@@ -32,7 +48,20 @@ public void periodic() {
             // hold
             intakeMotor.set(Math.min(0.05, outputPercentage));
         }
+    }
+    
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simIntakePhysics.setInput(simIntakeMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+        simIntakePhysics.update(0.02);
 
+        // update sim objects
+        simIntakeMotor.iterate(
+            simIntakePhysics.getAngularVelocityRPM(),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
     }
 
     public void setOutputPercentage(double outputPercentage) {

From e444934c942b7ab2ceebac95b112d6ba2b3093a2 Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 6 Feb 2025 19:03:45 -0600
Subject: [PATCH 35/71] removed unused references

---
 src/main/java/frc/robot/Constants.java                      | 3 ---
 src/main/java/frc/robot/subsystems/algae/AlgaeArm.java      | 2 --
 src/main/java/frc/robot/subsystems/coral/CoralArm.java      | 5 -----
 src/main/java/frc/robot/subsystems/coral/CoralElevator.java | 1 -
 src/main/java/frc/robot/subsystems/coral/CoralIntake.java   | 3 ---
 5 files changed, 14 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index b3e319a..88f0247 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import com.revrobotics.spark.config.LimitSwitchConfig.Type;
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -13,8 +12,6 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index d5d540e..33fd1d0 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -1,10 +1,8 @@
 package frc.robot.subsystems.algae;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.sim.CANcoderSimState;
 import com.revrobotics.sim.SparkFlexSim;
 import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 74d7132..d48222e 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,7 +1,6 @@
 package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.sim.CANcoderSimState;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.sim.SparkAbsoluteEncoderSim;
 import com.revrobotics.sim.SparkFlexSim;
@@ -16,11 +15,8 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.ControlAffinePlantInversionFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.Unit;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -40,7 +36,6 @@ public class CoralArm extends SubsystemBase{
     private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
     private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
     // sim encoders
-    private final CANcoderSimState simPivotEncoder = pivotEncoder.getSimState();
     private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
     private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index 306b4b8..f68a293 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -34,7 +34,6 @@ public class CoralElevator extends SubsystemBase {
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
     // sim encoders
     private final SparkRelativeEncoderSim simLeaderEncoder = new SparkRelativeEncoderSim(leaderMotor);
-    private final SparkRelativeEncoderSim simFollowerEncoder = new SparkRelativeEncoderSim(followerMotor);
 
     // physics simulations
     private final ElevatorSim simElevator = new ElevatorSim(
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 20616cc..441fccc 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,14 +1,11 @@
 package frc.robot.subsystems.coral;
 
-import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.signals.ReverseLimitValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;

From 2003b1ff01eec94af759020386823fd3ad5b9737 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Thu, 6 Feb 2025 19:26:51 -0600
Subject: [PATCH 36/71] AYAAYAYA

---
 .../deploy/pathplanner/autos/Testing.auto     |  8 +--
 .../deploy/pathplanner/paths/New Path.path    | 12 ++---
 src/main/deploy/pathplanner/paths/RIght.path  | 54 +++++++++++++++++++
 src/main/java/frc/robot/Constants.java        |  1 +
 4 files changed, 62 insertions(+), 13 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/RIght.path

diff --git a/src/main/deploy/pathplanner/autos/Testing.auto b/src/main/deploy/pathplanner/autos/Testing.auto
index f9fe73c..56de147 100644
--- a/src/main/deploy/pathplanner/autos/Testing.auto
+++ b/src/main/deploy/pathplanner/autos/Testing.auto
@@ -7,13 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "New Path"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Testing"
+            "pathName": "RIght"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 8c7148e..b544ba1 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.653176229508197,
-        "y": 1.2258709016393436
+        "x": 7.060758196721312,
+        "y": 1.7293545081967203
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.257581967213115,
-        "y": 1.2618340163934432
+        "x": 6.161680327868852,
+        "y": 2.02904713114754
       },
       "prevControl": {
-        "x": 6.821004098360655,
-        "y": 1.2498463114754088
+        "x": 6.725102459016392,
+        "y": 2.0170594262295056
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/RIght.path b/src/main/deploy/pathplanner/paths/RIght.path
new file mode 100644
index 0000000..aa5cc02
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/RIght.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.569262295081967,
+        "y": 1.4176741803278685
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.569930419680603,
+        "y": 1.8192129101417325
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.569262295081967,
+        "y": 2.62843237704918
+      },
+      "prevControl": {
+        "x": 6.571436131003902,
+        "y": 2.2223790917963644
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 822bf3d..8a81612 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -57,6 +57,7 @@ public static Alliance getAlliance() {
 
       return Alliance.Blue;
     }
+    
   }
 
   public static class SwerveModuleConstants {

From 759031f383d118d2d9f3282426b6bd16642df86b Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Thu, 6 Feb 2025 19:37:42 -0600
Subject: [PATCH 37/71] fixed the minimum to get simulation working

---
 simgui-ds.json                                | 102 ++++++++++++++++++
 src/main/java/frc/robot/Constants.java        |  19 ++--
 src/main/java/frc/robot/RobotContainer.java   |   6 +-
 .../robot/subsystems/coral/CoralIntake.java   |   5 +-
 4 files changed, 123 insertions(+), 9 deletions(-)
 create mode 100644 simgui-ds.json

diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..b48202b
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,102 @@
+{
+  "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
+    }
+  ],
+  "robotJoysticks": [
+    {},
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
+    {
+      "useGamepad": true
+    }
+  ]
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 88f0247..4696dfc 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -161,7 +162,7 @@ public static class Climber {
       1,
       0.0,
       0.0, 
-      null
+      new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
     );
   }
 
@@ -179,7 +180,7 @@ public static class Pivot {
         1,
         0.0,
         0.0,
-        null
+        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
       );
 
       // Updated with THEORETICAL values
@@ -205,7 +206,7 @@ public static class Roll {
         1,
         0.0,
         0.0,
-        null
+        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
       );
       public static final double MAXIMUM_ANGLE = 90;
 
@@ -227,7 +228,7 @@ public static class Pitch {
         1,
         0.0,
         0.0,
-        null
+        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
       );
 
       public static class PhysicalConstants {
@@ -267,7 +268,7 @@ public static final class Pivot {
         1,
         0.0,
         0.0,
-        null
+        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
       );
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
         1,
@@ -377,8 +378,12 @@ public static final class PathPlannerConstants {
         SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
         DCMotor.getKrakenX60(1),
         DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
-        4
-      )
+        1
+      ),
+      new Translation2d(-0.5, 0.5),
+      new Translation2d(0.5, 0.5),
+      new Translation2d(-0.5, -0.5),
+      new Translation2d(0.5, -0.5)
     );
   }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5336e2b..e58b97c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -172,7 +172,11 @@ public boolean getAsBoolean() {
             climberSubsystem.setOutput(-1);
         }));
         // TODO: something something maintainence
-        driverXbox.button(6).onTrue(new SequentialCommandGroup(null));
+        driverXbox.button(6).onTrue(new SequentialCommandGroup(
+            new InstantCommand(() -> {
+                System.out.print("swaaws");
+            })
+        ));
         // set field orientation
         driverXbox.button(7).onTrue(new InstantCommand(() -> {
             swerveDriveSubsystem.setHeading(0);
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 441fccc..24f2513 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
 import frc.robot.Constants.Coral;
 
 public class CoralIntake extends SubsystemBase {
@@ -75,6 +76,8 @@ public double getOutputPercentage() {
     }
 
     public boolean isHolding() {
-        return intakeMotor.getReverseLimit().getValue().value == 0;
+        return Robot.isReal()
+            ? intakeMotor.getReverseLimit().getValue().value == 0
+            : false;
     }
 }

From a797168a172cfeb8a646dbc2fb1510c5fc861ca9 Mon Sep 17 00:00:00 2001
From: Yunju607 <yj207lee@gmail.com>
Date: Sat, 8 Feb 2025 11:28:20 -0600
Subject: [PATCH 38/71] Revert "Merge branch 'Autos' into addManipulators"

This reverts commit ef4addfeb3f2132eb3f30a2e9baf2bc4b184ce0e, reversing
changes made to 759031f383d118d2d9f3282426b6bd16642df86b.
---
 .../deploy/pathplanner/autos/New Auto.auto    | 31 ------
 .../deploy/pathplanner/autos/Testing.auto     | 19 ----
 src/main/deploy/pathplanner/navgrid.json      |  1 -
 .../deploy/pathplanner/paths/New Path.path    | 54 -----------
 src/main/deploy/pathplanner/paths/Path 3.path | 75 ---------------
 src/main/deploy/pathplanner/paths/RIght.path  | 54 -----------
 .../deploy/pathplanner/paths/route 1.path     | 95 -------------------
 .../deploy/pathplanner/paths/route 2.path     | 54 -----------
 src/main/deploy/pathplanner/settings.json     | 32 -------
 src/main/java/frc/robot/Constants.java        | 12 +--
 src/main/java/frc/robot/RobotContainer.java   | 28 +-----
 .../frc/robot/subsystems/SwerveSubsystem.java | 54 ++---------
 vendordeps/AdvantageKit.json                  | 11 +--
 ....2.2.json => PathplannerLib-2025.1.1.json} |  8 +-
 ...enix6-25.2.1.json => Phoenix6-25.1.0.json} | 84 ++++++----------
 ...Lib-2025.0.2.json => REVLib-2025.0.0.json} | 15 +--
 ...ca-2025.0.1.json => Studica-2025.0.0.json} | 14 +--
 17 files changed, 69 insertions(+), 572 deletions(-)
 delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 delete mode 100644 src/main/deploy/pathplanner/autos/Testing.auto
 delete mode 100644 src/main/deploy/pathplanner/navgrid.json
 delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path
 delete mode 100644 src/main/deploy/pathplanner/paths/Path 3.path
 delete mode 100644 src/main/deploy/pathplanner/paths/RIght.path
 delete mode 100644 src/main/deploy/pathplanner/paths/route 1.path
 delete mode 100644 src/main/deploy/pathplanner/paths/route 2.path
 delete mode 100644 src/main/deploy/pathplanner/settings.json
 rename vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib-2025.1.1.json} (87%)
 rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.1.0.json} (85%)
 rename vendordeps/{REVLib-2025.0.2.json => REVLib-2025.0.0.json} (86%)
 rename vendordeps/{Studica-2025.0.1.json => Studica-2025.0.0.json} (89%)

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
deleted file mode 100644
index 4944055..0000000
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ /dev/null
@@ -1,31 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "New Path"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Path 3"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "route 2"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Testing.auto b/src/main/deploy/pathplanner/autos/Testing.auto
deleted file mode 100644
index 56de147..0000000
--- a/src/main/deploy/pathplanner/autos/Testing.auto
+++ /dev/null
@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "RIght"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
deleted file mode 100644
index 23e0db9..0000000
--- a/src/main/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1 +0,0 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
deleted file mode 100644
index b544ba1..0000000
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.108709016393442,
-        "y": 1.2618340163934432
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.060758196721312,
-        "y": 1.7293545081967203
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.161680327868852,
-        "y": 2.02904713114754
-      },
-      "prevControl": {
-        "x": 6.725102459016392,
-        "y": 2.0170594262295056
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0.0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Path 3.path b/src/main/deploy/pathplanner/paths/Path 3.path
deleted file mode 100644
index 3fea5ff..0000000
--- a/src/main/deploy/pathplanner/paths/Path 3.path	
+++ /dev/null
@@ -1,75 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 5.154713114754099,
-        "y": 1.273821721311475
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.471413934426229,
-        "y": 2.1129610655737703
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.800102459016393,
-        "y": 2.71234631147541
-      },
-      "prevControl": {
-        "x": 4.040483445926598,
-        "y": 2.643666029501063
-      },
-      "nextControl": {
-        "x": 3.5062341001529425,
-        "y": 2.7963086997221134
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.5703893442622954,
-        "y": 0.722387295081968
-      },
-      "prevControl": {
-        "x": 1.8461065573770497,
-        "y": 1.0460553278688542
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.8804478208716437,
-      "rotationDegrees": 59.99999999999999
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 53.20990143837071
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/RIght.path b/src/main/deploy/pathplanner/paths/RIght.path
deleted file mode 100644
index aa5cc02..0000000
--- a/src/main/deploy/pathplanner/paths/RIght.path
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 6.569262295081967,
-        "y": 1.4176741803278685
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 6.569930419680603,
-        "y": 1.8192129101417325
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.569262295081967,
-        "y": 2.62843237704918
-      },
-      "prevControl": {
-        "x": 6.571436131003902,
-        "y": 2.2223790917963644
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path
deleted file mode 100644
index 6c871fb..0000000
--- a/src/main/deploy/pathplanner/paths/route 1.path	
+++ /dev/null
@@ -1,95 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 5.658196721311475,
-        "y": 0.8782274590163933
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 5.082786885245901,
-        "y": 1.7653176229508185
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.9919057377049176,
-        "y": 2.7602971311475404
-      },
-      "prevControl": {
-        "x": 4.406172988804652,
-        "y": 2.7268884818653034
-      },
-      "nextControl": {
-        "x": 3.248668032786885,
-        "y": 2.820235655737705
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.3785860655737705,
-        "y": 0.8782274590163933
-      },
-      "prevControl": {
-        "x": 2.3136270491803277,
-        "y": 0.7583504098360652
-      },
-      "nextControl": {
-        "x": 1.1306156494023079,
-        "y": 0.9100185380127347
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.6802254098360656,
-        "y": 2.8921618852459017
-      },
-      "prevControl": {
-        "x": 3.464446721311475,
-        "y": 2.62843237704918
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.9092363054778103,
-      "rotationDegrees": 59.99999999999999
-    },
-    {
-      "waypointRelativePos": 1.87,
-      "rotationDegrees": 55.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -33.690067525979806
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/route 2.path b/src/main/deploy/pathplanner/paths/route 2.path
deleted file mode 100644
index 571c9e5..0000000
--- a/src/main/deploy/pathplanner/paths/route 2.path	
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6322745901639335,
-        "y": 2.8561987704918033
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.35655737704918,
-        "y": 2.5804815573770488
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.4385245901639343,
-        "y": 0.9261782786885238
-      },
-      "prevControl": {
-        "x": 1.7142418032786886,
-        "y": 1.3337602459016387
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 53.97262661489648
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 59.743562836470616
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
deleted file mode 100644
index 5bf438c..0000000
--- a/src/main/deploy/pathplanner/settings.json
+++ /dev/null
@@ -1,32 +0,0 @@
-{
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
-  "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
-  "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
-  "robotMOI": 6.883,
-  "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
-  "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
-  "wheelCOF": 1.2,
-  "flModuleX": 0.273,
-  "flModuleY": 0.273,
-  "frModuleX": 0.273,
-  "frModuleY": -0.273,
-  "blModuleX": -0.273,
-  "blModuleY": 0.273,
-  "brModuleX": -0.273,
-  "brModuleY": -0.273,
-  "bumperOffsetX": 0.0,
-  "bumperOffsetY": 0.0,
-  "robotFeatures": []
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index dd483d2..4696dfc 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -18,7 +18,6 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
@@ -45,8 +44,8 @@ public static class RobotConstants {
     public static final double robotLengthMeters = Units.inchesToMeters(25.0);
     
       // TODO: ############## REPLACE PLACEHOLDERS ##############
-      public static final double TOTAL_MASS_KG = 49; // 107lbs
-      public static final double MOMENT_OF_INERTIA = 5;
+      public static final double TOTAL_MASS_KG = 10;
+      public static final double MOMENT_OF_INERTIA = 1;
   }
 
   public static final class FieldConstants {
@@ -60,14 +59,13 @@ public static Alliance getAlliance() {
 
       return Alliance.Blue;
     }
-    
   }
 
   public static class SwerveModuleConstants {
     public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
-    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d); // 6.75:1
+    public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d);
     // This is for L2 modules with 16T pinions
-    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d);
+    public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d) * (16.f / 14.f);
 
     public static final double DRIVE_ROTATION_TO_METER = DRIVE_GEAR_RATIO * Math.PI * WHEEL_DIAMETER;
     public static final double STEER_ROTATION_TO_RADIANS = STEERING_GEAR_RATIO * Math.PI * 2d;
@@ -127,7 +125,7 @@ public static class DriveConstants {
     public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec
 
     // TODO: ############## REPLACE PLACEHOLDERS ##############
-    public static final double MAX_MODULE_CURRENT = 100;
+    public static final double MAX_MODULE_CURRENT = 10;
 
     public static final double TRACK_WIDTH = Units.inchesToMeters(19.75);
     public static final double WHEEL_BASE = Units.inchesToMeters(19.75);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 389cbae..e58b97c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,27 +20,14 @@
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
 import java.util.function.BooleanSupplier;
-import java.util.function.DoubleSupplier;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.ControllerConstants;
-import frc.robot.commands.DriveCommand;
-import frc.robot.commands.ElevatorCommand;
-import frc.robot.commands.ElevatorCommand.ElevatorPresets;
-import frc.robot.commands.ElevatorFollowCommand;
-import frc.robot.subsystems.ElevatorSubsystem;
-import frc.robot.subsystems.SwerveSubsystem;
+import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.button.*;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -92,19 +79,8 @@ public RobotContainer() {
         SmartDashboard.putData("Auto Chooser", autoChooser);
 
         swerveDriveSubsystem.setDefaultCommand(normalDrive);
-        NamedCommands.registerCommand("Testing", elevatorToTop);
     }
 
-    // Command shootAction =
-    // Command alignAction = ; // Self-deadlines
-    // Command spoolAction =
-    // Command intakeAction = ;
-    
-    private ElevatorCommand elevatorToTop = new ElevatorCommand(elevator, ElevatorPresets.TOP, 0.0);
-    private ElevatorCommand elevatorToMiddle = new ElevatorCommand(elevator, ElevatorPresets.MIDDLE, 0.0);
-    private ElevatorCommand elevatorToStow = new ElevatorCommand(elevator, ElevatorPresets.STOW, 0.0);
-
-
     /**
      * Use this method to define your trigger->command mappings. Triggers can be
      * created via the
diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index bd05ab5..45338f4 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -2,36 +2,28 @@
 
 import java.util.List;
 
+import com.studica.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.path.PathPoint;
-import com.studica.frc.AHRS;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.*;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -40,13 +32,9 @@
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Constants.DriveConstants;
-import frc.robot.Constants.FieldConstants;
-import frc.robot.Constants.PathPlannerConstants;
-import frc.robot.Constants.PoseConstants;
-import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.LimelightHelpers;
 import frc.robot.Robot;
+import frc.robot.Constants.*;
 
 public class SwerveSubsystem extends SubsystemBase {
     SwerveModule frontLeft = new SwerveModule(SwerveModuleConstants.FL_STEER_ID, SwerveModuleConstants.FL_DRIVE_ID,
@@ -118,19 +106,13 @@ public SwerveSubsystem() {
         // zeroHeading()
 
         // --------- Path Planner Init ---------- \\
-                RobotConfig config = null;
-        try{
-        config = RobotConfig.fromGUISettings();
-        } catch (Exception e) {
-        // Handle exception as needed
-        e.printStackTrace();
-        } 
+
         AutoBuilder.configure(
                 this::getPose, // Robot pose supplier
                 this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
                 this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                //(speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
-                /*PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, // todo -> check above method^^^
+                (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+                PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER,
                 PathPlannerConstants.ROBOT_CONFIG,
                 () -> {
                     // Boolean supplier that controls when the path will be mirrored for the red
@@ -142,23 +124,7 @@ public SwerveSubsystem() {
                         return alliance.get() == DriverStation.Alliance.Red;
                     }
 
-                    return false; */
-                (speeds, feedforwards) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
-                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
-                    new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                    new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
-            ),
-            config, // The robot configuration
-            () -> {
-              // Boolean supplier that controls when the path will be mirrored for the red alliance
-              // This will flip the path being followed to the red side of the field.
-              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
-              var alliance = DriverStation.getAlliance();
-              if (alliance.isPresent()) {
-                return alliance.get() == DriverStation.Alliance.Red;
-              }
-              return false;
+                    return false;
 
                 },
                 this // Reference to this subsystem to set requirements
@@ -187,7 +153,7 @@ public void periodic() {
         // TODO: Test
         // WARNING: REMOVE IF USING TAG FOLLOW!!!
         // updateVisionOdometry();
- 
+
         if (DriverStation.isTeleopEnabled()) {
             updateMegaTagOdometry();
         } else {
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 03df051..1fa7c03 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.1.0",
+    "version": "4.0.0",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,22 +12,21 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "4.1.0"
+            "version": "4.0.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "4.1.0",
+            "version": "4.0.0",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [
                 "linuxathena",
+                "windowsx86-64",
                 "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal",
-                "windowsx86-64"
+                "osxuniversal"
             ]
         }
     ],
diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.1.1.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.2.json
rename to vendordeps/PathplannerLib-2025.1.1.json
index a5bf9ee..6322388 100644
--- a/vendordeps/PathplannerLib-2025.2.2.json
+++ b/vendordeps/PathplannerLib-2025.1.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.2.json",
+    "fileName": "PathplannerLib-2025.1.1.json",
     "name": "PathplannerLib",
-    "version": "2025.2.2",
+    "version": "2025.1.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.2"
+            "version": "2025.1.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.2",
+            "version": "2025.1.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.1.0.json
similarity index 85%
rename from vendordeps/Phoenix6-25.2.1.json
rename to vendordeps/Phoenix6-25.1.0.json
index 1397da1..473f6a8 100644
--- a/vendordeps/Phoenix6-25.2.1.json
+++ b/vendordeps/Phoenix6-25.1.0.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.1.json",
+    "fileName": "Phoenix6-25.1.0.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.1",
+    "version": "25.1.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.1"
+            "version": "25.1.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,21 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +196,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +212,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +228,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +244,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +260,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +276,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +292,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +308,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +324,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -351,26 +337,10 @@
             ],
             "simMode": "swsim"
         },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
-            "libName": "CTRE_SimProTalonFXS",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxarm64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +356,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +372,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.1.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib-2025.0.0.json
similarity index 86%
rename from vendordeps/REVLib-2025.0.2.json
rename to vendordeps/REVLib-2025.0.0.json
index c29aefa..cde6011 100644
--- a/vendordeps/REVLib-2025.0.2.json
+++ b/vendordeps/REVLib-2025.0.0.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.2.json",
+    "fileName": "REVLib-2025.0.0.json",
     "name": "REVLib",
-    "version": "2025.0.2",
+    "version": "2025.0.0",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,18 +12,19 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.2"
+            "version": "2025.0.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.0",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
+                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -36,13 +37,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.2",
+            "version": "2025.0.0",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
+                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -53,13 +55,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.0",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
+                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.0.json
similarity index 89%
rename from vendordeps/Studica-2025.0.1.json
rename to vendordeps/Studica-2025.0.0.json
index 5010be0..ddb0e44 100644
--- a/vendordeps/Studica-2025.0.1.json
+++ b/vendordeps/Studica-2025.0.0.json
@@ -1,13 +1,13 @@
 {
-    "fileName": "Studica-2025.0.1.json",
+    "fileName": "Studica-2025.0.0.json",
     "name": "Studica",
-    "version": "2025.0.1",
+    "version": "2025.0.0",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
     "frcYear": "2025",
     "mavenUrls": [
         "https://dev.studica.com/maven/release/2025/"
     ],
-    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
     "cppDependencies": [
         {
             "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
             "libName": "Studica",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.1"
+            "version": "2025.0.0"
         },
         {
             "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
             "libName": "StudicaDriver",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.1"
+            "version": "2025.0.0"
         }
     ],
     "javaDependencies": [
         {
             "artifactId": "Studica-java",
             "groupId": "com.studica.frc",
-            "version": "2025.0.1"
+            "version": "2025.0.0"
         }
     ],
     "jniDependencies": [
@@ -65,7 +65,7 @@
                 "osxuniversal",
                 "windowsx86-64"
             ],
-            "version": "2025.0.1"
+            "version": "2025.0.0"
         }
     ]
 }
\ No newline at end of file

From 4421a4fe6fc436648a868d94fc007eebb1a39152 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 9 Feb 2025 00:14:25 -0600
Subject: [PATCH 39/71] Add debugging, command based sequencing, update control
 loops, etc.

---
 simgui-ds.json                                |   5 +
 src/main/java/frc/robot/Constants.java        | 157 +++---
 src/main/java/frc/robot/RobotContainer.java   | 117 +++--
 .../commands/coral/IntakeCoralCommand.java    |   6 +-
 .../commands/coral/ScoreCoralCommand.java     |  15 +-
 .../commands/coral/motion/MoveElevator.java   |  24 +
 .../commands/coral/motion/MovePitch.java      |  25 +
 .../commands/coral/motion/MovePivot.java      |  24 +
 .../robot/commands/coral/motion/MoveRoll.java |  24 +
 .../robot/commands/coral/motion/StowArm.java  |  26 +
 .../coral/motion/WaitPivotClearance.java      |  19 +
 .../frc/robot/subsystems/coral/CoralArm.java  | 453 ++++++++++--------
 .../robot/subsystems/coral/CoralElevator.java |  98 ++--
 .../robot/subsystems/coral/CoralIntake.java   |  51 +-
 .../subsystems/coral/CoralSubsystem.java      | 100 +++-
 15 files changed, 743 insertions(+), 401 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/MovePitch.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/MovePivot.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/StowArm.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java

diff --git a/simgui-ds.json b/simgui-ds.json
index b48202b..aa26d60 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4696dfc..f17a201 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import java.util.PropertyResourceBundle;
+
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -42,10 +44,10 @@ public static class ControllerConstants {
   public static class RobotConstants {
     public static final double robotWidthMeters = Units.inchesToMeters(25.0);
     public static final double robotLengthMeters = Units.inchesToMeters(25.0);
-    
-      // TODO: ############## REPLACE PLACEHOLDERS ##############
-      public static final double TOTAL_MASS_KG = 10;
-      public static final double MOMENT_OF_INERTIA = 1;
+
+    // TODO: ############## REPLACE PLACEHOLDERS ##############
+    public static final double TOTAL_MASS_KG = 10;
+    public static final double MOMENT_OF_INERTIA = 1;
   }
 
   public static final class FieldConstants {
@@ -159,11 +161,10 @@ public static class CommonConstants {
   public static class Climber {
     public static final int MOTOR_PORT = 20;
     public static final ProfiledPIDController PID = new ProfiledPIDController(
-      1,
-      0.0,
-      0.0, 
-      new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
-    );
+        1,
+        0.0,
+        0.0,
+        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
@@ -172,43 +173,41 @@ public static class Pivot {
       public static final int MOTOR_PORT = 14;
       public static final int ENCODER_PORT = 28;
 
-      public static final double MAXIMUM_ANGLE = 80;
-      public static final double FRAME_BORDER_ANGLE = 30;
+      public static final double MAXIMUM_ANGLE = Units.degreesToRadians(80);
+      public static final double FRAME_BORDER_ANGLE = Units.degreesToRadians(30);
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-        1,
-        0.0,
-        0.0,
-        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
-      );
+          1,
+          0.0,
+          0.0,
+          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
-        0.0,
-        1.04,// V
-        1.62,// V*s/rad
-        0.05// V*s^2/rad
+          0.0,
+          0.75, // V
+          1.62, // V*s/rad
+          0.05// V*s^2/rad
       );
+
       public static class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
         public static final double NET_REDUCTION = 96.0;
         public static final double MASS_KG = 4.8;
         public static final double ARM_LENGTH_METERS = 0.51;
         public static final double MOI = 0.2875548495; // Kg*m^2
-        public static final double GEARING = 1; // TODO: this is the only placholder
       }
     }
 
     public static class Roll {
       public static final int MOTOR_PORT = 15;
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-        1,
-        0.0,
-        0.0,
-        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
-      );
-      public static final double MAXIMUM_ANGLE = 90;
+          1,
+          0.0,
+          0.0,
+          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+      public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
@@ -216,20 +215,19 @@ public static class PhysicalConstants {
         public static final double MASS_KG = 2.85; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.083;
         public static final double MOI = 0.0403605447; // Kg*m^2
-        public static final double GEARING = 1; // TODO: this is the only placholder
       }
     }
+
     public static class Pitch {
       public static final int MOTOR_PORT = 16;
 
-      public static final double MAXIMUM_ANGLE = 115;
+      public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-        1,
-        0.0,
-        0.0,
-        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
-      );
+          1,
+          0.0,
+          0.0,
+          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
@@ -237,19 +235,21 @@ public static class PhysicalConstants {
         public static final double MASS_KG = 2.16; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.101;
         public static final double MOI = 0.0200055915; // Kg*m^2
-        public static final double GEARING = 1; // TODO: this is the only placeholder
       }
     }
 
     public static class Intake {
       public static final int MOTOR_PORT = 17;
 
-      public static final double POSITIVE_RATE_LIMIT = 5.0;
-      public static final double NEGATIVE_RATE_LIMIT = 5.0;
+      public static final double POSITIVE_RATE_LIMIT = 20.0; // Fast shoot
+      public static final double NEGATIVE_RATE_LIMIT = 5.0; // Slow intake
+
+      public static final double SCORE_EXTRA_SECONDS = 0.1;
 
       public static final double IN_OUT_CURRENT_LIMIT = 40.0; // Stator limit
       public static final double HOLD_CURRENT_LIMIT = 5.0; // Stator, TODO: Test this!
       // TODO: ################### PLACHOLDERS ###################
+
       public static final class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getFalcon500(1);
         public static final double MOI = 0.1; // J*KG / M^2
@@ -257,7 +257,7 @@ public static final class PhysicalConstants {
       }
     }
   }
-  
+
   // TODO: ##################### PLACEHOLDERS #####################
   public static final class Algae {
     public static final class Pivot {
@@ -265,16 +265,14 @@ public static final class Pivot {
       public static final int ENCODER_PORT = 27;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-        1,
-        0.0,
-        0.0,
-        new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)
-      );
+          1,
+          0.0,
+          0.0,
+          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
-        1,
-        1,
-        1
-      );
+          1,
+          1,
+          1);
 
       public static final double RETRACTED_LIMIT_DEGREES = 10.0;
       public static final double EXTENDED_LIMIT_DEGREES = 90.0;
@@ -291,7 +289,8 @@ public static class PhysicalConstants {
 
     public static final class Intake {
       public static final int MOTOR_PORT = 19;
-      public static final int BEAMBREAK_PORT = 0; // NOTE: Beambreak will *probably* be a rockwell proximity sensor wired into the SPARK max
+      public static final int BEAMBREAK_PORT = 0; // NOTE: Beambreak will *probably* be a rockwell proximity sensor
+                                                  // wired into the SPARK max
 
       public static final double POSITIVE_RATE_LIMIT = 5.0;
       public static final double NEGATIVE_RATE_LIMIT = -5.0;
@@ -304,7 +303,8 @@ public static final class PhysicalConstants {
     }
   }
 
-  // See https://cad.onshape.com/documents/fa9a0365dfdf7e376f93f1b4/w/36bfb0cc9de95ef5933791e3/e/700ba3cf920578fe61d3ec24
+  // See
+  // https://cad.onshape.com/documents/fa9a0365dfdf7e376f93f1b4/w/36bfb0cc9de95ef5933791e3/e/700ba3cf920578fe61d3ec24
   public static final class Elevator {
     public static final class Leader {
       public static final int MOTOR_PORT = 10;
@@ -324,7 +324,8 @@ public static class PID {
       public static double kI = 0.0;
       public static double kD = 0.5;
       public static double MAX_VELOCITY = 3.20;
-      // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if the elevator can make or exceed this
+      // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
+      // the elevator can make or exceed this
       public static double MAX_ACCELERATION = 20.0;
     }
 
@@ -333,27 +334,29 @@ public static class FeedforwardConstants {
       public static double Ks = 0.0;
       public static double Kv = 3.5;
       public static double Ka = 0.08;
-      public static double Kg = 0.75; // TODO: Check this!!!
+      public static double Kg = 0.35; // TODO: Check this!!!
     }
 
     public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward(
-      FeedforwardConstants.Ks,
-      FeedforwardConstants.Kg,
-      FeedforwardConstants.Kv,
-      FeedforwardConstants.Ka
-    );
+        FeedforwardConstants.Ks,
+        FeedforwardConstants.Kg,
+        FeedforwardConstants.Kv,
+        FeedforwardConstants.Ka);
 
     public static class PhysicalParameters {
       public static final double GEARING = 5.0 / 2.0;
       public static final double DRIVE_RADIUS_METERS = 0.0121;
-      public static final double CARRIAGE_MASS_KG = 8.77; // NOTE: This includes the weight "reduction" due to CF spring counterbalance!
+      public static final double CARRIAGE_MASS_KG = 6.0; // Load on the SECOND stage NOTE: This includes the weight
+                                                         // "reduction" due to CF spring counterbalance!
 
       public static final double MAX_TRAVEL = Units.inchesToMeters(59.5);
       public static final double BOTTOM_TO_FLOOR = Units.inchesToMeters(3.0); // Relative to bottom of stage 2
       public static final double CARRIAGE_HEIGHT = Units.inchesToMeters(8.0); // Bottom to top of stage 2
-      
-      public static final double CORAL_PIVOT_VERTICAL_OFFSET = Units.inchesToMeters(6.0); // From bottom of stage 2 to coral arm pivot axis
-      public static final double CORAL_PIVOT_HORIZONTAL_OFFSET = Units.inchesToMeters(7.5); // From bottom of stage 2 to coral arm pivot axis
+
+      public static final double CORAL_PIVOT_VERTICAL_OFFSET = Units.inchesToMeters(6.0); // From bottom of stage 2 to
+                                                                                          // coral arm pivot axis
+      public static final double CORAL_PIVOT_HORIZONTAL_OFFSET = Units.inchesToMeters(7.5); // From bottom of stage 2 to
+                                                                                            // coral arm pivot axis
       public static final double ELEVATOR_FORWARDS_OFFSET = Units.inchesToMeters(1); // To mid-plane of elevator
 
       public static final DCMotor MOTOR = DCMotor.getNeoVortex(2);
@@ -365,26 +368,24 @@ public static final class PathPlannerConstants {
     public static final PIDConstants ROTATION_PID = new PIDConstants(5, 0, 0.2);
 
     public static final PPHolonomicDriveController HOLONOMIC_FOLLOWER_CONTROLLER = new PPHolonomicDriveController(
-      TRANSLATION_PID,
-      ROTATION_PID
-    );
+        TRANSLATION_PID,
+        ROTATION_PID);
 
     public static final RobotConfig ROBOT_CONFIG = new RobotConfig(
-      RobotConstants.TOTAL_MASS_KG,
-      RobotConstants.MOMENT_OF_INERTIA,
-      new ModuleConfig(
-        SwerveModuleConstants.WHEEL_DIAMETER/2,
-        DriveConstants.MAX_MODULE_VELOCITY,
-        SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
-        DCMotor.getKrakenX60(1),
-        DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
-        1
-      ),
-      new Translation2d(-0.5, 0.5),
-      new Translation2d(0.5, 0.5),
-      new Translation2d(-0.5, -0.5),
-      new Translation2d(0.5, -0.5)
-    );
+        RobotConstants.TOTAL_MASS_KG,
+        RobotConstants.MOMENT_OF_INERTIA,
+        new ModuleConfig(
+            SwerveModuleConstants.WHEEL_DIAMETER / 2,
+            DriveConstants.MAX_MODULE_VELOCITY,
+            SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS
+                                                              // ##############
+            DCMotor.getKrakenX60(1),
+            DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
+            1),
+        new Translation2d(-0.5, 0.5),
+        new Translation2d(0.5, 0.5),
+        new Translation2d(-0.5, -0.5),
+        new Translation2d(0.5, -0.5));
   }
 
   public static final class PoseConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e58b97c..22fb776 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -11,6 +11,12 @@
 import frc.robot.commands.coral.IntakeCoralCommand;
 import frc.robot.commands.coral.PurgeCoralIntakeCommand;
 import frc.robot.commands.coral.ScoreCoralCommand;
+import frc.robot.commands.coral.motion.MoveElevator;
+import frc.robot.commands.coral.motion.MovePitch;
+import frc.robot.commands.coral.motion.MovePivot;
+import frc.robot.commands.coral.motion.MoveRoll;
+import frc.robot.commands.coral.motion.StowArm;
+import frc.robot.commands.coral.motion.WaitPivotClearance;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
@@ -20,9 +26,11 @@
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -57,14 +65,11 @@ public class RobotContainer {
     private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID());
 
     private final CoralSubsystem coralSubsystem = new CoralSubsystem();
-    private CoralPresets currentCoralPreset = CoralPresets.STOW;
 
     private final AlgaeSubsystem algaeSubsystem = new AlgaeSubsystem();
 
     private final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
 
-
-
     /*
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -81,6 +86,41 @@ public RobotContainer() {
         swerveDriveSubsystem.setDefaultCommand(normalDrive);
     }
 
+    private CoralPresets selectedScoringPreset = CoralPresets.STOW;
+    private CoralPresets lockedPreset = CoralPresets.STOW;
+
+    private void lockCoralArmPreset(CoralPresets preset) {
+        lockedPreset = preset;
+        SmartDashboard.putString("Locked Coral Preset", preset.toString());
+    }
+
+    private Supplier<CoralPresets> currentLockedPresetSupplier = new Supplier<CoralSubsystem.CoralPresets>() {
+        public CoralPresets get() {
+            return lockedPreset;
+        };
+    };
+
+    // go to preset position:
+    // 1. Stow arm & wrist
+    // 2. Move elevator
+    // 3. Deploy arm
+    // 4. Rotate wrist
+    private Command getGoToLockedPresetCommand() {
+        return new StowArm(coralSubsystem)
+                .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
+                .andThen(new ParallelCommandGroup(
+                        new MovePivot(coralSubsystem, currentLockedPresetSupplier),
+                        new WaitPivotClearance(coralSubsystem)
+                                .andThen(new MoveRoll(coralSubsystem, currentLockedPresetSupplier)
+                                        .alongWith(new MovePitch(coralSubsystem, currentLockedPresetSupplier)))));
+    }
+
+    private Command getStowCommand() {
+        return new InstantCommand(() -> {
+            lockCoralArmPreset(CoralPresets.STOW);
+        }).andThen(getGoToLockedPresetCommand());
+    }
+
     /**
      * Use this method to define your trigger->command mappings. Triggers can be
      * created via the
@@ -98,62 +138,75 @@ public RobotContainer() {
     private void configureBindings() {
         /*
          * operator
-        */
-        // low algae
+         */
+        // low algae TODO: Make a preset for low reef algae
         operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
             algaeSubsystem.setAlgaePreset(AlgaePresets.FLOOR);
         }));
-        // high algae
+        // high algae TODO: Make a preset for high reef algae
         operatorXbox.rightBumper().onTrue(new InstantCommand(() -> {
             algaeSubsystem.setAlgaePreset(AlgaePresets.STOW);
         }));
 
-        // L1 
+        // L1
         operatorXbox.a().onTrue(new InstantCommand(() -> {
-            currentCoralPreset = CoralPresets.LEVEL_1;
+            selectedScoringPreset = CoralPresets.LEVEL_1;
         }));
         // L2
         operatorXbox.x().onTrue(new InstantCommand(() -> {
-            currentCoralPreset = CoralPresets.LEVEL_2;
+            selectedScoringPreset = CoralPresets.LEVEL_2;
         }));
         // L3
         operatorXbox.y().onTrue(new InstantCommand(() -> {
-            currentCoralPreset = CoralPresets.LEVEL_3;
+            selectedScoringPreset = CoralPresets.LEVEL_3;
         }));
         // L4
         operatorXbox.b().onTrue(new InstantCommand(() -> {
-            currentCoralPreset = CoralPresets.LEVEL_4;
-        }));
-        // go to preset position
-        operatorXbox.rightTrigger().onTrue(new InstantCommand(() -> {
-            coralSubsystem.setCoralPreset(currentCoralPreset);
+            selectedScoringPreset = CoralPresets.LEVEL_4;
         }));
+
+        operatorXbox.rightTrigger().whileTrue(new InstantCommand(() -> {
+            // coralSubsystem.setCoralPreset(currentCoralPreset);
+            lockCoralArmPreset(selectedScoringPreset);
+        }).andThen(getGoToLockedPresetCommand())).whileFalse(getStowCommand());
+
         // wrist adjustment
-        operatorXbox.rightStick().and(new BooleanSupplier() {
-            // deadzone
-            @Override
-            public boolean getAsBoolean() {
-                return Math.sqrt(Math.pow(operatorXbox.getRightX(),2) + Math.pow(operatorXbox.getRightY(),2)) > 0.25; 
-            }
-        }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
-        // score / intake coral
+        // Hold for now, until everything else is working
+        // operatorXbox.rightStick().and(new BooleanSupplier() {
+        // // deadzone
+        // @Override
+        // public boolean getAsBoolean() {
+        // return Math.sqrt(Math.pow(operatorXbox.getRightX(), 2) +
+        // Math.pow(operatorXbox.getRightY(), 2)) > 0.25;
+        // }
+        // }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
+
+        // Score coral
+        // TODO: Is it a good idea to stow right after this?
         operatorXbox.rightBumper().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
                 return coralSubsystem.isHolding();
             }
-        }).whileTrue(new ScoreCoralCommand(coralSubsystem));
+        }).whileTrue(new ScoreCoralCommand(coralSubsystem).andThen(getStowCommand())).whileFalse(getStowCommand());
+
+        // Intake coral
         operatorXbox.rightBumper().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
                 return !coralSubsystem.isHolding();
             }
-        }).whileTrue(new IntakeCoralCommand(coralSubsystem));
+        }).whileTrue(new InstantCommand(() -> {
+            lockCoralArmPreset(CoralPresets.INTAKE);
+        }).andThen(getGoToLockedPresetCommand()).andThen(getStowCommand())).whileFalse(getStowCommand());
+        // .onFalse(normalDrive);
+
         // purge coral
         operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
+
         /*
          * driver
-        */
+         */
         // stop the climber
         driverXbox.x().onTrue(new InstantCommand(() -> {
             climberSubsystem.setOutput(0);
@@ -161,6 +214,7 @@ public boolean getAsBoolean() {
         // move the climber
         driverXbox.y().and(new BooleanSupplier() {
             private boolean deployed = true;
+
             @Override
             public boolean getAsBoolean() {
                 deployed = !(deployed);
@@ -173,18 +227,17 @@ public boolean getAsBoolean() {
         }));
         // TODO: something something maintainence
         driverXbox.button(6).onTrue(new SequentialCommandGroup(
-            new InstantCommand(() -> {
-                System.out.print("swaaws");
-            })
-        ));
+                new InstantCommand(() -> {
+                    System.out.print("swaaws");
+                })));
         // set field orientation
         driverXbox.button(7).onTrue(new InstantCommand(() -> {
             swerveDriveSubsystem.setHeading(0);
         }));
 
-        /* 
+        /*
          * coop
-        */
+         */
         // algae floor / shoot
         driverXbox.leftBumper().and(new BooleanSupplier() {
             @Override
diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
index db01362..f8bc571 100644
--- a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
@@ -4,12 +4,12 @@
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
 
-public class IntakeCoralCommand extends Command{
+public class IntakeCoralCommand extends Command {
     private final CoralSubsystem subsystem;
 
     public IntakeCoralCommand(CoralSubsystem subsystem) {
         this.subsystem = subsystem;
-        addRequirements(subsystem);
+        // addRequirements(subsystem);
     }
 
     @Override
@@ -20,7 +20,7 @@ public void initialize() {
     @Override
     public void end(boolean interrupted) {
         // if intaking failed
-        if (!subsystem.isHolding()) {
+        if (!subsystem.isHolding() || interrupted) {
             subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
         }
         // else continue to hold the piece
diff --git a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
index ffcbb44..e8728c7 100644
--- a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
@@ -1,16 +1,19 @@
 package frc.robot.commands.coral;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
 
-public class ScoreCoralCommand extends Command{
+public class ScoreCoralCommand extends Command {
 
     private final CoralSubsystem subsystem;
-   
+    private double coralExitTime = 0;
+
     public ScoreCoralCommand(CoralSubsystem subsystem) {
         this.subsystem = subsystem;
-        addRequirements(subsystem);
+        // addRequirements(subsystem);
     }
 
     @Override
@@ -25,7 +28,11 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return !subsystem.isHolding();
+        if (!subsystem.isHolding() && coralExitTime == 0) {
+            coralExitTime = Timer.getFPGATimestamp();
+        }
+        return !subsystem.isHolding()
+                && ((Timer.getFPGATimestamp() - coralExitTime) > Constants.Coral.Intake.SCORE_EXTRA_SECONDS);
     }
 
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
new file mode 100644
index 0000000..2cf037d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
@@ -0,0 +1,24 @@
+package frc.robot.commands.coral.motion;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class MoveElevator extends Command {
+    private CoralSubsystem coralSub;
+
+    public MoveElevator(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
+        this.coralSub = coralSub;
+        addRequirements(coralSub);
+        SmartDashboard.putString("Arm Sequence", "Moving Elevator");
+        coralSub.setCoralPresetElevator(presetSupplier.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralSub.isElevatorInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
new file mode 100644
index 0000000..d8f2600
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
@@ -0,0 +1,25 @@
+package frc.robot.commands.coral.motion;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class MovePitch extends Command {
+    private CoralSubsystem coralSub;
+
+    public MovePitch(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
+        this.coralSub = coralSub;
+        // HACK: This is much sketch
+        // addRequirements(coralSub);
+        SmartDashboard.putString("Arm Sequence", "Moving Pitch");
+        coralSub.setCoralPresetPitch(presetSupplier.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralSub.isPitchInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
new file mode 100644
index 0000000..c86cd74
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -0,0 +1,24 @@
+package frc.robot.commands.coral.motion;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class MovePivot extends Command {
+    private CoralSubsystem coralSub;
+
+    public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
+        this.coralSub = coralSub;
+        addRequirements(coralSub);
+        SmartDashboard.putString("Arm Sequence", "Moving Pivot");
+        coralSub.setCoralPresetPivot(presetSupplier.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralSub.isPivotInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
new file mode 100644
index 0000000..59d0c61
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
@@ -0,0 +1,24 @@
+package frc.robot.commands.coral.motion;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class MoveRoll extends Command {
+    private CoralSubsystem coralSub;
+
+    public MoveRoll(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
+        this.coralSub = coralSub;
+        // addRequirements(coralSub);
+        SmartDashboard.putString("Arm Sequence", "Moving Roll");
+        coralSub.setCoralPresetRoll(presetSupplier.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralSub.isRollInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
new file mode 100644
index 0000000..4d78928
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
@@ -0,0 +1,26 @@
+package frc.robot.commands.coral.motion;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class StowArm extends Command {
+    private CoralSubsystem coralSub;
+
+    public StowArm(CoralSubsystem coralSub) {
+        this.coralSub = coralSub;
+        addRequirements(coralSub);
+        SmartDashboard.putString("Arm Sequence", "Stowing Arm");
+        coralSub.setCoralPresetPitch(CoralPresets.STOW);
+        coralSub.setCoralPresetRoll(CoralPresets.STOW);
+        coralSub.setCoralPresetPivot(CoralPresets.STOW);
+    }
+
+    @Override
+    public boolean isFinished() {
+        // Intentionally not worrying about pitch because it doesn't really matter
+        // anyways
+        return coralSub.isRollInPosition() && coralSub.isPivotInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java b/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java
new file mode 100644
index 0000000..341b80a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java
@@ -0,0 +1,19 @@
+package frc.robot.commands.coral.motion;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.coral.CoralSubsystem;
+
+public class WaitPivotClearance extends Command {
+    private CoralSubsystem coralSub;
+
+    public WaitPivotClearance(CoralSubsystem coralSub) {
+        this.coralSub = coralSub;
+        // addRequirements(coralSub);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(coralSub.getPivotPositionDegrees()) > Constants.Coral.Pivot.FRAME_BORDER_ANGLE;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index d48222e..dcc9cc2 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -10,219 +10,280 @@
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.Constants.Coral;
 
-public class CoralArm extends SubsystemBase{
-    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-    // sim motors
-    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
-
-    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-    private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
-    private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
-    // sim encoders
-    private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
-    private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
-
-    // physics simulations
-    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-        Coral.Pivot.PhysicalConstants.MOTOR,
-        Coral.Pivot.PhysicalConstants.GEARING,
-        Coral.Pivot.PhysicalConstants.MOI,
-        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE * -1),
-        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE),
-        true,
-        0
-    );
-    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-        Coral.Roll.PhysicalConstants.MOTOR,
-        Coral.Roll.PhysicalConstants.GEARING,
-        Coral.Roll.PhysicalConstants.MOI,
-        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE * -1),
-        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE),
-        false,
-        0
-    );
-    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-        Coral.Pitch.PhysicalConstants.MOTOR,
-        Coral.Pitch.PhysicalConstants.GEARING,
-        Coral.Pitch.PhysicalConstants.MOI,
-        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE * -1),
-        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE),
-        false,
-        0
-    );
-
-    private final SparkMaxConfig pivotConfig = new SparkMaxConfig();
-    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-    private final ProfiledPIDController rollPID = Coral.Roll.PID;
-    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-    
-
-    private double pivotGoal = 0.0;
-    private double rollGoal = 0.0;
-    private double pitchGoal = 0.0;
-
-    public CoralArm() {
-        pivotConfig.idleMode(IdleMode.kBrake);
-        rollConfig.idleMode(IdleMode.kBrake);
-        pitchConfig.idleMode(IdleMode.kBrake);
-
-        pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        rollMotor.configure(rollConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        pitchMotor.configure(pitchConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        rollPID.enableContinuousInput(-180, 180);
-    }
-
-    @Override
-    public void periodic() {
-        // if entering the frame border 
-        // && wrist is at neutral
-        if (
-            (pivotPID.getGoal().position != pivotGoal)
-            && (rollPID.atGoal() && pitchPID.atGoal())
-        ) {
-            // allow arm to enter frame border
-            pivotPID.setGoal(pivotGoal);
-        } else if ( // if exiting the frame border && has exited the frame border
-            ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-            && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) 
-        ) {
-            // set roll and pitch back to their goals
-            rollPID.setGoal(rollGoal);
-            pitchPID.setGoal(pitchGoal);
+public class CoralArm extends SubsystemBase {
+        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+        // sim motors
+        private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
+
+        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+        private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
+        private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
+        // sim encoders
+        private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
+        private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
+
+        // physics simulations
+        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+                        Coral.Pivot.PhysicalConstants.MOTOR,
+                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pivot.PhysicalConstants.MOI,
+                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+                        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE * -1),
+                        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE),
+                        true,
+                        0);
+        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+                        Coral.Roll.PhysicalConstants.MOTOR,
+                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
+                        Coral.Roll.PhysicalConstants.MOI,
+                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+                        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE * -1),
+                        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE),
+                        false,
+                        0);
+        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+                        Coral.Pitch.PhysicalConstants.MOTOR,
+                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pitch.PhysicalConstants.MOI,
+                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+                        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE * -1),
+                        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE),
+                        false,
+                        0);
+
+        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+        private final ProfiledPIDController rollPID = Coral.Roll.PID;
+        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+        private double pivotGoal = 0.0; // Rads
+        private double rollGoal = 0.0; // Rads
+        private double pitchGoal = 0.0; // Rads
+
+        public CoralArm() {
+
+                pivotMotor.configure(
+                                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                                                new EncoderConfig()
+                                                                .positionConversionFactor(
+                                                                                (1.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                                                .velocityConversionFactor(
+                                                                                (60.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                                                                           // ->
+                                // Rotations &
+                                // Seconds
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                rollMotor.configure(
+                                rollConfig.idleMode(IdleMode.kBrake),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                pitchMotor.configure(pitchConfig
+                                .idleMode(IdleMode.kBrake),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+                /*
+                 * .apply(new EncoderConfig()
+                 * .positionConversionFactor(
+                 * (1.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+                 */
+                // Not sure how to get
+
+                // Disabling this for now because we don't want the wrist wire bundle to explode
+                // rollPID.enableContinuousInput(-180, 180);
+
+                // TODO: Are these reasonable? who knows.
+                pivotPID.setTolerance(Units.degreesToRadians(0.5));
+                rollPID.setTolerance(Units.degreesToRadians(0.5));
+                pitchPID.setTolerance(Units.degreesToRadians(0.5));
         }
 
-        // run the motors
-        pivotMotor.set(
-            pivotPID.calculate(pivotEncoder.getAbsolutePosition().getValueAsDouble())
-            + pivotFeedforward.calculate(Units.degreesToRadians(pivotPID.getGoal().position), 0.0)
-        );
-        rollMotor.set(rollPID.calculate(rollEncoder.getPosition()));
-        pitchMotor.set(pitchPID.calculate(pitchEncoder.getPosition()));
-    }
-    
-    @Override
-    public void simulationPeriodic() {
-        // update physics
-        simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
-        simRollPhysics.setInput(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
-        simPitchPhysics.setInput(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
-
-        simPivotPhysics.update(0.02);
-        simRollPhysics.update(0.02);
-        simPitchPhysics.update(0.02);
-
-        // update sim objects
-        simPivotMotor.iterate(
-            Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
-        simRollMotor.iterate(
-            Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
-        simPitchMotor.iterate(
-            Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
-    }
-
-    // this should be relative to straight upwards.
-    // i.e. 0 should be straight vertical,
-    // 20 would be to the right, -20 to the left
-    public void setPivotGoalDegrees(double goal) {
-        pivotGoal = MathUtil.clamp(goal, Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE);
-        // if passing through the frame border
-        if (
-            ((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0)) 
-            && (Math.abs(this.getPivotPositionDegrees()) < Coral.Pivot.FRAME_BORDER_ANGLE)
-        ) {
-            // set the pivot to stop at the frame border to allow the wrist to move neutral
-            pivotPID.setGoal(
-                Coral.Pivot.FRAME_BORDER_ANGLE
-                * (pivotGoal < 0 ? -1 : 1)
-            );
-            rollPID.setGoal(0);
-            pitchPID.setGoal(0);
-        } else {
-            pivotPID.setGoal(pivotGoal);
+        @Override
+        public void periodic() {
+                // if entering the frame border
+                // && wrist is at neutral
+                if ((pivotPID.getGoal().position != pivotGoal)
+                                && (rollPID.atGoal() && pitchPID.atGoal())) {
+                        // allow arm to enter frame border
+                        pivotPID.setGoal(pivotGoal);
+                } else if ( // if exiting the frame border && has exited the frame border
+                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
+                        // set roll and pitch back to their goals
+                        rollPID.setGoal(rollGoal);
+                        pitchPID.setGoal(pitchGoal);
+                }
+
+                double pivotPosition = Units
+                                .rotationsToRadians(pivotEncoder.getAbsolutePosition()
+                                                .getValueAsDouble());
+                double pivotFFout = pivotFeedforward.calculate(pivotPID.getSetpoint().position,
+                                pivotPID.getSetpoint().velocity);
+                double pivotPIDout = pivotPID.calculate(pivotPosition);
+
+                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+
+                // run the motors
+                pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+
+                double rollPIDout = rollPID.calculate(rollEncoder.getPosition());
+                SmartDashboard.putNumber("Coral/Roll/position", rollEncoder.getPosition());
+                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout);
+                rollMotor.setVoltage(rollPIDout);
+
+                double pitchPIDout = pitchPID.calculate(pitchEncoder.getPosition());
+                SmartDashboard.putNumber("Coral/Pitch/position", pitchEncoder.getPosition());
+                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout);
+                pitchMotor.setVoltage(pitchPIDout);
         }
-    }
-
-    // similarly, this ranges from -180 to 180
-    public void setRollGoalDegrees(double goal) {
-        rollGoal = MathUtil.clamp(goal, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-        // if outside of frame border
-        if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-            // start moving to goal
-            rollPID.setGoal(rollGoal);
+
+        @Override
+        public void simulationPeriodic() {
+                // update physics
+                simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+                simRollPhysics.setInput(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+                simPitchPhysics.setInput(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+
+                simPivotPhysics.update(0.02);
+                simRollPhysics.update(0.02);
+                simPitchPhysics.update(0.02);
+
+                // update sim objects
+                simPivotMotor.iterate(
+                                simPivotPhysics.getVelocityRadPerSec(),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simRollMotor.iterate(
+                                simRollPhysics.getVelocityRadPerSec(),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simPitchMotor.iterate(
+                                simPitchPhysics.getVelocityRadPerSec(),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
         }
-    }
-
-    public void setPitchGoalDegrees(double goal) {
-        pitchGoal = MathUtil.clamp(goal, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-        // if outside the frame border
-        if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-            // start moving to goal
-            pitchPID.setGoal(pitchGoal);
+
+        // this should be relative to straight upwards.
+        // i.e. 0 should be straight vertical,
+        // 20 would be to the right, -20 to the left
+        public void setPivotGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pivotGoal = MathUtil.clamp(goalRads,
+                                Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE);
+                // if passing through the frame border
+                if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+                                && (Math.abs(this.getPivotPositionDegrees()) < Coral.Pivot.FRAME_BORDER_ANGLE)) {
+                        // set the pivot to stop at the frame border to allow the wrist to move
+                        // neutral
+                        pivotPID.setGoal(
+                                        Coral.Pivot.FRAME_BORDER_ANGLE
+                                                        * (pivotGoal < 0 ? -1 : 1));
+                        rollPID.setGoal(0);
+                        pitchPID.setGoal(0);
+                } else {
+                        pivotPID.setGoal(pivotGoal);
+                }
         }
-    }
-
-    public double getPivotGoalDegrees() {
-        return pivotGoal;
-    }
-
-    public double getRollGoalDegrees() {
-        return rollGoal;
-    }
-
-    public double getPitchGoalDegrees() {
-        return pitchGoal;
-    }
-
-    public double getPivotPositionDegrees() {
-        return Robot.isReal() 
-            ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
-            : Units.radiansToDegrees(simPivotPhysics.getAngleRads());
-    }
-
-    public double getRollPositionDegrees() {
-        return Robot.isReal()
-            ? rollEncoder.getPosition()
-            : simRollEncoder.getPosition();
-    }
-
-    public double getPitchPositionDegrees() {
-        return Robot.isReal() 
-            ? pitchEncoder.getPosition()
-            : simPitchEncoder.getPosition();
-    }
 
+        // similarly, this ranges from -180 to 180
+        public void setRollGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+                // if outside of frame border
+                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
+                        // start moving to goal
+                        rollPID.setGoal(rollGoal);
+                }
+
+        }
+
+        public void setPitchGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+                // if outside the frame border
+                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
+                        // start moving to goal
+                        pitchPID.setGoal(pitchGoal);
+                }
+        }
+
+        public double getPivotGoalDegrees() {
+                return Units.radiansToDegrees(pivotGoal);
+        }
+
+        public double getRollGoalDegrees() {
+                return Units.radiansToDegrees(rollGoal);
+        }
+
+        public double getPitchGoalDegrees() {
+                return Units.radiansToDegrees(pitchGoal);
+        }
+
+        public double getPivotPositionDegrees() {
+                return Units.radiansToDegrees(Robot.isReal()
+                                ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                                : simPivotPhysics.getAngleRads());
+        }
+
+        public double getRollPositionDegrees() {
+                return Units.radiansToDegrees(Robot.isReal()
+                                ? rollEncoder.getPosition()
+                                : simRollEncoder.getPosition());
+        }
+
+        public double getPitchPositionDegrees() {
+                return Units.radiansToDegrees(Robot.isReal()
+                                ? pitchEncoder.getPosition()
+                                : simPitchEncoder.getPosition());
+        }
+
+        public boolean isPitchInPosition() {
+                return pitchPID.atGoal();
+        }
+
+        public boolean isRollInPosition() {
+                return rollPID.atGoal();
+        }
+
+        public boolean isPivotInPosition() {
+                return pivotPID.atGoal();
+        }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index f68a293..cacf26c 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -10,18 +10,23 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkFlexConfig;
 
+import edu.wpi.first.epilogue.Epilogue;
+import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Robot;
 import frc.robot.Constants.Elevator;
 
+@Logged
 public class CoralElevator extends SubsystemBase {
 
     private final SparkFlex leaderMotor = new SparkFlex(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless);
@@ -37,53 +42,51 @@ public class CoralElevator extends SubsystemBase {
 
     // physics simulations
     private final ElevatorSim simElevator = new ElevatorSim(
-        Elevator.PhysicalParameters.MOTOR,
-        Elevator.PhysicalParameters.GEARING,
-        Elevator.PhysicalParameters.CARRIAGE_MASS_KG,
-        Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
-        0,
-        Elevator.PhysicalParameters.MAX_TRAVEL,
-        true,
-        0
-    );
+            Elevator.PhysicalParameters.MOTOR,
+            Elevator.PhysicalParameters.GEARING,
+            Elevator.PhysicalParameters.CARRIAGE_MASS_KG,
+            Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
+            0,
+            Elevator.PhysicalParameters.MAX_TRAVEL,
+            true,
+            0);
 
     private final SparkFlexConfig leaderConfig = new SparkFlexConfig();
     private final SparkFlexConfig followerConfig = new SparkFlexConfig();
 
-
     private final ElevatorFeedforward feedForward = Elevator.FEEDFORWARD;
     private final ProfiledPIDController elevatorPID = new ProfiledPIDController(
-        Elevator.PID.kP,
-        Elevator.PID.kI,
-        Elevator.PID.kD,
-        new TrapezoidProfile.Constraints(
-            Elevator.PID.MAX_VELOCITY, 
-            Elevator.PID.MAX_ACCELERATION
-        )
-    );
+            Elevator.PID.kP,
+            Elevator.PID.kI,
+            Elevator.PID.kD,
+            new TrapezoidProfile.Constraints(
+                    Elevator.PID.MAX_VELOCITY,
+                    Elevator.PID.MAX_ACCELERATION));
 
     public CoralElevator() {
         leaderConfig
-            .idleMode(IdleMode.kBrake)
-            .inverted(Elevator.Leader.INVERTED);  
+                .idleMode(IdleMode.kBrake)
+                .inverted(Elevator.Leader.INVERTED);
         leaderConfig.encoder
-            .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
-            .velocityConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER);
-            
+                .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
+                .velocityConversionFactor(60.0 / Elevator.MOTOR_REVOLUTIONS_PER_METER); // Multiply by 60/RpM to get m/s
+
         followerConfig
-            .idleMode(IdleMode.kBrake)
-            .inverted(Elevator.Follower.INVERTED);
+                .idleMode(IdleMode.kBrake)
+                .inverted(Elevator.Follower.INVERTED);
         followerConfig.encoder
-            .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
-            .velocityConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER);
+                .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER)
+                .velocityConversionFactor(60.0 / Elevator.MOTOR_REVOLUTIONS_PER_METER);
 
         leaderMotor.configure(leaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
         leaderEncoder.setPosition(0);
         followerEncoder.setPosition(0);
-    }
 
+        // Add some tolerance to the elevator controller
+        elevatorPID.setTolerance(Units.inchesToMeters(0.5));
+    }
 
     private boolean isZeroed = false;
 
@@ -93,8 +96,9 @@ public CoralElevator() {
     @Override
     public void periodic() {
         // check if needs to be zeroed and is at zero
-                                // TODO: ######################### PLACEHOLDERS AGAIN #########################
-        if (!isZeroed && (leaderMotor.getOutputCurrent() > 2 || followerMotor.getOutputCurrent() > 2)) {
+        // TODO: ######################### PLACEHOLDERS AGAIN #########################
+        if (!isZeroed && (leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2 > 20
+                || Robot.isSimulation()) {
             leaderEncoder.setPosition(0);
             isZeroed = true;
         }
@@ -108,8 +112,15 @@ public void periodic() {
             lastTime = Timer.getFPGATimestamp();
             lastVelocity = setpoint.velocity;
 
+            double pid_out = elevatorPID.calculate(leaderEncoder.getPosition());
             double output = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime)
-                + elevatorPID.calculate(leaderEncoder.getPosition());
+                    + pid_out;
+
+            SmartDashboard.putNumber("Elevator/Output", output);
+            SmartDashboard.putNumber("Elevator/position", leaderEncoder.getPosition());
+            SmartDashboard.putNumber("Elevator/target", setpoint.position);
+            SmartDashboard.putNumber("Elevator/goal", elevatorPID.getGoal().position);
+            SmartDashboard.putNumber("Elevator/pid_out", pid_out);
 
             leaderMotor.setVoltage(output);
             followerMotor.setVoltage(output);
@@ -124,35 +135,32 @@ public void periodic() {
     public void simulationPeriodic() {
         // update physics
         simElevator.setInput(
-            (leaderMotor.getAppliedOutput() + followerMotor.getAppliedOutput()) * RoboRioSim.getVInVoltage()
-        );
+                (leaderMotor.getAppliedOutput() + followerMotor.getAppliedOutput()) * 0.5 * RoboRioSim.getVInVoltage());
         simElevator.update(0.02);
 
         // update sim objects
         simLeaderMotor.iterate(
-            simElevator.getVelocityMetersPerSecond() / Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
+                simElevator.getVelocityMetersPerSecond(),
+                RoboRioSim.getVInVoltage(),
+                0.02);
         simFollowerMotor.iterate(
-            simElevator.getVelocityMetersPerSecond() / Elevator.PhysicalParameters.DRIVE_RADIUS_METERS,
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
+                simElevator.getVelocityMetersPerSecond(),
+                RoboRioSim.getVInVoltage(),
+                0.02);
     }
 
     public void setGoalPosition(double position) {
         elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.MAX_TRAVEL));
     }
-    
+
     public double getGoalPosition() {
         return elevatorPID.getGoal().position;
     }
 
     public double getPosition() {
-        return Robot.isReal() 
-            ? leaderEncoder.getPosition()
-            : simLeaderEncoder.getPosition();
+        return Robot.isReal()
+                ? leaderEncoder.getPosition()
+                : simLeaderEncoder.getPosition();
     }
 
     public boolean isInPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 24f2513..fb43908 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.coral;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
@@ -10,6 +11,7 @@
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.Constants.Coral;
 
@@ -20,50 +22,59 @@ public class CoralIntake extends SubsystemBase {
 
     // physics simulation
     private final FlywheelSim simIntakePhysics = new FlywheelSim(
-        LinearSystemId.createFlywheelSystem(
-            Coral.Intake.PhysicalConstants.MOTOR,
-            Coral.Intake.PhysicalConstants.MOI,
-            Coral.Intake.PhysicalConstants.GEARING
-        ),
-        Coral.Intake.PhysicalConstants.MOTOR
-    );
- 
+            LinearSystemId.createFlywheelSystem(
+                    Coral.Intake.PhysicalConstants.MOTOR,
+                    Coral.Intake.PhysicalConstants.MOI,
+                    Coral.Intake.PhysicalConstants.GEARING),
+            Coral.Intake.PhysicalConstants.MOTOR);
+
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
-        Coral.Intake.POSITIVE_RATE_LIMIT,
-        Coral.Intake.NEGATIVE_RATE_LIMIT,
-        0
-    );
+            Coral.Intake.POSITIVE_RATE_LIMIT,
+            -Coral.Intake.NEGATIVE_RATE_LIMIT,
+            0);
 
     public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
+        intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(20.0));
     }
 
     private double outputPercentage = 0.0;
+    private boolean lastHolding = false;
 
     @Override
     public void periodic() {
-        // if no coral 
-        if (this.isHolding()) {
+        boolean curHolding = isHolding();
+        // if no coral
+        if (curHolding) {
             double output = intakeProfile.calculate(outputPercentage);
             intakeMotor.set(output);
         } else {
-            // hold
-            intakeMotor.set(Math.min(0.05, outputPercentage));
+            // hold, negative is out so intake a bit.
+            // Set to 0.1 to be good
+            intakeMotor.set(Math.min(0.1, outputPercentage));
         }
 
+        if (lastHolding != curHolding) {
+            intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(
+                    (curHolding && (outputPercentage > 0.0)) ? Constants.Coral.Intake.HOLD_CURRENT_LIMIT
+                            : Constants.Coral.Intake.IN_OUT_CURRENT_LIMIT));
+
+        }
+
+        lastHolding = curHolding;
     }
 
     @Override
     public void simulationPeriodic() {
         simIntakeMotor = intakeMotor.getSimState();
         simIntakeMotor.setSupplyVoltage(RoboRioSim.getVInVoltage());
-        
+
         // update physics
         simIntakePhysics.setInput(simIntakeMotor.getMotorVoltage() * RoboRioSim.getVInVoltage());
         simIntakePhysics.update(0.02);
 
         // update sim objects
-        //                              rpm to rps
+        // rpm to rps
         simIntakeMotor.setRotorVelocity(simIntakePhysics.getAngularVelocityRPM() / 60);
     }
 
@@ -77,7 +88,7 @@ public double getOutputPercentage() {
 
     public boolean isHolding() {
         return Robot.isReal()
-            ? intakeMotor.getReverseLimit().getValue().value == 0
-            : false;
+                ? intakeMotor.getReverseLimit().getValue().value == 0
+                : false;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index d694fcf..40f46f0 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -1,5 +1,8 @@
 package frc.robot.subsystems.coral;
 
+// import edu.wpi.first.epilogue.Epilogue;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -12,14 +15,12 @@ public class CoralSubsystem extends SubsystemBase {
     private final CoralIntake intake = new CoralIntake();
     private final CoralElevator elevator = new CoralElevator();
 
-    private final Mechanism2d coralMechanism = new Mechanism2d(2,3);
+    private final Mechanism2d coralMechanism = new Mechanism2d(2, 3);
     private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.5, 0);
     private final MechanismLigament2d elevatorMechanism = rootMechanism.append(
-        new MechanismLigament2d("Elevator", 1, 0)
-    );
+            new MechanismLigament2d("Elevator", 1, 0));
     private final MechanismLigament2d pivotMechanism = elevatorMechanism.append(
-        new MechanismLigament2d("Coral", 1, 0)
-    );
+            new MechanismLigament2d("Coral", 1, 0));
 
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
@@ -31,19 +32,25 @@ public enum CoralPresets {
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 
-        double elevatorHeight; // Elevator height (relative to bottom of elevator/fully retracted)
-        double pivotAngle; // Looking at the robot from the FRONT (algae intake side), positive to the right, and negative to the left (positive=CW)
-        double rollAngle; // Wrist 1 angle, degrees from pointing at the bumpers on the CORAL ARM side of the robot. positive=CCW
-        double pitchAngle; // Wrist 2 angle, degrees from pointing straight up (max: 115deg)
+        double elevatorHeightM; // Elevator height (relative to bottom of elevator/fully retracted)
+        double pivotAngleDeg; // Looking at the robot from the FRONT (algae intake side), positive to the
+                              // right, and negative to the left (positive=CW)
+        double rollAngleDeg; // Wrist 1 angle, degrees from pointing at the bumpers on the CORAL ARM side of
+                             // the robot. positive=CCW
+        double pitchAngleDeg; // Wrist 2 angle, degrees from pointing straight up (max: 115deg)
 
         private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
-            this.elevatorHeight = elevatorHeight;
-            this.pivotAngle = pivotAngle;
-            this.rollAngle = rollAngle;
-            this.pitchAngle = pitchAngle;
+            this.elevatorHeightM = elevatorHeight;
+            this.pivotAngleDeg = pivotAngle;
+            this.rollAngleDeg = rollAngle;
+            this.pitchAngleDeg = pitchAngle;
         }
     }
 
+    public CoralSubsystem() {
+        // Epilogue.bind(this);
+    }
+
     public enum MirrorPresets {
         RIGHT(false),
         LEFT(true),
@@ -51,13 +58,15 @@ public enum MirrorPresets {
         PORT(true);
 
         boolean isMirrored;
+
         private MirrorPresets(boolean isMirrored) {
             this.isMirrored = isMirrored;
         }
-    } 
+    }
 
     public enum CoralIntakePresets {
         INTAKE(1),
+        HOLD(0.1),
         PURGE(-1),
         SCORE(-1),
         STOP(0),
@@ -65,6 +74,7 @@ public enum CoralIntakePresets {
         CUSTOM(Double.NaN);
 
         double intakePercentage;
+
         private CoralIntakePresets(double intakePercentage) {
             this.intakePercentage = intakePercentage;
         }
@@ -83,22 +93,63 @@ public void periodic() {
     private MirrorPresets mirrorSetting = MirrorPresets.RIGHT;
     private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP;
 
-    public void setCoralPreset(CoralPresets preset) {
+    public void setCoralPresetDIRECT(CoralPresets preset) {
         if (preset == CoralPresets.CUSTOM) {
-            // uhhh i don't now how to throw an exception and i don't feel like figuring it out
+            // uhhh i don't now how to throw an exception and i don't feel like figuring it
+            // out
         } else if (preset != currentPreset) {
-            elevator.setGoalPosition(preset.elevatorHeight);
+            elevator.setGoalPosition(preset.elevatorHeightM);
             arm.setPivotGoalDegrees(
-                preset.pivotAngle 
-                * (mirrorSetting.isMirrored ? -1 : 1)
-            );
-            arm.setRollGoalDegrees(preset.rollAngle);
-            arm.setPitchGoalDegrees(preset.pitchAngle);
+                    preset.pivotAngleDeg
+                            * (mirrorSetting.isMirrored ? -1 : 1));
+            arm.setRollGoalDegrees(preset.rollAngleDeg);
+            arm.setPitchGoalDegrees(preset.pitchAngleDeg);
 
             currentPreset = preset;
         }
     }
 
+    public void setCoralPresetElevator(CoralPresets preset) {
+        elevator.setGoalPosition(preset.elevatorHeightM);
+        currentPreset = preset;
+    }
+
+    public boolean isElevatorInPosition() {
+        return elevator.isInPosition();
+    }
+
+    public void setCoralPresetPivot(CoralPresets preset) {
+        arm.setPivotGoalDegrees(
+                preset.pivotAngleDeg
+                        * (mirrorSetting.isMirrored ? -1 : 1));
+        currentPreset = preset;
+    }
+
+    public boolean isPivotInPosition() {
+        return arm.isPivotInPosition();
+    }
+
+    public void setCoralPresetPitch(CoralPresets preset) {
+        arm.setPitchGoalDegrees(
+                preset.pitchAngleDeg);
+        currentPreset = preset;
+    }
+
+    public boolean isPitchInPosition() {
+        return arm.isPitchInPosition();
+    }
+
+    public void setCoralPresetRoll(CoralPresets preset) {
+        arm.setRollGoalDegrees(
+                preset.rollAngleDeg
+                        * (mirrorSetting.isMirrored ? -1 : 1));
+        currentPreset = preset;
+    }
+
+    public boolean isRollInPosition() {
+        return arm.isRollInPosition();
+    }
+
     public double getPivotGoalDegrees() {
         return arm.getPivotGoalDegrees();
     }
@@ -167,5 +218,8 @@ public void setCustomIntakePercent(double percentage) {
         currentIntakePreset = CoralIntakePresets.CUSTOM;
         intake.setOutputPercentage(percentage);
     }
+
+    public double getPivotPositionDegrees() {
+        return arm.getPivotPositionDegrees();
+    }
 }
- 
\ No newline at end of file

From 5eeaf133c3e8b67832eec14d89d358d4ffacf8e6 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 9 Feb 2025 00:33:33 -0600
Subject: [PATCH 40/71] Hopefully fix for analog wrist encoders.

---
 src/main/java/frc/robot/Constants.java        |  5 +++
 .../frc/robot/subsystems/coral/CoralArm.java  | 37 ++++++++++++++-----
 2 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f17a201..04fd24a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -202,6 +202,9 @@ public static class PhysicalConstants {
 
     public static class Roll {
       public static final int MOTOR_PORT = 15;
+      public static final boolean ENCODER_INVERTED = false;
+      public static final double ENCODER_OFFSET_RADS = 0.0;
+
       public static final ProfiledPIDController PID = new ProfiledPIDController(
           1,
           0.0,
@@ -220,6 +223,8 @@ public static class PhysicalConstants {
 
     public static class Pitch {
       public static final int MOTOR_PORT = 16;
+      public static final boolean ENCODER_INVERTED = false;
+      public static final double ENCODER_OFFSET_RADS = 0.0;
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index dcc9cc2..05049ea 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -5,11 +5,14 @@
 import com.revrobotics.sim.SparkAbsoluteEncoderSim;
 import com.revrobotics.sim.SparkFlexSim;
 import com.revrobotics.sim.SparkMaxSim;
+import com.revrobotics.spark.SparkAnalogSensor;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.AbsoluteEncoderConfig;
+import com.revrobotics.spark.config.AnalogSensorConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
@@ -38,8 +41,8 @@ public class CoralArm extends SubsystemBase {
         private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
 
         private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-        private final AbsoluteEncoder rollEncoder = rollMotor.getAbsoluteEncoder();
-        private final AbsoluteEncoder pitchEncoder = pitchMotor.getAbsoluteEncoder();
+        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
         // sim encoders
         private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
         private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
@@ -88,6 +91,10 @@ public class CoralArm extends SubsystemBase {
 
         public CoralArm() {
 
+                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
+                                .positionConversionFactor((Math.PI * 2) / 5.0)
+                                .velocityConversionFactor((Math.PI * 2) / 5.0);
+
                 pivotMotor.configure(
                                 pivotConfig.idleMode(IdleMode.kBrake).apply(
                                                 new EncoderConfig()
@@ -102,10 +109,12 @@ public CoralArm() {
                                 // Seconds
                                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
                 rollMotor.configure(
-                                rollConfig.idleMode(IdleMode.kBrake),
+                                rollConfig.idleMode(IdleMode.kBrake)
+                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
                                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
                 pitchMotor.configure(pitchConfig
-                                .idleMode(IdleMode.kBrake),
+                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
                                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
                 /*
@@ -160,15 +169,15 @@ public void periodic() {
                 // run the motors
                 pivotMotor.setVoltage(pivotPIDout + pivotFFout);
 
-                double rollPIDout = rollPID.calculate(rollEncoder.getPosition());
-                SmartDashboard.putNumber("Coral/Roll/position", rollEncoder.getPosition());
+                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
                 SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
                 SmartDashboard.putNumber("Coral/Roll/out", rollPIDout);
                 rollMotor.setVoltage(rollPIDout);
 
-                double pitchPIDout = pitchPID.calculate(pitchEncoder.getPosition());
-                SmartDashboard.putNumber("Coral/Pitch/position", pitchEncoder.getPosition());
+                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
                 SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
                 SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout);
@@ -265,13 +274,13 @@ public double getPivotPositionDegrees() {
 
         public double getRollPositionDegrees() {
                 return Units.radiansToDegrees(Robot.isReal()
-                                ? rollEncoder.getPosition()
+                                ? readRollEncoderPosition()
                                 : simRollEncoder.getPosition());
         }
 
         public double getPitchPositionDegrees() {
                 return Units.radiansToDegrees(Robot.isReal()
-                                ? pitchEncoder.getPosition()
+                                ? readPitchEncoderPosition()
                                 : simPitchEncoder.getPosition());
         }
 
@@ -286,4 +295,12 @@ public boolean isRollInPosition() {
         public boolean isPivotInPosition() {
                 return pivotPID.atGoal();
         }
+
+        public double readPitchEncoderPosition() {
+                return pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS;
+        }
+
+        public double readRollEncoderPosition() {
+                return rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS;
+        }
 }

From 337847e211ba0cdf3a7c27d440695708d0e0b4f9 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 9 Feb 2025 15:48:06 -0600
Subject: [PATCH 41/71] Update CAN IDs for comp bot.

---
 simgui-ds.json                              |  7 +------
 src/main/java/frc/robot/Constants.java      | 16 ++++++++--------
 src/main/java/frc/robot/RobotContainer.java |  3 ++-
 3 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index aa26d60..5561453 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "enabled": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -101,7 +96,7 @@
       "useGamepad": true
     },
     {
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 04fd24a..e25c1ee 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -86,33 +86,33 @@ public static class SwerveModuleConstants {
     public static final double MODULE_KD = 0.0066806;// 0.0057682; //0.0076954;
 
     // --------- Front Left Module --------- \\
-    public static final int FL_DRIVE_ID = 4;
+    public static final int FL_DRIVE_ID = 34;
     public static final int FL_STEER_ID = 4;
-    public static final int FL_ABSOLUTE_ENCODER_PORT = 4;
+    public static final int FL_ABSOLUTE_ENCODER_PORT = 54;
     public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(0.389893) + Math.PI * 0.5 + Math.PI;
     public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean FL_MOTOR_REVERSED = true;
 
     // --------- Front Right Module --------- \\
-    public static final int FR_DRIVE_ID = 1;
+    public static final int FR_DRIVE_ID = 31;
     public static final int FR_STEER_ID = 1;
-    public static final int FR_ABSOLUTE_ENCODER_PORT = 1;
+    public static final int FR_ABSOLUTE_ENCODER_PORT = 51;
     public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(0.323730) + Math.PI * 0.5 + Math.PI;
     public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean FR_MOTOR_REVERSED = true;
 
     // --------- Back Right Module --------- \\
-    public static final int BR_DRIVE_ID = 2;
+    public static final int BR_DRIVE_ID = 32;
     public static final int BR_STEER_ID = 2;
-    public static final int BR_ABSOLUTE_ENCODER_PORT = 2;
+    public static final int BR_ABSOLUTE_ENCODER_PORT = 52;
     public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(-0.360107) + Math.PI * 0.5 + Math.PI;
     public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean BR_MOTOR_REVERSED = true;
 
     // --------- Back Left Module --------- \\
-    public static final int BL_DRIVE_ID = 3;
+    public static final int BL_DRIVE_ID = 33;
     public static final int BL_STEER_ID = 3;
-    public static final int BL_ABSOLUTE_ENCODER_PORT = 3;
+    public static final int BL_ABSOLUTE_ENCODER_PORT = 53;
     public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(0.399902) + Math.PI * 0.5 + Math.PI;
     public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean BL_MOTOR_REVERSED = true;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 22fb776..7914b61 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -198,7 +198,8 @@ public boolean getAsBoolean() {
             }
         }).whileTrue(new InstantCommand(() -> {
             lockCoralArmPreset(CoralPresets.INTAKE);
-        }).andThen(getGoToLockedPresetCommand()).andThen(getStowCommand())).whileFalse(getStowCommand());
+        }).andThen(getGoToLockedPresetCommand()).andThen(new IntakeCoralCommand(coralSubsystem))
+                .andThen(getStowCommand())).whileFalse(getStowCommand());
         // .onFalse(normalDrive);
 
         // purge coral

From b7359ecb50306af530eee9a348454c7a8d646022 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 9 Feb 2025 16:17:02 -0600
Subject: [PATCH 42/71] Add debugging facilities and more encoder
 configuration.

---
 src/main/java/frc/robot/Constants.java        | 10 ++++++++++
 src/main/java/frc/robot/RobotContainer.java   |  8 ++++++++
 .../commands/coral/motion/MoveElevator.java   |  3 ++-
 .../commands/coral/motion/MovePitch.java      |  3 ++-
 .../commands/coral/motion/MovePivot.java      |  3 ++-
 .../robot/commands/coral/motion/MoveRoll.java |  3 ++-
 .../robot/commands/coral/motion/StowArm.java  |  4 +++-
 .../robot/subsystems/ClimberSubsystem.java    |  4 +++-
 .../frc/robot/subsystems/coral/CoralArm.java  | 20 +++++++++++--------
 .../robot/subsystems/coral/CoralElevator.java | 15 +++++++++-----
 .../robot/subsystems/coral/CoralIntake.java   |  6 ++++--
 11 files changed, 58 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index e25c1ee..f37abbf 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -165,6 +165,8 @@ public static class Climber {
         0.0,
         0.0,
         new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+
+    public static boolean DBG_DISABLED = true;
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
@@ -172,6 +174,9 @@ public static class Coral {
     public static class Pivot {
       public static final int MOTOR_PORT = 14;
       public static final int ENCODER_PORT = 28;
+      public static boolean DBG_DISABLED = true;
+
+      public static final boolean ENCODER_INVERTED = false;
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(80);
       public static final double FRAME_BORDER_ANGLE = Units.degreesToRadians(30);
@@ -204,6 +209,7 @@ public static class Roll {
       public static final int MOTOR_PORT = 15;
       public static final boolean ENCODER_INVERTED = false;
       public static final double ENCODER_OFFSET_RADS = 0.0;
+      public static boolean DBG_DISABLED = true;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
           1,
@@ -225,6 +231,7 @@ public static class Pitch {
       public static final int MOTOR_PORT = 16;
       public static final boolean ENCODER_INVERTED = false;
       public static final double ENCODER_OFFSET_RADS = 0.0;
+      public static boolean DBG_DISABLED = false;
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
@@ -245,6 +252,7 @@ public static class PhysicalConstants {
 
     public static class Intake {
       public static final int MOTOR_PORT = 17;
+      public static boolean DBG_DISABLED = true;
 
       public static final double POSITIVE_RATE_LIMIT = 20.0; // Fast shoot
       public static final double NEGATIVE_RATE_LIMIT = 5.0; // Slow intake
@@ -321,6 +329,8 @@ public static final class Follower {
       public static final boolean INVERTED = false;
     }
 
+    public static boolean DBG_DISABLED = true;
+
     public static double MOTOR_REVOLUTIONS_PER_METER = 32.81;
 
     // TODO: Tune! Use FWERB for now
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7914b61..a86c539 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -52,6 +52,7 @@ public class RobotContainer {
             ControllerConstants.DRIVER_CONTROLLER_PORT);
     private final CommandXboxController operatorXbox = new CommandXboxController(
             ControllerConstants.OPERATOR_CONTROLLER_PORT);
+    private final CommandXboxController debugXboxController = new CommandXboxController(3);
 
     // private final CommandXboxController debugXbox = new CommandXboxController(0);
 
@@ -246,6 +247,13 @@ public boolean getAsBoolean() {
                 return algaeSubsystem.isHolding();
             }
         }).whileTrue(new ShootAlgaeCommand(algaeSubsystem));
+
+        /////////////////// DEBUGGING //////////////////
+        debugXboxController.a().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPitch(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPitch(CoralPresets.STOW);
+        }));
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
index 2cf037d..c5e7d67 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
@@ -19,6 +20,6 @@ public MoveElevator(CoralSubsystem coralSub, Supplier<CoralPresets> presetSuppli
 
     @Override
     public boolean isFinished() {
-        return coralSub.isElevatorInPosition();
+        return Constants.Elevator.DBG_DISABLED || coralSub.isElevatorInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
index d8f2600..d989685 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
@@ -20,6 +21,6 @@ public MovePitch(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return coralSub.isPitchInPosition();
+        return Constants.Coral.Pitch.DBG_DISABLED || coralSub.isPitchInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
index c86cd74..1edc173 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
@@ -19,6 +20,6 @@ public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return coralSub.isPivotInPosition();
+        return Constants.Coral.Pivot.DBG_DISABLED || coralSub.isPivotInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
index 59d0c61..85548b2 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
@@ -19,6 +20,6 @@ public MoveRoll(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return coralSub.isRollInPosition();
+        return Constants.Coral.Roll.DBG_DISABLED || coralSub.isRollInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
index 4d78928..f267d12 100644
--- a/src/main/java/frc/robot/commands/coral/motion/StowArm.java
+++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
 
@@ -21,6 +22,7 @@ public StowArm(CoralSubsystem coralSub) {
     public boolean isFinished() {
         // Intentionally not worrying about pitch because it doesn't really matter
         // anyways
-        return coralSub.isRollInPosition() && coralSub.isPivotInPosition();
+        return Constants.Coral.Pivot.DBG_DISABLED || Constants.Coral.Roll.DBG_DISABLED
+                || coralSub.isRollInPosition() && coralSub.isPivotInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 5f4d7db..e45b262 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -8,6 +8,7 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Constants.Climber;
 
 public class ClimberSubsystem extends SubsystemBase {
@@ -24,7 +25,8 @@ public ClimberSubsystem() {
 
     @Override
     public void periodic() {
-        climberMotor.set(output);
+        if (!Constants.Climber.DBG_DISABLED)
+            climberMotor.set(output);
     }
 
     // TODO: limit the output somehow
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 05049ea..eb07913 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.coral;
 
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.sim.SparkAbsoluteEncoderSim;
@@ -90,7 +91,6 @@ public class CoralArm extends SubsystemBase {
         private double pitchGoal = 0.0; // Rads
 
         public CoralArm() {
-
                 AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
                                 .positionConversionFactor((Math.PI * 2) / 5.0)
                                 .velocityConversionFactor((Math.PI * 2) / 5.0);
@@ -153,8 +153,8 @@ public void periodic() {
                 }
 
                 double pivotPosition = Units
-                                .rotationsToRadians(pivotEncoder.getAbsolutePosition()
-                                                .getValueAsDouble());
+                                .rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
                 double pivotFFout = pivotFeedforward.calculate(pivotPID.getSetpoint().position,
                                 pivotPID.getSetpoint().velocity);
                 double pivotPIDout = pivotPID.calculate(pivotPosition);
@@ -167,21 +167,24 @@ public void periodic() {
                 SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
 
                 // run the motors
-                pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+                if (!Constants.Coral.Pivot.DBG_DISABLED)
+                        pivotMotor.setVoltage(pivotPIDout + pivotFFout);
 
                 double rollPIDout = rollPID.calculate(readRollEncoderPosition());
                 SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
                 SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
                 SmartDashboard.putNumber("Coral/Roll/out", rollPIDout);
-                rollMotor.setVoltage(rollPIDout);
+                if (!Constants.Coral.Roll.DBG_DISABLED)
+                        rollMotor.setVoltage(rollPIDout);
 
                 double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
                 SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
                 SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
                 SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout);
-                pitchMotor.setVoltage(pitchPIDout);
+                if (!Constants.Coral.Pitch.DBG_DISABLED)
+                        pitchMotor.setVoltage(pitchPIDout);
         }
 
         @Override
@@ -267,8 +270,9 @@ public double getPitchGoalDegrees() {
         }
 
         public double getPivotPositionDegrees() {
-                return Units.radiansToDegrees(Robot.isReal()
-                                ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                return Units.rotationsToDegrees(Robot.isReal()
+                                ? (pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))
                                 : simPivotPhysics.getAngleRads());
         }
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index cacf26c..a1557b9 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.Constants.Elevator;
 
@@ -122,12 +123,16 @@ public void periodic() {
             SmartDashboard.putNumber("Elevator/goal", elevatorPID.getGoal().position);
             SmartDashboard.putNumber("Elevator/pid_out", pid_out);
 
-            leaderMotor.setVoltage(output);
-            followerMotor.setVoltage(output);
+            if (!Constants.Elevator.DBG_DISABLED) {
+                leaderMotor.setVoltage(output);
+                followerMotor.setVoltage(output);
+            }
         } else {
-            // slowly move down to zero
-            leaderMotor.set(-0.05);
-            followerMotor.set(-0.05);
+            if (!Constants.Elevator.DBG_DISABLED) {
+                // slowly move down to zero
+                leaderMotor.set(-0.05);
+                followerMotor.set(-0.05);
+            }
         }
     }
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index fb43908..63ba661 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -47,11 +47,13 @@ public void periodic() {
         // if no coral
         if (curHolding) {
             double output = intakeProfile.calculate(outputPercentage);
-            intakeMotor.set(output);
+            if (!Constants.Coral.Intake.DBG_DISABLED)
+                intakeMotor.set(output);
         } else {
             // hold, negative is out so intake a bit.
             // Set to 0.1 to be good
-            intakeMotor.set(Math.min(0.1, outputPercentage));
+            if (!Constants.Coral.Intake.DBG_DISABLED)
+                intakeMotor.set(Math.min(0.1, outputPercentage));
         }
 
         if (lastHolding != curHolding) {

From 334adea17d36da2843835878867ee4791b26da12 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Mon, 10 Feb 2025 02:58:07 -0600
Subject: [PATCH 43/71] Pivot, etc. simulation & rudimentary tuning.

---
 simgui-ds.json                                |   5 +
 src/main/java/frc/robot/Constants.java        |  24 +-
 src/main/java/frc/robot/RobotContainer.java   |  17 +
 .../frc/robot/subsystems/coral/CoralArm.java  | 558 +++++++++---------
 .../subsystems/coral/CoralSubsystem.java      |   2 +
 5 files changed, 325 insertions(+), 281 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index 5561453..1327cce 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d27854e..05ab32c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -184,24 +184,24 @@ public static class Pivot {
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          1,
+          20.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+          new TrapezoidProfile.Constraints(7.0, 20.0));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
           0.0,
-          0.75, // V
-          1.62, // V*s/rad
-          0.05// V*s^2/rad
+          0.4, // V
+          1.0, // V*s/rad
+          0.04// V*s^2/rad
       );
 
       public static class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
         public static final double NET_REDUCTION = 96.0;
-        public static final double MASS_KG = 4.8;
-        public static final double ARM_LENGTH_METERS = 0.51;
+        public static final double MASS_KG = 4.7727;
+        public static final double ARM_LENGTH_METERS = 0.510;
         public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(23.0);
         public static final double MOI = 0.2875548495; // Kg*m^2
       }
@@ -214,11 +214,11 @@ public static class Roll {
       public static boolean DBG_DISABLED = true;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          1,
+          10.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(20.0, 200.0));
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.4);
+          new TrapezoidProfile.Constraints(20.0, 150.0));
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.5);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
@@ -243,9 +243,9 @@ public static class Pitch {
           20.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 400.0)); // Radians
+          new TrapezoidProfile.Constraints(12.0, 200.0)); // Radians
 
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.88);
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.9);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a86c539..d1202e0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -254,6 +254,23 @@ public boolean getAsBoolean() {
         })).onFalse(new InstantCommand(() -> {
             coralSubsystem.setCoralPresetPitch(CoralPresets.STOW);
         }));
+
+        debugXboxController.b().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetRoll(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetRoll(CoralPresets.STOW);
+        }));
+
+        debugXboxController.x().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPivot(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPivot(CoralPresets.STOW);
+        }));
+        debugXboxController.y().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetElevator(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetElevator(CoralPresets.STOW);
+        }));
     }
 
     /**
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index a199cb1..368aec7 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -34,273 +34,293 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase {
-        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-        // sim motors
-        private double simPivotVoltage = 0.0;
-        private double simRollVoltage = 0.0;
-        private double simPitchVoltage = 0.0;
-
-        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
-        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
-        // sim encoders
-        private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
-        private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
-
-        // physics simulations
-        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-                        Coral.Pivot.PhysicalConstants.MOTOR,
-                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pivot.PhysicalConstants.MOI,
-                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Pivot.MAXIMUM_ANGLE * -1,
-                        Coral.Pivot.MAXIMUM_ANGLE, true, 0);
-        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-                        Coral.Roll.PhysicalConstants.MOTOR,
-                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
-                        Coral.Roll.PhysicalConstants.MOI,
-                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Roll.MAXIMUM_ANGLE * -1,
-                        Coral.Roll.MAXIMUM_ANGLE,
-                        false,
-                        0);
-        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-                        Coral.Pitch.PhysicalConstants.MOTOR,
-                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pitch.PhysicalConstants.MOI,
-                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Pitch.MAXIMUM_ANGLE * -1,
-                        Coral.Pitch.MAXIMUM_ANGLE,
-                        false,
-                        0);
-
-        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
-        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-        private final ProfiledPIDController rollPID = Coral.Roll.PID;
-        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-
-        private double pivotGoal = 0.0; // Rads
-        private double rollGoal = 0.0; // Rads
-        private double pitchGoal = 0.0; // Rads
-
-        public CoralArm() {
-                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
-                                .positionConversionFactor((Math.PI * 2) / 5.0)
-                                .velocityConversionFactor((Math.PI * 2) / 5.0);
-
-                pivotMotor.configure(
-                                pivotConfig.idleMode(IdleMode.kBrake).apply(
-                                                new EncoderConfig()
-                                                                .positionConversionFactor(
-                                                                                (1.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
-                                                                .velocityConversionFactor(
-                                                                                (60.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
-                                                                                                                                                           // ->
-                                // Rotations &
-                                // Seconds
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                rollMotor.configure(
-                                rollConfig.idleMode(IdleMode.kBrake)
-                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                pitchMotor.configure(pitchConfig
-                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-                /*
-                 * .apply(new EncoderConfig()
-                 * .positionConversionFactor(
-                 * (1.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
-                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
-                 */
-                // Not sure how to get
-
-                // Disabling this for now because we don't want the wrist wire bundle to explode
-                // rollPID.enableContinuousInput(-180, 180);
-
-                // TODO: Are these reasonable? who knows.
-                pivotPID.setTolerance(Units.degreesToRadians(0.5));
-                rollPID.setTolerance(Units.degreesToRadians(0.5));
-                pitchPID.setTolerance(Units.degreesToRadians(0.5));
-        }
-
-        @Override
-        public void periodic() {
-                // if entering the frame border
-                // && wrist is at neutral
-                if ((pivotPID.getGoal().position != pivotGoal)
-                                && (rollPID.atGoal() && pitchPID.atGoal())) {
-                        // allow arm to enter frame border
-                        pivotPID.setGoal(pivotGoal);
-                } else if ( // if exiting the frame border && has exited the frame border
-                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                        // set roll and pitch back to their goals
-                        rollPID.setGoal(rollGoal);
-                        pitchPID.setGoal(pitchGoal);
-                }
-
-                double pivotPosition = Units
-                                .rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-                double pivotFFout = pivotFeedforward.calculate(pivotPID.getSetpoint().position,
-                                pivotPID.getSetpoint().velocity);
-                double pivotPIDout = pivotPID.calculate(pivotPosition);
-
-                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
-                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
-                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-
-                // run the motors
-                if (!Constants.Coral.Pivot.DBG_DISABLED)
-                        pivotMotor.setVoltage(pivotPIDout + pivotFFout);
-                simPivotVoltage = pivotPIDout + pivotFFout;
-
-                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
-                double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
-                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
-                if (!Constants.Coral.Roll.DBG_DISABLED)
-                        rollMotor.setVoltage(rollPIDout + rollFFout);
-                simRollVoltage = rollPIDout;
-
-                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
-                double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
-                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
-                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
-                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
-                if (!Constants.Coral.Pitch.DBG_DISABLED)
-                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-                simPitchVoltage = pitchPIDout;
-        }
-
-        @Override
-        public void simulationPeriodic() {
-                // update physics
-                simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
-                simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
-                simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
-                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
-                                * RoboRioSim.getVInVoltage());
-
-                simPivotPhysics.update(0.02);
-                simRollPhysics.update(0.02);
-                simPitchPhysics.update(0.02);
-        }
-
-        // this should be relative to straight upwards.
-        // i.e. 0 should be straight vertical,
-        // 20 would be to the right, -20 to the left
-        public void setPivotGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pivotGoal = MathUtil.clamp(goalRads,
-                                Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE);
-                // if passing through the frame border
-                if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
-                                && (Math.abs(this.getPivotPositionDegrees()) < Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                        // set the pivot to stop at the frame border to allow the wrist to move
-                        // neutral
-                        pivotPID.setGoal(
-                                        Coral.Pivot.FRAME_BORDER_ANGLE
-                                                        * (pivotGoal < 0 ? -1 : 1));
-                        rollPID.setGoal(0);
-                        pitchPID.setGoal(0);
-                } else {
-                        pivotPID.setGoal(pivotGoal);
-                }
-        }
-
-        // similarly, this ranges from -180 to 180
-        public void setRollGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-                // if outside of frame border
-                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-                        // start moving to goal
-                        rollPID.setGoal(rollGoal);
-                }
-
-        }
-
-        public void setPitchGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-                // if outside the frame border
-                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-                        // start moving to goal
-                        pitchPID.setGoal(pitchGoal);
-                }
-        }
-
-        public double getPivotGoalDegrees() {
-                return Units.radiansToDegrees(pivotGoal);
-        }
-
-        public double getRollGoalDegrees() {
-                return Units.radiansToDegrees(rollGoal);
-        }
-
-        public double getPitchGoalDegrees() {
-                return Units.radiansToDegrees(pitchGoal);
-        }
-
-        public double getPivotPositionDegrees() {
-                return Units.rotationsToDegrees(Robot.isReal()
-                                ? (pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))
-                                : simPivotPhysics.getAngleRads());
-        }
-
-        public double getRollPositionDegrees() {
-                return Units.radiansToDegrees(readRollEncoderPosition());
-        }
-
-        public double getPitchPositionDegrees() {
-                return Units.radiansToDegrees(readPitchEncoderPosition());
-        }
-
-        public boolean isPitchInPosition() {
-                return pitchPID.atGoal();
-        }
-
-        public boolean isRollInPosition() {
-                return rollPID.atGoal();
-        }
-
-        public boolean isPivotInPosition() {
-                return pivotPID.atGoal();
-        }
-
-        public double readPitchEncoderPosition() {
-                return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
-                                : MathUtil.angleModulus(
-                                                pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
-        }
-
-        public double readRollEncoderPosition() {
-                return Robot.isSimulation() ? simRollPhysics.getAngleRads()
-                                : MathUtil.angleModulus(
-                                                rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
-        }
+    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+    // sim motors
+    private double simPivotVoltage = 0.0;
+    private double simRollVoltage = 0.0;
+    private double simPitchVoltage = 0.0;
+
+    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+    private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+    private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
+    // sim encoders
+    // private final SparkAbsoluteEncoderSim simRollEncoder = new
+    // SparkAbsoluteEncoderSim(rollMotor);
+    // private final SparkAbsoluteEncoderSim simPitchEncoder = new
+    // SparkAbsoluteEncoderSim(pitchMotor);
+
+    // physics simulations
+    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+            Coral.Pivot.PhysicalConstants.MOTOR,
+            Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+            Coral.Pivot.PhysicalConstants.MOI,
+            Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+            0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
+            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI - 0.05);
+    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+            Coral.Roll.PhysicalConstants.MOTOR,
+            Coral.Roll.PhysicalConstants.NET_REDUCTION,
+            Coral.Roll.PhysicalConstants.MOI,
+            Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Roll.MAXIMUM_ANGLE * -1,
+            Coral.Roll.MAXIMUM_ANGLE,
+            false,
+            0);
+    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+            Coral.Pitch.PhysicalConstants.MOTOR,
+            Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+            Coral.Pitch.PhysicalConstants.MOI,
+            Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Pitch.MAXIMUM_ANGLE * -1,
+            Coral.Pitch.MAXIMUM_ANGLE,
+            false,
+            0);
+
+    private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+    private final ProfiledPIDController rollPID = Coral.Roll.PID;
+    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+    private double pivotGoal = 0.0; // Rads
+    private double rollGoal = 0.0; // Rads
+    private double pitchGoal = 0.0; // Rads
+
+    public CoralArm() {
+        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
+                .positionConversionFactor((Math.PI * 2) / 5.0)
+                .velocityConversionFactor((Math.PI * 2) / 5.0);
+
+        pivotMotor.configure(
+                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                        new EncoderConfig()
+                                .positionConversionFactor(
+                                        (1.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                .velocityConversionFactor(
+                                        (60.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                           // ->
+                // Rotations &
+                // Seconds
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        rollMotor.configure(
+                rollConfig.idleMode(IdleMode.kBrake)
+                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        pitchMotor.configure(pitchConfig
+                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        /*
+         * .apply(new EncoderConfig()
+         * .positionConversionFactor(
+         * (1.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+         * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+         */
+        // Not sure how to get
+
+        // Disabling this for now because we don't want the wrist wire bundle to explode
+        // rollPID.enableContinuousInput(-180, 180);
+
+        // TODO: Are these reasonable? who knows.
+        pivotPID.setTolerance(Units.degreesToRadians(0.5));
+        rollPID.setTolerance(Units.degreesToRadians(0.5));
+        pitchPID.setTolerance(Units.degreesToRadians(0.5));
+    }
+
+    @Override
+    public void periodic() {
+        // if entering the frame border
+        // && wrist is at neutral
+        /*
+         * if ((pivotPID.getGoal().position != pivotGoal)
+         * && (rollPID.atGoal() && pitchPID.atGoal())) {
+         * // allow arm to enter frame border
+         * pivotPID.setGoal(pivotGoal);
+         * } else if ( // if exiting the frame border && has exited the frame border
+         * ((rollGoal != rollPID.getGoal().position) || (pitchGoal !=
+         * pitchPID.getGoal().position))
+         * && (Math.abs(this.getPitchPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+         * // set roll and pitch back to their goals
+         * rollPID.setGoal(rollGoal);
+         * pitchPID.setGoal(pitchGoal);
+         * }
+         */
+
+        double pivotPosition = readPivotEncoderPosition();
+        double pivotFFout = pivotFeedforward.calculate(
+                Math.PI * 0.5 + pivotPosition,
+                pivotPID.getSetpoint().velocity);
+        double pivotPIDout = pivotPID.calculate(pivotPosition);
+
+        SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+        SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+        SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+
+        // run the motors
+        if (!Constants.Coral.Pivot.DBG_DISABLED)
+            pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+        simPivotVoltage = pivotPIDout + pivotFFout;
+
+        double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+        double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+        SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
+        if (!Constants.Coral.Roll.DBG_DISABLED)
+            rollMotor.setVoltage(rollPIDout + rollFFout);
+        simRollVoltage = rollPIDout;
+
+        double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+        double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+        SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+        SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+        SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
+        if (!Constants.Coral.Pitch.DBG_DISABLED)
+            pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+        simPitchVoltage = pitchPIDout;
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
+        simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
+        simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
+        SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                * RoboRioSim.getVInVoltage());
+
+        simPivotPhysics.update(0.02);
+        simRollPhysics.update(0.02);
+        simPitchPhysics.update(0.02);
+    }
+
+    // this should be relative to straight upwards.
+    // i.e. 0 should be straight vertical,
+    // 20 would be to the right, -20 to the left
+    public void setPivotGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pivotGoal = MathUtil.clamp(goalRads,
+                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
+        // if passing through the frame border
+        /*
+         * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+         * && (Math.abs(this.getPivotPositionDegrees()) <
+         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+         * // set the pivot to stop at the frame border to allow the wrist to move
+         * // neutral
+         * pivotPID.setGoal(
+         * Coral.Pivot.FRAME_BORDER_ANGLE
+         * (pivotGoal < 0 ? -1 : 1));
+         * rollPID.setGoal(0);
+         * pitchPID.setGoal(0);
+         * } else {
+         * pivotPID.setGoal(pivotGoal);
+         * }
+         */
+
+        pivotPID.setGoal(pivotGoal);
+    }
+
+    // similarly, this ranges from -180 to 180
+    public void setRollGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+        /*
+         * // if outside of frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * rollPID.setGoal(rollGoal);
+         * }
+         */
+        rollPID.setGoal(rollGoal);
+    }
+
+    public void setPitchGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+        /*
+         * // if outside the frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * pitchPID.setGoal(pitchGoal);
+         * }
+         */
+        pitchPID.setGoal(pitchGoal);
+    }
+
+    public double getPivotGoalDegrees() {
+        return Units.radiansToDegrees(pivotGoal);
+    }
+
+    public double getRollGoalDegrees() {
+        return Units.radiansToDegrees(rollGoal);
+    }
+
+    public double getPitchGoalDegrees() {
+        return Units.radiansToDegrees(pitchGoal);
+    }
+
+    public double getPivotPositionDegrees() {
+        return Units.radiansToDegrees(readPivotEncoderPosition());
+    }
+
+    public double getRollPositionDegrees() {
+        return Units.radiansToDegrees(readRollEncoderPosition());
+    }
+
+    public double getPitchPositionDegrees() {
+        return Units.radiansToDegrees(readPitchEncoderPosition());
+    }
+
+    public boolean isPitchInPosition() {
+        return pitchPID.atGoal();
+    }
+
+    public boolean isRollInPosition() {
+        return rollPID.atGoal();
+    }
+
+    public boolean isPivotInPosition() {
+        return pivotPID.atGoal();
+    }
+
+    public double readPitchEncoderPosition() {
+        return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
+                : MathUtil.angleModulus(
+                        pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
+    }
+
+    public double readRollEncoderPosition() {
+        return Robot.isSimulation() ? simRollPhysics.getAngleRads()
+                : MathUtil.angleModulus(
+                        rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
+    }
+
+    public double readPivotEncoderPosition() {
+        return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
+                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index d705180..e5e8a0b 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -3,6 +3,7 @@
 // import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -121,6 +122,7 @@ public boolean isElevatorInPosition() {
     }
 
     public void setCoralPresetPivot(CoralPresets preset) {
+        SmartDashboard.putNumber("Pivot Pre", preset.pivotAngleDeg);
         arm.setPivotGoalDegrees(
                 preset.pivotAngleDeg
                         * (mirrorSetting.isMirrored ? -1 : 1));

From 7ea94eb2644102824ba8480201a0649077e98ded Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 9 Feb 2025 21:22:13 -0600
Subject: [PATCH 44/71] Rough elevator PID tuning.

---
 src/main/java/frc/robot/Constants.java        |  6 +++---
 .../robot/subsystems/coral/CoralElevator.java | 20 ++++++++++---------
 2 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 05ab32c..a0bad81 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -344,7 +344,7 @@ public static final class Follower {
     public static class PID {
       public static double kP = 20.0;
       public static double kI = 0.0;
-      public static double kD = 0.5;
+      public static double kD = 0.01;
       public static double MAX_VELOCITY = 3.20;
       // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
       // the elevator can make or exceed this
@@ -354,8 +354,8 @@ public static class PID {
     // TODO: PAD THE ELEVATOR!!!!!!!
     public static class FeedforwardConstants {
       public static double Ks = 0.0;
-      public static double Kv = 3.5;
-      public static double Ka = 0.08;
+      public static double Kv = 3.0;
+      public static double Ka = 0.04;
       public static double Kg = 0.35; // TODO: Check this!!!
     }
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index a1557b9..7bdcb83 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -10,7 +10,6 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkFlexConfig;
 
-import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -35,6 +34,7 @@ public class CoralElevator extends SubsystemBase {
     // sim motors
     private final SparkFlexSim simLeaderMotor = new SparkFlexSim(leaderMotor, DCMotor.getNeoVortex(1));
     private final SparkFlexSim simFollowerMotor = new SparkFlexSim(followerMotor, DCMotor.getNeoVortex(1));
+    private double simOutputVoltage = 0.0;
 
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
@@ -113,20 +113,23 @@ public void periodic() {
             lastTime = Timer.getFPGATimestamp();
             lastVelocity = setpoint.velocity;
 
-            double pid_out = elevatorPID.calculate(leaderEncoder.getPosition());
-            double output = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime)
-                    + pid_out;
+            double pid_out = elevatorPID.calculate(getPosition());
+            double ff_out = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime);
+            double output = ff_out + pid_out;
 
-            SmartDashboard.putNumber("Elevator/Output", output);
-            SmartDashboard.putNumber("Elevator/position", leaderEncoder.getPosition());
+            SmartDashboard.putNumber("Elevator/output", output);
+            SmartDashboard.putNumber("Elevator/position", getPosition());
             SmartDashboard.putNumber("Elevator/target", setpoint.position);
             SmartDashboard.putNumber("Elevator/goal", elevatorPID.getGoal().position);
             SmartDashboard.putNumber("Elevator/pid_out", pid_out);
+            SmartDashboard.putNumber("Elevator/ff_out", ff_out);
 
             if (!Constants.Elevator.DBG_DISABLED) {
                 leaderMotor.setVoltage(output);
                 followerMotor.setVoltage(output);
             }
+
+            simOutputVoltage = output;
         } else {
             if (!Constants.Elevator.DBG_DISABLED) {
                 // slowly move down to zero
@@ -139,8 +142,7 @@ public void periodic() {
     @Override
     public void simulationPeriodic() {
         // update physics
-        simElevator.setInput(
-                (leaderMotor.getAppliedOutput() + followerMotor.getAppliedOutput()) * 0.5 * RoboRioSim.getVInVoltage());
+        simElevator.setInput(MathUtil.clamp(simOutputVoltage, -12.0, 12.0));
         simElevator.update(0.02);
 
         // update sim objects
@@ -165,7 +167,7 @@ public double getGoalPosition() {
     public double getPosition() {
         return Robot.isReal()
                 ? leaderEncoder.getPosition()
-                : simLeaderEncoder.getPosition();
+                : simElevator.getPositionMeters();
     }
 
     public boolean isInPosition() {

From 8ca498d7591e513c08d5b9813909a09e4cc9ebc1 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Mon, 10 Feb 2025 00:45:25 -0600
Subject: [PATCH 45/71] Simple feedforwards for pitch/roll. Rough tuning of
 pitch (sim)

---
 src/main/java/frc/robot/Constants.java        | 13 +++-
 .../frc/robot/subsystems/coral/CoralArm.java  | 76 +++++++++----------
 .../subsystems/coral/CoralSubsystem.java      | 10 ++-
 3 files changed, 52 insertions(+), 47 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f37abbf..d27854e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -14,6 +14,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -201,6 +202,7 @@ public static class PhysicalConstants {
         public static final double NET_REDUCTION = 96.0;
         public static final double MASS_KG = 4.8;
         public static final double ARM_LENGTH_METERS = 0.51;
+        public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(23.0);
         public static final double MOI = 0.2875548495; // Kg*m^2
       }
     }
@@ -215,7 +217,9 @@ public static class Roll {
           1,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+          new TrapezoidProfile.Constraints(20.0, 200.0));
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.4);
+
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
       public static class PhysicalConstants {
@@ -236,16 +240,19 @@ public static class Pitch {
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          1,
+          20.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+          new TrapezoidProfile.Constraints(12.0, 400.0)); // Radians
+
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.88);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
         public static final double NET_REDUCTION = 92.85714286; // Yeah this is cursed
         public static final double MASS_KG = 2.16; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.101;
+        public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(13.875);
         public static final double MOI = 0.0200055915; // Kg*m^2
       }
     }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index eb07913..a199cb1 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -24,6 +24,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -37,9 +38,9 @@ public class CoralArm extends SubsystemBase {
         private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
         private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
         // sim motors
-        private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
-        private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
-        private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
+        private double simPivotVoltage = 0.0;
+        private double simRollVoltage = 0.0;
+        private double simPitchVoltage = 0.0;
 
         private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
         private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
@@ -54,17 +55,15 @@ public class CoralArm extends SubsystemBase {
                         Coral.Pivot.PhysicalConstants.NET_REDUCTION,
                         Coral.Pivot.PhysicalConstants.MOI,
                         Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-                        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE * -1),
-                        Units.degreesToRadians(Coral.Pivot.MAXIMUM_ANGLE),
-                        true,
-                        0);
+                        Coral.Pivot.MAXIMUM_ANGLE * -1,
+                        Coral.Pivot.MAXIMUM_ANGLE, true, 0);
         private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
                         Coral.Roll.PhysicalConstants.MOTOR,
                         Coral.Roll.PhysicalConstants.NET_REDUCTION,
                         Coral.Roll.PhysicalConstants.MOI,
                         Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-                        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE * -1),
-                        Units.degreesToRadians(Coral.Roll.MAXIMUM_ANGLE),
+                        Coral.Roll.MAXIMUM_ANGLE * -1,
+                        Coral.Roll.MAXIMUM_ANGLE,
                         false,
                         0);
         private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
@@ -72,8 +71,8 @@ public class CoralArm extends SubsystemBase {
                         Coral.Pitch.PhysicalConstants.NET_REDUCTION,
                         Coral.Pitch.PhysicalConstants.MOI,
                         Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-                        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE * -1),
-                        Units.degreesToRadians(Coral.Pitch.MAXIMUM_ANGLE),
+                        Coral.Pitch.MAXIMUM_ANGLE * -1,
+                        Coral.Pitch.MAXIMUM_ANGLE,
                         false,
                         0);
 
@@ -169,48 +168,45 @@ public void periodic() {
                 // run the motors
                 if (!Constants.Coral.Pivot.DBG_DISABLED)
                         pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+                simPivotVoltage = pivotPIDout + pivotFFout;
 
                 double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+                double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
                 SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
                 SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
                 if (!Constants.Coral.Roll.DBG_DISABLED)
-                        rollMotor.setVoltage(rollPIDout);
+                        rollMotor.setVoltage(rollPIDout + rollFFout);
+                simRollVoltage = rollPIDout;
 
                 double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+                double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
                 SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
                 SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout);
+                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
                 if (!Constants.Coral.Pitch.DBG_DISABLED)
-                        pitchMotor.setVoltage(pitchPIDout);
+                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+                simPitchVoltage = pitchPIDout;
         }
 
         @Override
         public void simulationPeriodic() {
                 // update physics
-                simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
-                simRollPhysics.setInput(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
-                simPitchPhysics.setInput(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
+                simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
+                simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
+                simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
+                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                                * RoboRioSim.getVInVoltage());
 
                 simPivotPhysics.update(0.02);
                 simRollPhysics.update(0.02);
                 simPitchPhysics.update(0.02);
-
-                // update sim objects
-                simPivotMotor.iterate(
-                                simPivotPhysics.getVelocityRadPerSec(),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
-                simRollMotor.iterate(
-                                simRollPhysics.getVelocityRadPerSec(),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
-                simPitchMotor.iterate(
-                                simPitchPhysics.getVelocityRadPerSec(),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
         }
 
         // this should be relative to straight upwards.
@@ -277,15 +273,11 @@ public double getPivotPositionDegrees() {
         }
 
         public double getRollPositionDegrees() {
-                return Units.radiansToDegrees(Robot.isReal()
-                                ? readRollEncoderPosition()
-                                : simRollEncoder.getPosition());
+                return Units.radiansToDegrees(readRollEncoderPosition());
         }
 
         public double getPitchPositionDegrees() {
-                return Units.radiansToDegrees(Robot.isReal()
-                                ? readPitchEncoderPosition()
-                                : simPitchEncoder.getPosition());
+                return Units.radiansToDegrees(readPitchEncoderPosition());
         }
 
         public boolean isPitchInPosition() {
@@ -301,10 +293,14 @@ public boolean isPivotInPosition() {
         }
 
         public double readPitchEncoderPosition() {
-                return pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS;
+                return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
+                                : MathUtil.angleModulus(
+                                                pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
         }
 
         public double readRollEncoderPosition() {
-                return rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS;
+                return Robot.isSimulation() ? simRollPhysics.getAngleRads()
+                                : MathUtil.angleModulus(
+                                                rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
         }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 40f46f0..d705180 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 
 public class CoralSubsystem extends SubsystemBase {
 
@@ -16,11 +17,11 @@ public class CoralSubsystem extends SubsystemBase {
     private final CoralElevator elevator = new CoralElevator();
 
     private final Mechanism2d coralMechanism = new Mechanism2d(2, 3);
-    private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.5, 0);
+    private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 0.0, 0.0);
     private final MechanismLigament2d elevatorMechanism = rootMechanism.append(
-            new MechanismLigament2d("Elevator", 1, 0));
+            new MechanismLigament2d("Elevator", Constants.Elevator.PhysicalParameters.BOTTOM_TO_FLOOR, 0));
     private final MechanismLigament2d pivotMechanism = elevatorMechanism.append(
-            new MechanismLigament2d("Coral", 1, 0));
+            new MechanismLigament2d("Coral", Constants.Coral.Pivot.PhysicalConstants.JOINT_LENGTH_METERS, 0));
 
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
@@ -83,7 +84,8 @@ private CoralIntakePresets(double intakePercentage) {
     @Override
     public void periodic() {
         // i have no idea what any of the getPositions output
-        elevatorMechanism.setLength(elevator.getPosition());
+        elevatorMechanism
+                .setLength(elevator.getPosition() + Constants.Elevator.PhysicalParameters.CORAL_PIVOT_VERTICAL_OFFSET);
         pivotMechanism.setAngle(arm.getPivotPositionDegrees());
 
         SmartDashboard.putData("Coral Mechanism", coralMechanism);

From e9ccf4e1c5328b152e8b4319be517fc2e3749312 Mon Sep 17 00:00:00 2001
From: Nifley <67334280+NifleySnifley@users.noreply.github.com>
Date: Mon, 10 Feb 2025 18:09:54 -0600
Subject: [PATCH 46/71] Sim pidtuning (#5)

* Pivot, etc. simulation & rudimentary tuning.

* Rough elevator PID tuning.
---
 simgui-ds.json                                |   5 +
 src/main/java/frc/robot/Constants.java        |  30 +-
 src/main/java/frc/robot/RobotContainer.java   |  17 +
 .../frc/robot/subsystems/coral/CoralArm.java  | 558 +++++++++---------
 .../robot/subsystems/coral/CoralElevator.java |  20 +-
 .../subsystems/coral/CoralSubsystem.java      |   2 +
 6 files changed, 339 insertions(+), 293 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index 5561453..1327cce 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d27854e..a0bad81 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -184,24 +184,24 @@ public static class Pivot {
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          1,
+          20.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
+          new TrapezoidProfile.Constraints(7.0, 20.0));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
           0.0,
-          0.75, // V
-          1.62, // V*s/rad
-          0.05// V*s^2/rad
+          0.4, // V
+          1.0, // V*s/rad
+          0.04// V*s^2/rad
       );
 
       public static class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getNeoVortex(1);
         public static final double NET_REDUCTION = 96.0;
-        public static final double MASS_KG = 4.8;
-        public static final double ARM_LENGTH_METERS = 0.51;
+        public static final double MASS_KG = 4.7727;
+        public static final double ARM_LENGTH_METERS = 0.510;
         public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(23.0);
         public static final double MOI = 0.2875548495; // Kg*m^2
       }
@@ -214,11 +214,11 @@ public static class Roll {
       public static boolean DBG_DISABLED = true;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          1,
+          10.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(20.0, 200.0));
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.4);
+          new TrapezoidProfile.Constraints(20.0, 150.0));
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.5);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
@@ -243,9 +243,9 @@ public static class Pitch {
           20.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 400.0)); // Radians
+          new TrapezoidProfile.Constraints(12.0, 200.0)); // Radians
 
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.88);
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.9);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
@@ -344,7 +344,7 @@ public static final class Follower {
     public static class PID {
       public static double kP = 20.0;
       public static double kI = 0.0;
-      public static double kD = 0.5;
+      public static double kD = 0.01;
       public static double MAX_VELOCITY = 3.20;
       // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
       // the elevator can make or exceed this
@@ -354,8 +354,8 @@ public static class PID {
     // TODO: PAD THE ELEVATOR!!!!!!!
     public static class FeedforwardConstants {
       public static double Ks = 0.0;
-      public static double Kv = 3.5;
-      public static double Ka = 0.08;
+      public static double Kv = 3.0;
+      public static double Ka = 0.04;
       public static double Kg = 0.35; // TODO: Check this!!!
     }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a86c539..d1202e0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -254,6 +254,23 @@ public boolean getAsBoolean() {
         })).onFalse(new InstantCommand(() -> {
             coralSubsystem.setCoralPresetPitch(CoralPresets.STOW);
         }));
+
+        debugXboxController.b().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetRoll(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetRoll(CoralPresets.STOW);
+        }));
+
+        debugXboxController.x().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPivot(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetPivot(CoralPresets.STOW);
+        }));
+        debugXboxController.y().onTrue(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetElevator(CoralPresets.LEVEL_4);
+        })).onFalse(new InstantCommand(() -> {
+            coralSubsystem.setCoralPresetElevator(CoralPresets.STOW);
+        }));
     }
 
     /**
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index a199cb1..368aec7 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -34,273 +34,293 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase {
-        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-        // sim motors
-        private double simPivotVoltage = 0.0;
-        private double simRollVoltage = 0.0;
-        private double simPitchVoltage = 0.0;
-
-        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
-        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
-        // sim encoders
-        private final SparkAbsoluteEncoderSim simRollEncoder = new SparkAbsoluteEncoderSim(rollMotor);
-        private final SparkAbsoluteEncoderSim simPitchEncoder = new SparkAbsoluteEncoderSim(pitchMotor);
-
-        // physics simulations
-        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-                        Coral.Pivot.PhysicalConstants.MOTOR,
-                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pivot.PhysicalConstants.MOI,
-                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Pivot.MAXIMUM_ANGLE * -1,
-                        Coral.Pivot.MAXIMUM_ANGLE, true, 0);
-        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-                        Coral.Roll.PhysicalConstants.MOTOR,
-                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
-                        Coral.Roll.PhysicalConstants.MOI,
-                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Roll.MAXIMUM_ANGLE * -1,
-                        Coral.Roll.MAXIMUM_ANGLE,
-                        false,
-                        0);
-        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-                        Coral.Pitch.PhysicalConstants.MOTOR,
-                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pitch.PhysicalConstants.MOI,
-                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Pitch.MAXIMUM_ANGLE * -1,
-                        Coral.Pitch.MAXIMUM_ANGLE,
-                        false,
-                        0);
-
-        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
-        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-        private final ProfiledPIDController rollPID = Coral.Roll.PID;
-        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-
-        private double pivotGoal = 0.0; // Rads
-        private double rollGoal = 0.0; // Rads
-        private double pitchGoal = 0.0; // Rads
-
-        public CoralArm() {
-                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
-                                .positionConversionFactor((Math.PI * 2) / 5.0)
-                                .velocityConversionFactor((Math.PI * 2) / 5.0);
-
-                pivotMotor.configure(
-                                pivotConfig.idleMode(IdleMode.kBrake).apply(
-                                                new EncoderConfig()
-                                                                .positionConversionFactor(
-                                                                                (1.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
-                                                                .velocityConversionFactor(
-                                                                                (60.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
-                                                                                                                                                           // ->
-                                // Rotations &
-                                // Seconds
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                rollMotor.configure(
-                                rollConfig.idleMode(IdleMode.kBrake)
-                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                pitchMotor.configure(pitchConfig
-                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-                /*
-                 * .apply(new EncoderConfig()
-                 * .positionConversionFactor(
-                 * (1.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
-                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
-                 */
-                // Not sure how to get
-
-                // Disabling this for now because we don't want the wrist wire bundle to explode
-                // rollPID.enableContinuousInput(-180, 180);
-
-                // TODO: Are these reasonable? who knows.
-                pivotPID.setTolerance(Units.degreesToRadians(0.5));
-                rollPID.setTolerance(Units.degreesToRadians(0.5));
-                pitchPID.setTolerance(Units.degreesToRadians(0.5));
-        }
-
-        @Override
-        public void periodic() {
-                // if entering the frame border
-                // && wrist is at neutral
-                if ((pivotPID.getGoal().position != pivotGoal)
-                                && (rollPID.atGoal() && pitchPID.atGoal())) {
-                        // allow arm to enter frame border
-                        pivotPID.setGoal(pivotGoal);
-                } else if ( // if exiting the frame border && has exited the frame border
-                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                        // set roll and pitch back to their goals
-                        rollPID.setGoal(rollGoal);
-                        pitchPID.setGoal(pitchGoal);
-                }
-
-                double pivotPosition = Units
-                                .rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-                double pivotFFout = pivotFeedforward.calculate(pivotPID.getSetpoint().position,
-                                pivotPID.getSetpoint().velocity);
-                double pivotPIDout = pivotPID.calculate(pivotPosition);
-
-                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
-                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
-                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-
-                // run the motors
-                if (!Constants.Coral.Pivot.DBG_DISABLED)
-                        pivotMotor.setVoltage(pivotPIDout + pivotFFout);
-                simPivotVoltage = pivotPIDout + pivotFFout;
-
-                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
-                double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
-                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
-                if (!Constants.Coral.Roll.DBG_DISABLED)
-                        rollMotor.setVoltage(rollPIDout + rollFFout);
-                simRollVoltage = rollPIDout;
-
-                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
-                double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
-                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
-                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
-                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
-                if (!Constants.Coral.Pitch.DBG_DISABLED)
-                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-                simPitchVoltage = pitchPIDout;
-        }
-
-        @Override
-        public void simulationPeriodic() {
-                // update physics
-                simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
-                simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
-                simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
-                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
-                                * RoboRioSim.getVInVoltage());
-
-                simPivotPhysics.update(0.02);
-                simRollPhysics.update(0.02);
-                simPitchPhysics.update(0.02);
-        }
-
-        // this should be relative to straight upwards.
-        // i.e. 0 should be straight vertical,
-        // 20 would be to the right, -20 to the left
-        public void setPivotGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pivotGoal = MathUtil.clamp(goalRads,
-                                Coral.Pivot.MAXIMUM_ANGLE, -1 * Coral.Pivot.MAXIMUM_ANGLE);
-                // if passing through the frame border
-                if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
-                                && (Math.abs(this.getPivotPositionDegrees()) < Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                        // set the pivot to stop at the frame border to allow the wrist to move
-                        // neutral
-                        pivotPID.setGoal(
-                                        Coral.Pivot.FRAME_BORDER_ANGLE
-                                                        * (pivotGoal < 0 ? -1 : 1));
-                        rollPID.setGoal(0);
-                        pitchPID.setGoal(0);
-                } else {
-                        pivotPID.setGoal(pivotGoal);
-                }
-        }
-
-        // similarly, this ranges from -180 to 180
-        public void setRollGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-                // if outside of frame border
-                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-                        // start moving to goal
-                        rollPID.setGoal(rollGoal);
-                }
-
-        }
-
-        public void setPitchGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-                // if outside the frame border
-                if (Math.abs(this.getPivotPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE) {
-                        // start moving to goal
-                        pitchPID.setGoal(pitchGoal);
-                }
-        }
-
-        public double getPivotGoalDegrees() {
-                return Units.radiansToDegrees(pivotGoal);
-        }
-
-        public double getRollGoalDegrees() {
-                return Units.radiansToDegrees(rollGoal);
-        }
-
-        public double getPitchGoalDegrees() {
-                return Units.radiansToDegrees(pitchGoal);
-        }
-
-        public double getPivotPositionDegrees() {
-                return Units.rotationsToDegrees(Robot.isReal()
-                                ? (pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))
-                                : simPivotPhysics.getAngleRads());
-        }
-
-        public double getRollPositionDegrees() {
-                return Units.radiansToDegrees(readRollEncoderPosition());
-        }
-
-        public double getPitchPositionDegrees() {
-                return Units.radiansToDegrees(readPitchEncoderPosition());
-        }
-
-        public boolean isPitchInPosition() {
-                return pitchPID.atGoal();
-        }
-
-        public boolean isRollInPosition() {
-                return rollPID.atGoal();
-        }
-
-        public boolean isPivotInPosition() {
-                return pivotPID.atGoal();
-        }
-
-        public double readPitchEncoderPosition() {
-                return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
-                                : MathUtil.angleModulus(
-                                                pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
-        }
-
-        public double readRollEncoderPosition() {
-                return Robot.isSimulation() ? simRollPhysics.getAngleRads()
-                                : MathUtil.angleModulus(
-                                                rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
-        }
+    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+    // sim motors
+    private double simPivotVoltage = 0.0;
+    private double simRollVoltage = 0.0;
+    private double simPitchVoltage = 0.0;
+
+    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+    private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+    private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
+    // sim encoders
+    // private final SparkAbsoluteEncoderSim simRollEncoder = new
+    // SparkAbsoluteEncoderSim(rollMotor);
+    // private final SparkAbsoluteEncoderSim simPitchEncoder = new
+    // SparkAbsoluteEncoderSim(pitchMotor);
+
+    // physics simulations
+    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+            Coral.Pivot.PhysicalConstants.MOTOR,
+            Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+            Coral.Pivot.PhysicalConstants.MOI,
+            Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+            0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
+            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI - 0.05);
+    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+            Coral.Roll.PhysicalConstants.MOTOR,
+            Coral.Roll.PhysicalConstants.NET_REDUCTION,
+            Coral.Roll.PhysicalConstants.MOI,
+            Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Roll.MAXIMUM_ANGLE * -1,
+            Coral.Roll.MAXIMUM_ANGLE,
+            false,
+            0);
+    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+            Coral.Pitch.PhysicalConstants.MOTOR,
+            Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+            Coral.Pitch.PhysicalConstants.MOI,
+            Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Pitch.MAXIMUM_ANGLE * -1,
+            Coral.Pitch.MAXIMUM_ANGLE,
+            false,
+            0);
+
+    private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+    private final ProfiledPIDController rollPID = Coral.Roll.PID;
+    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+    private double pivotGoal = 0.0; // Rads
+    private double rollGoal = 0.0; // Rads
+    private double pitchGoal = 0.0; // Rads
+
+    public CoralArm() {
+        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
+                .positionConversionFactor((Math.PI * 2) / 5.0)
+                .velocityConversionFactor((Math.PI * 2) / 5.0);
+
+        pivotMotor.configure(
+                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                        new EncoderConfig()
+                                .positionConversionFactor(
+                                        (1.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                .velocityConversionFactor(
+                                        (60.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                           // ->
+                // Rotations &
+                // Seconds
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        rollMotor.configure(
+                rollConfig.idleMode(IdleMode.kBrake)
+                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        pitchMotor.configure(pitchConfig
+                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        /*
+         * .apply(new EncoderConfig()
+         * .positionConversionFactor(
+         * (1.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+         * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+         */
+        // Not sure how to get
+
+        // Disabling this for now because we don't want the wrist wire bundle to explode
+        // rollPID.enableContinuousInput(-180, 180);
+
+        // TODO: Are these reasonable? who knows.
+        pivotPID.setTolerance(Units.degreesToRadians(0.5));
+        rollPID.setTolerance(Units.degreesToRadians(0.5));
+        pitchPID.setTolerance(Units.degreesToRadians(0.5));
+    }
+
+    @Override
+    public void periodic() {
+        // if entering the frame border
+        // && wrist is at neutral
+        /*
+         * if ((pivotPID.getGoal().position != pivotGoal)
+         * && (rollPID.atGoal() && pitchPID.atGoal())) {
+         * // allow arm to enter frame border
+         * pivotPID.setGoal(pivotGoal);
+         * } else if ( // if exiting the frame border && has exited the frame border
+         * ((rollGoal != rollPID.getGoal().position) || (pitchGoal !=
+         * pitchPID.getGoal().position))
+         * && (Math.abs(this.getPitchPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+         * // set roll and pitch back to their goals
+         * rollPID.setGoal(rollGoal);
+         * pitchPID.setGoal(pitchGoal);
+         * }
+         */
+
+        double pivotPosition = readPivotEncoderPosition();
+        double pivotFFout = pivotFeedforward.calculate(
+                Math.PI * 0.5 + pivotPosition,
+                pivotPID.getSetpoint().velocity);
+        double pivotPIDout = pivotPID.calculate(pivotPosition);
+
+        SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+        SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+        SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+
+        // run the motors
+        if (!Constants.Coral.Pivot.DBG_DISABLED)
+            pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+        simPivotVoltage = pivotPIDout + pivotFFout;
+
+        double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+        double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+        SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
+        if (!Constants.Coral.Roll.DBG_DISABLED)
+            rollMotor.setVoltage(rollPIDout + rollFFout);
+        simRollVoltage = rollPIDout;
+
+        double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+        double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+        SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+        SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+        SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
+        if (!Constants.Coral.Pitch.DBG_DISABLED)
+            pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+        simPitchVoltage = pitchPIDout;
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
+        simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
+        simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
+        SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                * RoboRioSim.getVInVoltage());
+
+        simPivotPhysics.update(0.02);
+        simRollPhysics.update(0.02);
+        simPitchPhysics.update(0.02);
+    }
+
+    // this should be relative to straight upwards.
+    // i.e. 0 should be straight vertical,
+    // 20 would be to the right, -20 to the left
+    public void setPivotGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pivotGoal = MathUtil.clamp(goalRads,
+                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
+        // if passing through the frame border
+        /*
+         * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+         * && (Math.abs(this.getPivotPositionDegrees()) <
+         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+         * // set the pivot to stop at the frame border to allow the wrist to move
+         * // neutral
+         * pivotPID.setGoal(
+         * Coral.Pivot.FRAME_BORDER_ANGLE
+         * (pivotGoal < 0 ? -1 : 1));
+         * rollPID.setGoal(0);
+         * pitchPID.setGoal(0);
+         * } else {
+         * pivotPID.setGoal(pivotGoal);
+         * }
+         */
+
+        pivotPID.setGoal(pivotGoal);
+    }
+
+    // similarly, this ranges from -180 to 180
+    public void setRollGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+        /*
+         * // if outside of frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * rollPID.setGoal(rollGoal);
+         * }
+         */
+        rollPID.setGoal(rollGoal);
+    }
+
+    public void setPitchGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+        /*
+         * // if outside the frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * pitchPID.setGoal(pitchGoal);
+         * }
+         */
+        pitchPID.setGoal(pitchGoal);
+    }
+
+    public double getPivotGoalDegrees() {
+        return Units.radiansToDegrees(pivotGoal);
+    }
+
+    public double getRollGoalDegrees() {
+        return Units.radiansToDegrees(rollGoal);
+    }
+
+    public double getPitchGoalDegrees() {
+        return Units.radiansToDegrees(pitchGoal);
+    }
+
+    public double getPivotPositionDegrees() {
+        return Units.radiansToDegrees(readPivotEncoderPosition());
+    }
+
+    public double getRollPositionDegrees() {
+        return Units.radiansToDegrees(readRollEncoderPosition());
+    }
+
+    public double getPitchPositionDegrees() {
+        return Units.radiansToDegrees(readPitchEncoderPosition());
+    }
+
+    public boolean isPitchInPosition() {
+        return pitchPID.atGoal();
+    }
+
+    public boolean isRollInPosition() {
+        return rollPID.atGoal();
+    }
+
+    public boolean isPivotInPosition() {
+        return pivotPID.atGoal();
+    }
+
+    public double readPitchEncoderPosition() {
+        return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
+                : MathUtil.angleModulus(
+                        pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
+    }
+
+    public double readRollEncoderPosition() {
+        return Robot.isSimulation() ? simRollPhysics.getAngleRads()
+                : MathUtil.angleModulus(
+                        rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
+    }
+
+    public double readPivotEncoderPosition() {
+        return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
+                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index a1557b9..7bdcb83 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -10,7 +10,6 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkFlexConfig;
 
-import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -35,6 +34,7 @@ public class CoralElevator extends SubsystemBase {
     // sim motors
     private final SparkFlexSim simLeaderMotor = new SparkFlexSim(leaderMotor, DCMotor.getNeoVortex(1));
     private final SparkFlexSim simFollowerMotor = new SparkFlexSim(followerMotor, DCMotor.getNeoVortex(1));
+    private double simOutputVoltage = 0.0;
 
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
@@ -113,20 +113,23 @@ public void periodic() {
             lastTime = Timer.getFPGATimestamp();
             lastVelocity = setpoint.velocity;
 
-            double pid_out = elevatorPID.calculate(leaderEncoder.getPosition());
-            double output = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime)
-                    + pid_out;
+            double pid_out = elevatorPID.calculate(getPosition());
+            double ff_out = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime);
+            double output = ff_out + pid_out;
 
-            SmartDashboard.putNumber("Elevator/Output", output);
-            SmartDashboard.putNumber("Elevator/position", leaderEncoder.getPosition());
+            SmartDashboard.putNumber("Elevator/output", output);
+            SmartDashboard.putNumber("Elevator/position", getPosition());
             SmartDashboard.putNumber("Elevator/target", setpoint.position);
             SmartDashboard.putNumber("Elevator/goal", elevatorPID.getGoal().position);
             SmartDashboard.putNumber("Elevator/pid_out", pid_out);
+            SmartDashboard.putNumber("Elevator/ff_out", ff_out);
 
             if (!Constants.Elevator.DBG_DISABLED) {
                 leaderMotor.setVoltage(output);
                 followerMotor.setVoltage(output);
             }
+
+            simOutputVoltage = output;
         } else {
             if (!Constants.Elevator.DBG_DISABLED) {
                 // slowly move down to zero
@@ -139,8 +142,7 @@ public void periodic() {
     @Override
     public void simulationPeriodic() {
         // update physics
-        simElevator.setInput(
-                (leaderMotor.getAppliedOutput() + followerMotor.getAppliedOutput()) * 0.5 * RoboRioSim.getVInVoltage());
+        simElevator.setInput(MathUtil.clamp(simOutputVoltage, -12.0, 12.0));
         simElevator.update(0.02);
 
         // update sim objects
@@ -165,7 +167,7 @@ public double getGoalPosition() {
     public double getPosition() {
         return Robot.isReal()
                 ? leaderEncoder.getPosition()
-                : simLeaderEncoder.getPosition();
+                : simElevator.getPositionMeters();
     }
 
     public boolean isInPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index d705180..e5e8a0b 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -3,6 +3,7 @@
 // import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -121,6 +122,7 @@ public boolean isElevatorInPosition() {
     }
 
     public void setCoralPresetPivot(CoralPresets preset) {
+        SmartDashboard.putNumber("Pivot Pre", preset.pivotAngleDeg);
         arm.setPivotGoalDegrees(
                 preset.pivotAngleDeg
                         * (mirrorSetting.isMirrored ? -1 : 1));

From 87c35bbe0247549cb9a16457aac40a6bef9b572f Mon Sep 17 00:00:00 2001
From: Spoopr <spoopr@users.noreply.github.com>
Date: Mon, 10 Feb 2025 20:51:30 -0600
Subject: [PATCH 47/71] cleanup code and reitegrate simulation objects

---
 src/main/java/frc/robot/Constants.java        |  1 -
 src/main/java/frc/robot/RobotContainer.java   | 10 +-
 .../coral/motion/WaitPivotClearance.java      | 19 ----
 .../frc/robot/subsystems/algae/AlgaeArm.java  |  1 +
 .../robot/subsystems/algae/AlgaeIntake.java   |  9 +-
 .../frc/robot/subsystems/coral/CoralArm.java  | 96 ++++++++++++-------
 .../robot/subsystems/coral/CoralElevator.java | 15 ++-
 .../subsystems/coral/CoralSubsystem.java      |  3 -
 8 files changed, 78 insertions(+), 76 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a0bad81..3734e24 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import java.util.PropertyResourceBundle;
 
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d1202e0..1b587c0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,7 +7,6 @@
 import frc.robot.Constants.*;
 import frc.robot.commands.*;
 import frc.robot.commands.algae.ShootAlgaeCommand;
-import frc.robot.commands.coral.CoralWristFollowCommand;
 import frc.robot.commands.coral.IntakeCoralCommand;
 import frc.robot.commands.coral.PurgeCoralIntakeCommand;
 import frc.robot.commands.coral.ScoreCoralCommand;
@@ -16,7 +15,6 @@
 import frc.robot.commands.coral.motion.MovePivot;
 import frc.robot.commands.coral.motion.MoveRoll;
 import frc.robot.commands.coral.motion.StowArm;
-import frc.robot.commands.coral.motion.WaitPivotClearance;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
@@ -30,7 +28,6 @@
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -111,9 +108,8 @@ private Command getGoToLockedPresetCommand() {
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
                 .andThen(new ParallelCommandGroup(
                         new MovePivot(coralSubsystem, currentLockedPresetSupplier),
-                        new WaitPivotClearance(coralSubsystem)
-                                .andThen(new MoveRoll(coralSubsystem, currentLockedPresetSupplier)
-                                        .alongWith(new MovePitch(coralSubsystem, currentLockedPresetSupplier)))));
+                        new MoveRoll(coralSubsystem, currentLockedPresetSupplier),
+                        new MovePitch(coralSubsystem, currentLockedPresetSupplier)));
     }
 
     private Command getStowCommand() {
@@ -167,7 +163,6 @@ private void configureBindings() {
         }));
 
         operatorXbox.rightTrigger().whileTrue(new InstantCommand(() -> {
-            // coralSubsystem.setCoralPreset(currentCoralPreset);
             lockCoralArmPreset(selectedScoringPreset);
         }).andThen(getGoToLockedPresetCommand())).whileFalse(getStowCommand());
 
@@ -201,7 +196,6 @@ public boolean getAsBoolean() {
             lockCoralArmPreset(CoralPresets.INTAKE);
         }).andThen(getGoToLockedPresetCommand()).andThen(new IntakeCoralCommand(coralSubsystem))
                 .andThen(getStowCommand())).whileFalse(getStowCommand());
-        // .onFalse(normalDrive);
 
         // purge coral
         operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java b/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java
deleted file mode 100644
index 341b80a..0000000
--- a/src/main/java/frc/robot/commands/coral/motion/WaitPivotClearance.java
+++ /dev/null
@@ -1,19 +0,0 @@
-package frc.robot.commands.coral.motion;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.subsystems.coral.CoralSubsystem;
-
-public class WaitPivotClearance extends Command {
-    private CoralSubsystem coralSub;
-
-    public WaitPivotClearance(CoralSubsystem coralSub) {
-        this.coralSub = coralSub;
-        // addRequirements(coralSub);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return Math.abs(coralSub.getPivotPositionDegrees()) > Constants.Coral.Pivot.FRAME_BORDER_ANGLE;
-    }
-}
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index 33fd1d0..2b5afad 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -56,6 +56,7 @@ public void periodic() {
         );
 
         pivotMotor.set(output);
+        simPivotMotor.setAppliedOutput(output);
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index dfde316..b294051 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -40,14 +40,17 @@ public class AlgaeIntake extends SubsystemBase {
 
     @Override
     public void periodic() {
+        double output;
         // if no algae
         if (intakeBeambreak.get()) {
-            double output = intakeProfile.calculate(outputPercentage);
-            intakeMotor.set(output);
+            output = intakeProfile.calculate(outputPercentage);
         } else {
             // hold
-            intakeMotor.set(Math.min(0.05, outputPercentage));
+            output = Math.min(0.05, outputPercentage);
         }
+
+        intakeMotor.set(output);
+        simIntakeMotor.setAppliedOutput(output);
     }
     
     @Override
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 368aec7..b3d67f9 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -1,9 +1,7 @@
 package frc.robot.subsystems.coral;
 
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.sim.SparkAbsoluteEncoderSim;
+import com.revrobotics.sim.SparkAnalogSensorSim;
 import com.revrobotics.sim.SparkFlexSim;
 import com.revrobotics.sim.SparkMaxSim;
 import com.revrobotics.spark.SparkAnalogSensor;
@@ -12,19 +10,16 @@
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.AbsoluteEncoderConfig;
 import com.revrobotics.spark.config.AnalogSensorConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
-import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -38,18 +33,17 @@ public class CoralArm extends SubsystemBase {
     private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
     // sim motors
-    private double simPivotVoltage = 0.0;
-    private double simRollVoltage = 0.0;
-    private double simPitchVoltage = 0.0;
+    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);    
+
 
     private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
     private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
     private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
     // sim encoders
-    // private final SparkAbsoluteEncoderSim simRollEncoder = new
-    // SparkAbsoluteEncoderSim(rollMotor);
-    // private final SparkAbsoluteEncoderSim simPitchEncoder = new
-    // SparkAbsoluteEncoderSim(pitchMotor);
+    private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
+    private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
 
     // physics simulations
     private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
@@ -141,20 +135,23 @@ public CoralArm() {
     public void periodic() {
         // if entering the frame border
         // && wrist is at neutral
+        if (
+            (pivotPID.getGoal().position != pivotGoal)
+            && (rollPID.atGoal() && pitchPID.atGoal())
+        ) {
+            // allow arm to enter frame border
+            pivotPID.setGoal(pivotGoal);
+        } else if ( // if exiting the frame border && has exited the frame border
+            ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+            && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)
+        ) {
+            // set roll and pitch back to their goals
+            rollPID.setGoal(rollGoal);
+            pitchPID.setGoal(pitchGoal);
+        }
+        
         /*
-         * if ((pivotPID.getGoal().position != pivotGoal)
-         * && (rollPID.atGoal() && pitchPID.atGoal())) {
-         * // allow arm to enter frame border
-         * pivotPID.setGoal(pivotGoal);
-         * } else if ( // if exiting the frame border && has exited the frame border
-         * ((rollGoal != rollPID.getGoal().position) || (pitchGoal !=
-         * pitchPID.getGoal().position))
-         * && (Math.abs(this.getPitchPositionDegrees()) >
-         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
-         * // set roll and pitch back to their goals
-         * rollPID.setGoal(rollGoal);
-         * pitchPID.setGoal(pitchGoal);
-         * }
+         * run the motors
          */
 
         double pivotPosition = readPivotEncoderPosition();
@@ -162,18 +159,15 @@ public void periodic() {
                 Math.PI * 0.5 + pivotPosition,
                 pivotPID.getSetpoint().velocity);
         double pivotPIDout = pivotPID.calculate(pivotPosition);
-
         SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
         SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
         SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
         SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
         SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
         SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-
-        // run the motors
         if (!Constants.Coral.Pivot.DBG_DISABLED)
             pivotMotor.setVoltage(pivotPIDout + pivotFFout);
-        simPivotVoltage = pivotPIDout + pivotFFout;
+        simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
 
         double rollPIDout = rollPID.calculate(readRollEncoderPosition());
         double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
@@ -185,7 +179,7 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
         if (!Constants.Coral.Roll.DBG_DISABLED)
             rollMotor.setVoltage(rollPIDout + rollFFout);
-        simRollVoltage = rollPIDout;
+        simRollMotor.setAppliedOutput(rollPIDout);
 
         double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
         double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
@@ -197,21 +191,49 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
         if (!Constants.Coral.Pitch.DBG_DISABLED)
             pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-        simPitchVoltage = pitchPIDout;
+        simPitchMotor.setAppliedOutput(pitchPIDout);
     }
 
     @Override
     public void simulationPeriodic() {
         // update physics
-        simPivotPhysics.setInput(MathUtil.clamp(simPivotVoltage, -12.0, 12.0));
-        simRollPhysics.setInput(MathUtil.clamp(simRollVoltage, -12.0, 12.0));
-        simPitchPhysics.setInput(MathUtil.clamp(simPitchVoltage, -12.0, 12.0));
+        simPivotPhysics.setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+        simRollPhysics.setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+        simPitchPhysics.setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
         SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
                 * RoboRioSim.getVInVoltage());
 
         simPivotPhysics.update(0.02);
         simRollPhysics.update(0.02);
         simPitchPhysics.update(0.02);
+
+        // update sim objects        
+        simPivotMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+        simRollMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+        simPitchMotor.iterate(
+            Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
+            RoboRioSim.getVInVoltage(),
+            0.02
+        );
+
+        simRollEncoder.setPosition(simRollPhysics.getAngleRads() / 5.0);
+        simRollEncoder.iterate(
+            simRollPhysics.getVelocityRadPerSec(),
+            0.02
+        );
+        simPitchEncoder.setPosition(simPitchPhysics.getAngleRads() / 5.0);
+        simPitchEncoder.iterate(
+            simPitchPhysics.getVelocityRadPerSec(),
+            0.02
+        );
     }
 
     // this should be relative to straight upwards.
@@ -307,13 +329,13 @@ public boolean isPivotInPosition() {
     }
 
     public double readPitchEncoderPosition() {
-        return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
+        return Robot.isSimulation() ? simPitchEncoder.getPosition()
                 : MathUtil.angleModulus(
                         pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
     }
 
     public double readRollEncoderPosition() {
-        return Robot.isSimulation() ? simRollPhysics.getAngleRads()
+        return Robot.isSimulation() ? simRollEncoder.getPosition()
                 : MathUtil.angleModulus(
                         rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
     }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index 7bdcb83..7f5a5bd 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -34,7 +34,6 @@ public class CoralElevator extends SubsystemBase {
     // sim motors
     private final SparkFlexSim simLeaderMotor = new SparkFlexSim(leaderMotor, DCMotor.getNeoVortex(1));
     private final SparkFlexSim simFollowerMotor = new SparkFlexSim(followerMotor, DCMotor.getNeoVortex(1));
-    private double simOutputVoltage = 0.0;
 
     private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder();
     private final RelativeEncoder followerEncoder = followerMotor.getEncoder();
@@ -98,8 +97,13 @@ public CoralElevator() {
     public void periodic() {
         // check if needs to be zeroed and is at zero
         // TODO: ######################### PLACEHOLDERS AGAIN #########################
-        if (!isZeroed && (leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2 > 20
-                || Robot.isSimulation()) {
+        if (
+            !isZeroed
+            && (
+                ((leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2) > 20
+                || Robot.isSimulation()
+            )
+        ) {
             leaderEncoder.setPosition(0);
             isZeroed = true;
         }
@@ -129,7 +133,8 @@ public void periodic() {
                 followerMotor.setVoltage(output);
             }
 
-            simOutputVoltage = output;
+            simLeaderMotor.setAppliedOutput(output);
+            simFollowerMotor.setAppliedOutput(output);
         } else {
             if (!Constants.Elevator.DBG_DISABLED) {
                 // slowly move down to zero
@@ -142,7 +147,7 @@ public void periodic() {
     @Override
     public void simulationPeriodic() {
         // update physics
-        simElevator.setInput(MathUtil.clamp(simOutputVoltage, -12.0, 12.0));
+        simElevator.setInput(MathUtil.clamp(simLeaderMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
         simElevator.update(0.02);
 
         // update sim objects
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index e5e8a0b..8371d39 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -1,9 +1,6 @@
 package frc.robot.subsystems.coral;
 
 // import edu.wpi.first.epilogue.Epilogue;
-import edu.wpi.first.epilogue.Logged;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;

From 3666c2e33e482450968fe3a8fd5a7cba988499a4 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 02:38:12 -0600
Subject: [PATCH 48/71] Tuned roll, pitch, pivot, drives!

---
 src/main/java/frc/robot/Constants.java        | 36 ++++++++++---------
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../frc/robot/subsystems/coral/CoralArm.java  | 33 +++++++++++------
 .../robot/subsystems/coral/CoralIntake.java   |  4 +++
 .../subsystems/coral/CoralSubsystem.java      |  9 ++---
 5 files changed, 52 insertions(+), 32 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a0bad81..a0b8dd9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -90,7 +90,7 @@ public static class SwerveModuleConstants {
     public static final int FL_DRIVE_ID = 34;
     public static final int FL_STEER_ID = 4;
     public static final int FL_ABSOLUTE_ENCODER_PORT = 54;
-    public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(0.389893) + Math.PI * 0.5 + Math.PI;
+    public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(-0.310303) + Math.PI * 0.5 + Math.PI;
     public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean FL_MOTOR_REVERSED = true;
 
@@ -98,7 +98,7 @@ public static class SwerveModuleConstants {
     public static final int FR_DRIVE_ID = 31;
     public static final int FR_STEER_ID = 1;
     public static final int FR_ABSOLUTE_ENCODER_PORT = 51;
-    public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(0.323730) + Math.PI * 0.5 + Math.PI;
+    public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(-0.253906) + Math.PI * 0.5 + Math.PI;
     public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean FR_MOTOR_REVERSED = true;
 
@@ -106,7 +106,7 @@ public static class SwerveModuleConstants {
     public static final int BR_DRIVE_ID = 32;
     public static final int BR_STEER_ID = 2;
     public static final int BR_ABSOLUTE_ENCODER_PORT = 52;
-    public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(-0.360107) + Math.PI * 0.5 + Math.PI;
+    public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(0.353027) + Math.PI * 0.5 + Math.PI;
     public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean BR_MOTOR_REVERSED = true;
 
@@ -114,7 +114,7 @@ public static class SwerveModuleConstants {
     public static final int BL_DRIVE_ID = 33;
     public static final int BL_STEER_ID = 3;
     public static final int BL_ABSOLUTE_ENCODER_PORT = 53;
-    public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(0.399902) + Math.PI * 0.5 + Math.PI;
+    public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(-0.134033) + Math.PI * 0.5 + Math.PI;
     public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = true;
     public static final boolean BL_MOTOR_REVERSED = true;
 
@@ -175,7 +175,7 @@ public static class Coral {
     public static class Pivot {
       public static final int MOTOR_PORT = 14;
       public static final int ENCODER_PORT = 28;
-      public static boolean DBG_DISABLED = true;
+      public static boolean DBG_DISABLED = false;
 
       public static final boolean ENCODER_INVERTED = false;
 
@@ -184,17 +184,17 @@ public static class Pivot {
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          20.0,
-          0.0,
+          15.0,
           0.0,
+          0.1,
           new TrapezoidProfile.Constraints(7.0, 20.0));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
           0.0,
           0.4, // V
-          1.0, // V*s/rad
-          0.04// V*s^2/rad
+          0.4, // 1.0, // V*s/rad
+          0.00// V*s^2/rad
       );
 
       public static class PhysicalConstants {
@@ -209,16 +209,17 @@ public static class PhysicalConstants {
 
     public static class Roll {
       public static final int MOTOR_PORT = 15;
+      public static final boolean MOTOR_INVERTED = false;
       public static final boolean ENCODER_INVERTED = false;
-      public static final double ENCODER_OFFSET_RADS = 0.0;
-      public static boolean DBG_DISABLED = true;
+      public static final double ENCODER_OFFSET_VOLTS = -1.85;
+      public static boolean DBG_DISABLED = false;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          10.0,
+          2.5,
           0.0,
           0.0,
           new TrapezoidProfile.Constraints(20.0, 150.0));
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.5);
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.2);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
@@ -233,19 +234,20 @@ public static class PhysicalConstants {
 
     public static class Pitch {
       public static final int MOTOR_PORT = 16;
-      public static final boolean ENCODER_INVERTED = false;
-      public static final double ENCODER_OFFSET_RADS = 0.0;
+      public static final boolean MOTOR_INVERTED = true;
+      public static final boolean ENCODER_INVERTED = true;
+      public static final double ENCODER_OFFSET_VOLTS = -2.7;
       public static boolean DBG_DISABLED = false;
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          20.0,
+          5.0,
           0.0,
           0.0,
           new TrapezoidProfile.Constraints(12.0, 200.0)); // Radians
 
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.9);
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d1202e0..2a3b468 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -250,7 +250,7 @@ public boolean getAsBoolean() {
 
         /////////////////// DEBUGGING //////////////////
         debugXboxController.a().onTrue(new InstantCommand(() -> {
-            coralSubsystem.setCoralPresetPitch(CoralPresets.LEVEL_4);
+            coralSubsystem.setCoralPresetPitch(CoralPresets.INTAKE);
         })).onFalse(new InstantCommand(() -> {
             coralSubsystem.setCoralPresetPitch(CoralPresets.STOW);
         }));
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 368aec7..f917bf2 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -15,6 +15,7 @@
 import com.revrobotics.spark.config.AbsoluteEncoderConfig;
 import com.revrobotics.spark.config.AnalogSensorConfig;
 import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -23,6 +24,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
@@ -91,10 +93,11 @@ public class CoralArm extends SubsystemBase {
     private double rollGoal = 0.0; // Rads
     private double pitchGoal = 0.0; // Rads
 
+    LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
+    LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
+
     public CoralArm() {
-        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig()
-                .positionConversionFactor((Math.PI * 2) / 5.0)
-                .velocityConversionFactor((Math.PI * 2) / 5.0);
+        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
 
         pivotMotor.configure(
                 pivotConfig.idleMode(IdleMode.kBrake).apply(
@@ -111,13 +114,18 @@ public CoralArm() {
                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         rollMotor.configure(
                 rollConfig.idleMode(IdleMode.kBrake)
-                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)),
+                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
+                        .apply(new SparkMaxConfig().inverted(
+                                Constants.Coral.Pitch.MOTOR_INVERTED)),
                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         pitchMotor.configure(pitchConfig
                 .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)),
+                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
+                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
                 ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
+        pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
+
         /*
          * .apply(new EncoderConfig()
          * .positionConversionFactor(
@@ -157,7 +165,7 @@ public void periodic() {
          * }
          */
 
-        double pivotPosition = readPivotEncoderPosition();
+        double pivotPosition = pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
         double pivotFFout = pivotFeedforward.calculate(
                 Math.PI * 0.5 + pivotPosition,
                 pivotPID.getSetpoint().velocity);
@@ -174,6 +182,7 @@ public void periodic() {
         if (!Constants.Coral.Pivot.DBG_DISABLED)
             pivotMotor.setVoltage(pivotPIDout + pivotFFout);
         simPivotVoltage = pivotPIDout + pivotFFout;
+        // pivotMotor.set(0.02);
 
         double rollPIDout = rollPID.calculate(readRollEncoderPosition());
         double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
@@ -308,14 +317,18 @@ public boolean isPivotInPosition() {
 
     public double readPitchEncoderPosition() {
         return Robot.isSimulation() ? simPitchPhysics.getAngleRads()
-                : MathUtil.angleModulus(
-                        pitchEncoder.getPosition() + Constants.Coral.Pitch.ENCODER_OFFSET_RADS);
+                : MathUtil
+                        .angleModulus(((pitchEncFilter.calculate(pitchEncoder.getPosition())
+                                + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS) / 3.3)
+                                * 2 * Math.PI);
     }
 
     public double readRollEncoderPosition() {
         return Robot.isSimulation() ? simRollPhysics.getAngleRads()
-                : MathUtil.angleModulus(
-                        rollEncoder.getPosition() + Constants.Coral.Roll.ENCODER_OFFSET_RADS);
+                : MathUtil
+                        .angleModulus(((rollEncFilter.calculate(rollEncoder.getPosition())
+                                + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
+                                * 2 * Math.PI);
     }
 
     public double readPivotEncoderPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 63ba661..fa3ec61 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,7 +1,9 @@
 package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 
@@ -36,6 +38,8 @@ public class CoralIntake extends SubsystemBase {
     public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
         intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(20.0));
+        intakeMotor.getConfigurator()
+                .apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive));
     }
 
     private double outputPercentage = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index e5e8a0b..4eca028 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -2,6 +2,7 @@
 
 // import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -26,10 +27,10 @@ public class CoralSubsystem extends SubsystemBase {
 
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
-        LEVEL_2(0.237, 19.032, -90, 105.968),
-        LEVEL_3(0.640, 19.032, -90, 105.968),
-        LEVEL_4(1.342, 23.238, -90, 111.762),
-        INTAKE(0.0, 9.559, -90, 50.44),
+        LEVEL_2(0.237, 19.032, 90, 105.968),
+        LEVEL_3(0.640, 19.032, 90, 105.968),
+        LEVEL_4(1.342, 23.238, 90, 111.762),
+        INTAKE(0.0, 9.559, 90, 50.44),
         STOW(0.0, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);

From 6efbd573ba3886f2f09c4015d99be9b0882485a5 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 03:53:38 -0600
Subject: [PATCH 49/71] Tuned elevator, pivot PIDs. intake working.

---
 src/main/java/frc/robot/Constants.java        | 31 +++++++++--------
 src/main/java/frc/robot/RobotContainer.java   | 34 +++++++++++++------
 .../commands/coral/IntakeCoralCommand.java    |  8 ++++-
 .../robot/commands/coral/motion/StowArm.java  |  3 +-
 .../frc/robot/subsystems/coral/CoralArm.java  |  6 ++--
 .../robot/subsystems/coral/CoralIntake.java   | 25 ++++++++------
 .../subsystems/coral/CoralSubsystem.java      | 10 ++++--
 7 files changed, 73 insertions(+), 44 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a0b8dd9..b1864e9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -261,14 +261,15 @@ public static class PhysicalConstants {
 
     public static class Intake {
       public static final int MOTOR_PORT = 17;
-      public static boolean DBG_DISABLED = true;
+      public static boolean DBG_DISABLED = false;
+      public static boolean MOTOR_INVERTED = true;
 
-      public static final double POSITIVE_RATE_LIMIT = 20.0; // Fast shoot
-      public static final double NEGATIVE_RATE_LIMIT = 5.0; // Slow intake
+      public static final double POSITIVE_RATE_LIMIT = 200.0; // Fast shoot
+      public static final double NEGATIVE_RATE_LIMIT = 100.0; // Slow intake
 
-      public static final double SCORE_EXTRA_SECONDS = 0.1;
+      public static final double SCORE_EXTRA_SECONDS = 1.0;
 
-      public static final double IN_OUT_CURRENT_LIMIT = 40.0; // Stator limit
+      public static final double IN_OUT_CURRENT_LIMIT = 60.0; // Stator limit
       public static final double HOLD_CURRENT_LIMIT = 5.0; // Stator, TODO: Test this!
       // TODO: ################### PLACHOLDERS ###################
 
@@ -330,35 +331,37 @@ public static final class PhysicalConstants {
   public static final class Elevator {
     public static final class Leader {
       public static final int MOTOR_PORT = 10;
-      public static final boolean INVERTED = true;
+      public static final boolean INVERTED = false;
     }
 
     public static final class Follower {
       public static final int MOTOR_PORT = 11;
-      public static final boolean INVERTED = false;
+      public static final boolean INVERTED = true;
     }
 
-    public static boolean DBG_DISABLED = true;
+    public static boolean DBG_DISABLED = false;
 
     public static double MOTOR_REVOLUTIONS_PER_METER = 32.81;
 
     // TODO: Tune! Use FWERB for now
     public static class PID {
-      public static double kP = 20.0;
+      public static double kP = 28.0;
       public static double kI = 0.0;
       public static double kD = 0.01;
-      public static double MAX_VELOCITY = 3.20;
+      public static double MAX_VELOCITY = 3.00;
       // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
       // the elevator can make or exceed this
-      public static double MAX_ACCELERATION = 20.0;
+      public static double MAX_ACCELERATION = 13.0;
     }
 
     // TODO: PAD THE ELEVATOR!!!!!!!
     public static class FeedforwardConstants {
       public static double Ks = 0.0;
-      public static double Kv = 3.0;
-      public static double Ka = 0.04;
-      public static double Kg = 0.35; // TODO: Check this!!!
+      public static double Kv = 2.45
+
+      ;
+      public static double Ka = 0.1;
+      public static double Kg = 0.05; // TODO: Check this!!!
     }
 
     public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward(
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2a3b468..cb72fc2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -107,7 +107,9 @@ public CoralPresets get() {
     // 3. Deploy arm
     // 4. Rotate wrist
     private Command getGoToLockedPresetCommand() {
-        return new StowArm(coralSubsystem)
+        return new InstantCommand(() -> {
+            SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
+        }).andThen(new StowArm(coralSubsystem))
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
                 .andThen(new ParallelCommandGroup(
                         new MovePivot(coralSubsystem, currentLockedPresetSupplier),
@@ -141,13 +143,13 @@ private void configureBindings() {
          * operator
          */
         // low algae TODO: Make a preset for low reef algae
-        operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
-            algaeSubsystem.setAlgaePreset(AlgaePresets.FLOOR);
-        }));
-        // high algae TODO: Make a preset for high reef algae
-        operatorXbox.rightBumper().onTrue(new InstantCommand(() -> {
-            algaeSubsystem.setAlgaePreset(AlgaePresets.STOW);
-        }));
+        // operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
+        // algaeSubsystem.setAlgaePreset(AlgaePresets.FLOOR);
+        // }));
+        // // high algae TODO: Make a preset for high reef algae
+        // operatorXbox.rightBumper().onTrue(new InstantCommand(() -> {
+        // algaeSubsystem.setAlgaePreset(AlgaePresets.STOW);
+        // }));
 
         // L1
         operatorXbox.a().onTrue(new InstantCommand(() -> {
@@ -166,7 +168,12 @@ private void configureBindings() {
             selectedScoringPreset = CoralPresets.LEVEL_4;
         }));
 
-        operatorXbox.rightTrigger().whileTrue(new InstantCommand(() -> {
+        operatorXbox.rightTrigger().and(new BooleanSupplier() {
+            @Override
+            public boolean getAsBoolean() {
+                return coralSubsystem.isHolding();
+            }
+        }).whileTrue(new InstantCommand(() -> {
             // coralSubsystem.setCoralPreset(currentCoralPreset);
             lockCoralArmPreset(selectedScoringPreset);
         }).andThen(getGoToLockedPresetCommand())).whileFalse(getStowCommand());
@@ -192,12 +199,13 @@ public boolean getAsBoolean() {
         }).whileTrue(new ScoreCoralCommand(coralSubsystem).andThen(getStowCommand())).whileFalse(getStowCommand());
 
         // Intake coral
-        operatorXbox.rightBumper().and(new BooleanSupplier() {
+        operatorXbox.rightTrigger().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
                 return !coralSubsystem.isHolding();
             }
         }).whileTrue(new InstantCommand(() -> {
+            System.out.println("Intaking");
             lockCoralArmPreset(CoralPresets.INTAKE);
         }).andThen(getGoToLockedPresetCommand()).andThen(new IntakeCoralCommand(coralSubsystem))
                 .andThen(getStowCommand())).whileFalse(getStowCommand());
@@ -250,7 +258,7 @@ public boolean getAsBoolean() {
 
         /////////////////// DEBUGGING //////////////////
         debugXboxController.a().onTrue(new InstantCommand(() -> {
-            coralSubsystem.setCoralPresetPitch(CoralPresets.INTAKE);
+            coralSubsystem.setCoralPresetPitch(CoralPresets.LEVEL_4);
         })).onFalse(new InstantCommand(() -> {
             coralSubsystem.setCoralPresetPitch(CoralPresets.STOW);
         }));
@@ -271,6 +279,10 @@ public boolean getAsBoolean() {
         })).onFalse(new InstantCommand(() -> {
             coralSubsystem.setCoralPresetElevator(CoralPresets.STOW);
         }));
+
+        debugXboxController.rightBumper().whileTrue(new IntakeCoralCommand(coralSubsystem));
+        debugXboxController.leftBumper().whileTrue(new ScoreCoralCommand(coralSubsystem));
+
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
index f8bc571..ffbe6c9 100644
--- a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.coral;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
@@ -20,8 +21,13 @@ public void initialize() {
     @Override
     public void end(boolean interrupted) {
         // if intaking failed
-        if (!subsystem.isHolding() || interrupted) {
+        if (!subsystem.isHolding()) {
             subsystem.setCoralIntakePreset(CoralIntakePresets.STOP);
+            SmartDashboard.putString("Intake Command", "Stopped");
+        } else {
+            SmartDashboard.putString("Intake Command", "Holding");
+
+            subsystem.setCoralIntakePreset(CoralIntakePresets.HOLD);
         }
         // else continue to hold the piece
     }
diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
index f267d12..76fd343 100644
--- a/src/main/java/frc/robot/commands/coral/motion/StowArm.java
+++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
@@ -22,7 +22,6 @@ public StowArm(CoralSubsystem coralSub) {
     public boolean isFinished() {
         // Intentionally not worrying about pitch because it doesn't really matter
         // anyways
-        return Constants.Coral.Pivot.DBG_DISABLED || Constants.Coral.Roll.DBG_DISABLED
-                || coralSub.isRollInPosition() && coralSub.isPivotInPosition();
+        return true;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index f917bf2..98a5ad6 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -140,9 +140,9 @@ public CoralArm() {
         // rollPID.enableContinuousInput(-180, 180);
 
         // TODO: Are these reasonable? who knows.
-        pivotPID.setTolerance(Units.degreesToRadians(0.5));
-        rollPID.setTolerance(Units.degreesToRadians(0.5));
-        pitchPID.setTolerance(Units.degreesToRadians(0.5));
+        pivotPID.setTolerance(Units.degreesToRadians(3.0));
+        rollPID.setTolerance(Units.degreesToRadians(3.0));
+        pitchPID.setTolerance(Units.degreesToRadians(3.0));
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index fa3ec61..15fa1e1 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -39,17 +39,19 @@ public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
         intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(20.0));
         intakeMotor.getConfigurator()
-                .apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive));
+                .apply(new MotorOutputConfigs()
+                        .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive
+                                : InvertedValue.CounterClockwise_Positive));
     }
 
     private double outputPercentage = 0.0;
-    private boolean lastHolding = false;
+    // private boolean lastHolding = false;
 
     @Override
     public void periodic() {
         boolean curHolding = isHolding();
         // if no coral
-        if (curHolding) {
+        if (!curHolding) {
             double output = intakeProfile.calculate(outputPercentage);
             if (!Constants.Coral.Intake.DBG_DISABLED)
                 intakeMotor.set(output);
@@ -60,14 +62,15 @@ public void periodic() {
                 intakeMotor.set(Math.min(0.1, outputPercentage));
         }
 
-        if (lastHolding != curHolding) {
-            intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(
-                    (curHolding && (outputPercentage > 0.0)) ? Constants.Coral.Intake.HOLD_CURRENT_LIMIT
-                            : Constants.Coral.Intake.IN_OUT_CURRENT_LIMIT));
+        // if (lastHolding != curHolding) {
+        // intakeMotor.getConfigurator().apply(new
+        // CurrentLimitsConfigs().withStatorCurrentLimit(
+        // (curHolding && (outputPercentage > 0.0)) ?
+        // Constants.Coral.Intake.HOLD_CURRENT_LIMIT
+        // : Constants.Coral.Intake.IN_OUT_CURRENT_LIMIT));
+        // }
 
-        }
-
-        lastHolding = curHolding;
+        // lastHolding = curHolding;
     }
 
     @Override
@@ -94,7 +97,7 @@ public double getOutputPercentage() {
 
     public boolean isHolding() {
         return Robot.isReal()
-                ? intakeMotor.getReverseLimit().getValue().value == 0
+                ? intakeMotor.getForwardLimit().getValue().value == 0
                 : false;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 4eca028..46cb82f 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -31,7 +32,7 @@ public enum CoralPresets {
         LEVEL_3(0.640, 19.032, 90, 105.968),
         LEVEL_4(1.342, 23.238, 90, 111.762),
         INTAKE(0.0, 9.559, 90, 50.44),
-        STOW(0.0, 0.0, 0.0, 0.0),
+        STOW(0.02, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 
@@ -69,7 +70,7 @@ private MirrorPresets(boolean isMirrored) {
 
     public enum CoralIntakePresets {
         INTAKE(1),
-        HOLD(0.1),
+        HOLD(0.05),
         PURGE(-1),
         SCORE(-1),
         STOP(0),
@@ -91,6 +92,10 @@ public void periodic() {
         pivotMechanism.setAngle(arm.getPivotPositionDegrees());
 
         SmartDashboard.putData("Coral Mechanism", coralMechanism);
+        SmartDashboard.putBoolean("Elevator in position", isElevatorInPosition());
+        SmartDashboard.putBoolean("Roll in position", isRollInPosition());
+        SmartDashboard.putBoolean("Pitch in position", isPitchInPosition());
+        SmartDashboard.putBoolean("Pivot in position", isPivotInPosition());
     }
 
     private CoralPresets currentPreset = CoralPresets.STOW;
@@ -213,6 +218,7 @@ public void mirrorArm(MirrorPresets preset) {
     }
 
     public void setCoralIntakePreset(CoralIntakePresets preset) {
+        SmartDashboard.putString("Coral Intake Preset", preset.toString());
         if (preset != currentIntakePreset) {
             intake.setOutputPercentage(preset.intakePercentage);
             currentIntakePreset = preset;

From b729b916db5c4bef9adf2ba2f3a06456cc0e90af Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 15:20:22 -0600
Subject: [PATCH 50/71] Minor simulation fixes for pivot joint, analog
 encoders, etc.

---
 src/main/java/frc/robot/Constants.java        |  5 +--
 .../commands/coral/motion/MoveElevator.java   |  2 +-
 .../commands/coral/motion/MovePitch.java      |  2 +-
 .../commands/coral/motion/MovePivot.java      |  2 +-
 .../robot/commands/coral/motion/MoveRoll.java |  2 +-
 .../frc/robot/subsystems/coral/CoralArm.java  | 39 ++++++++++++-------
 .../robot/subsystems/coral/CoralElevator.java | 18 +++++----
 .../subsystems/coral/CoralSubsystem.java      |  4 ++
 8 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 04ad766..90044c4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -227,7 +226,7 @@ public static class PhysicalConstants {
         public static final double NET_REDUCTION = 45.0;
         public static final double MASS_KG = 2.85; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.083;
-        public static final double MOI = 0.0403605447; // Kg*m^2
+        public static final double MOI = 0.00403605447; // Kg*m^2
       }
     }
 
@@ -254,7 +253,7 @@ public static class PhysicalConstants {
         public static final double MASS_KG = 2.16; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.101;
         public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(13.875);
-        public static final double MOI = 0.0200055915; // Kg*m^2
+        public static final double MOI = 0.00200055915; // Kg*m^2
       }
     }
 
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
index c5e7d67..69ae85c 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
@@ -20,6 +20,6 @@ public MoveElevator(CoralSubsystem coralSub, Supplier<CoralPresets> presetSuppli
 
     @Override
     public boolean isFinished() {
-        return Constants.Elevator.DBG_DISABLED || coralSub.isElevatorInPosition();
+        return Constants.Elevator.DBG_DISABLED || coralSub.isElevatorSupposedToBeInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
index d989685..65b472f 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
@@ -21,6 +21,6 @@ public MovePitch(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return Constants.Coral.Pitch.DBG_DISABLED || coralSub.isPitchInPosition();
+        return coralSub.isPitchInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
index 1edc173..21c6bf1 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -20,6 +20,6 @@ public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return Constants.Coral.Pivot.DBG_DISABLED || coralSub.isPivotInPosition();
+        return coralSub.isPivotInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
index 85548b2..ce865a6 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
@@ -20,6 +20,6 @@ public MoveRoll(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
 
     @Override
     public boolean isFinished() {
-        return Constants.Coral.Roll.DBG_DISABLED || coralSub.isRollInPosition();
+        return coralSub.isRollInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index da21a31..3f0f721 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -4,6 +4,7 @@
 import com.revrobotics.sim.SparkAnalogSensorSim;
 import com.revrobotics.sim.SparkFlexSim;
 import com.revrobotics.sim.SparkMaxSim;
+import com.revrobotics.sim.SparkRelativeEncoderSim;
 import com.revrobotics.spark.SparkAnalogSensor;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
@@ -45,6 +46,7 @@ public class CoralArm extends SubsystemBase {
     // sim encoders
     private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
     private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
+    private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
 
     // physics simulations
     private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
@@ -53,7 +55,7 @@ public class CoralArm extends SubsystemBase {
             Coral.Pivot.PhysicalConstants.MOI,
             Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
             0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
-            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI - 0.05);
+            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
     private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
             Coral.Roll.PhysicalConstants.MOTOR,
             Coral.Roll.PhysicalConstants.NET_REDUCTION,
@@ -158,7 +160,8 @@ public void periodic() {
          * run the motors
          */
 
-        double pivotPosition = pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
+        double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
+                : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
         double pivotFFout = pivotFeedforward.calculate(
                 Math.PI * 0.5 + pivotPosition,
                 pivotPID.getSetpoint().velocity);
@@ -202,7 +205,7 @@ public void periodic() {
     public void simulationPeriodic() {
         // update physics
         simPivotPhysics
-                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
         simRollPhysics
                 .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
         simPitchPhysics
@@ -228,14 +231,19 @@ public void simulationPeriodic() {
                 RoboRioSim.getVInVoltage(),
                 0.02);
 
-        simRollEncoder.setPosition(simRollPhysics.getAngleRads() / 5.0);
+        simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
+                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
         simRollEncoder.iterate(
                 simRollPhysics.getVelocityRadPerSec(),
                 0.02);
-        simPitchEncoder.setPosition(simPitchPhysics.getAngleRads() / 5.0);
+        simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
+                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
         simPitchEncoder.iterate(
                 simPitchPhysics.getVelocityRadPerSec(),
                 0.02);
+
+        simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
+        simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
     }
 
     // this should be relative to straight upwards.
@@ -331,19 +339,20 @@ public boolean isPivotInPosition() {
     }
 
     public double readPitchEncoderPosition() {
-        return Robot.isSimulation() ? simPitchEncoder.getPosition()
-                : MathUtil
-                        .angleModulus(((pitchEncFilter.calculate(pitchEncoder.getPosition())
-                                + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS) / 3.3)
-                                * 2 * Math.PI);
+        return MathUtil
+                .angleModulus(((pitchEncFilter.calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
+                        : pitchEncoder.getPosition())
+                        + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
+                        / 3.3)
+                        * 2 * Math.PI);
     }
 
     public double readRollEncoderPosition() {
-        return Robot.isSimulation() ? simRollEncoder.getPosition()
-                : MathUtil
-                        .angleModulus(((rollEncFilter.calculate(rollEncoder.getPosition())
-                                + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
-                                * 2 * Math.PI);
+        return MathUtil
+                .angleModulus(((rollEncFilter
+                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition() : rollEncoder.getPosition())
+                        + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
+                        * 2 * Math.PI);
     }
 
     public double readPivotEncoderPosition() {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index 7f5a5bd..79dc061 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -97,13 +97,9 @@ public CoralElevator() {
     public void periodic() {
         // check if needs to be zeroed and is at zero
         // TODO: ######################### PLACEHOLDERS AGAIN #########################
-        if (
-            !isZeroed
-            && (
-                ((leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2) > 20
-                || Robot.isSimulation()
-            )
-        ) {
+        if (!isZeroed
+                && (((leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2) > 20
+                        || Robot.isSimulation())) {
             leaderEncoder.setPosition(0);
             isZeroed = true;
         }
@@ -147,7 +143,8 @@ public void periodic() {
     @Override
     public void simulationPeriodic() {
         // update physics
-        simElevator.setInput(MathUtil.clamp(simLeaderMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+        simElevator
+                .setInput(MathUtil.clamp(simLeaderMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
         simElevator.update(0.02);
 
         // update sim objects
@@ -179,6 +176,11 @@ public boolean isInPosition() {
         return elevatorPID.atGoal();
     }
 
+    public boolean isSupposedToBeInPosition() {
+        return MathUtil.isNear(elevatorPID.getSetpoint().position, elevatorPID.getGoal().position,
+                Units.inchesToMeters(1 / 16.0));
+    }
+
     public void zeroElevator() {
         isZeroed = false;
     }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 959ec6f..7bf95a9 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -228,4 +228,8 @@ public void setCustomIntakePercent(double percentage) {
     public double getPivotPositionDegrees() {
         return arm.getPivotPositionDegrees();
     }
+
+    public boolean isElevatorSupposedToBeInPosition() {
+        return elevator.isSupposedToBeInPosition();
+    }
 }

From e97bf0cc11e34d3f62c946950e21b061a7bc60e5 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 16:18:52 -0600
Subject: [PATCH 51/71] Fix motion commands, tweak operator controls, rumble,
 coral intaking simulation.

---
 src/main/java/frc/robot/RobotContainer.java   | 43 +++++++++++++------
 .../commands/coral/IntakeCoralCommand.java    |  4 ++
 .../commands/coral/ScoreCoralCommand.java     | 20 ++++++---
 .../commands/coral/motion/MoveElevator.java   |  6 +++
 .../commands/coral/motion/MovePitch.java      |  6 +++
 .../commands/coral/motion/MovePivot.java      |  6 +++
 .../robot/commands/coral/motion/MoveRoll.java |  6 +++
 .../robot/commands/coral/motion/StowArm.java  |  7 ++-
 .../frc/robot/subsystems/coral/CoralArm.java  | 12 ++++++
 .../robot/subsystems/coral/CoralIntake.java   |  1 +
 .../subsystems/coral/CoralSubsystem.java      | 35 +++++++++++++--
 11 files changed, 124 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 415f648..5996303 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -29,6 +29,7 @@
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
@@ -86,6 +87,14 @@ public RobotContainer() {
 
     private CoralPresets selectedScoringPreset = CoralPresets.STOW;
     private CoralPresets lockedPreset = CoralPresets.STOW;
+    private boolean isScoring = false;
+
+    Trigger coralAquisition = new Trigger(coralSubsystem.isHoldingSupplier());
+    Trigger coralInPosition = new Trigger(new BooleanSupplier() {
+        public boolean getAsBoolean() {
+            return coralSubsystem.isSupposedToBeInPosition();
+        };
+    });
 
     private void lockCoralArmPreset(CoralPresets preset) {
         lockedPreset = preset;
@@ -164,15 +173,20 @@ private void configureBindings() {
             selectedScoringPreset = CoralPresets.LEVEL_4;
         }));
 
-        operatorXbox.rightTrigger().and(new BooleanSupplier() {
-            @Override
-            public boolean getAsBoolean() {
-                return coralSubsystem.isHolding();
-            }
-        }).whileTrue(new InstantCommand(() -> {
+        // Score!!!
+        operatorXbox.rightTrigger().whileTrue((new InstantCommand(() -> {
             // coralSubsystem.setCoralPreset(currentCoralPreset);
             lockCoralArmPreset(selectedScoringPreset);
-        }).andThen(getGoToLockedPresetCommand())).whileFalse(getStowCommand());
+            isScoring = true;
+        }).andThen(getGoToLockedPresetCommand().andThen(
+                new InstantCommand(() -> {
+                    operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
+                }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> {
+                    operatorXbox.setRumble(RumbleType.kBothRumble, 0.0);
+                }))))).onlyIf(coralSubsystem.isHoldingSupplier()))
+                .whileFalse(getStowCommand().alongWith(new InstantCommand(() -> {
+                    isScoring = false;
+                })));
 
         // wrist adjustment
         // Hold for now, until everything else is working
@@ -186,19 +200,19 @@ public boolean getAsBoolean() {
         // }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
 
         // Score coral
-        // TODO: Is it a good idea to stow right after this?
         operatorXbox.rightBumper().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
-                return coralSubsystem.isHolding();
+                return coralSubsystem.isHolding() && isScoring;
             }
-        }).whileTrue(new ScoreCoralCommand(coralSubsystem).andThen(getStowCommand())).whileFalse(getStowCommand());
+        }).whileTrue(new ScoreCoralCommand(coralSubsystem));
+        operatorXbox.rightBumper().whileFalse(getStowCommand());
 
         // Intake coral
         operatorXbox.rightTrigger().and(new BooleanSupplier() {
             @Override
             public boolean getAsBoolean() {
-                return !coralSubsystem.isHolding();
+                return !coralSubsystem.isHolding() && !isScoring;
             }
         }).whileTrue(new InstantCommand(() -> {
             System.out.println("Intaking");
@@ -209,6 +223,12 @@ public boolean getAsBoolean() {
         // purge coral
         operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
 
+        coralAquisition.onChange(new InstantCommand(() -> {
+            operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
+        }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> {
+            operatorXbox.setRumble(RumbleType.kBothRumble, 0.0);
+        })));
+
         /*
          * driver
          */
@@ -277,7 +297,6 @@ public boolean getAsBoolean() {
 
         debugXboxController.rightBumper().whileTrue(new IntakeCoralCommand(coralSubsystem));
         debugXboxController.leftBumper().whileTrue(new ScoreCoralCommand(coralSubsystem));
-
     }
 
     /**
diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
index ffbe6c9..ce4b319 100644
--- a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
 
@@ -16,6 +17,9 @@ public IntakeCoralCommand(CoralSubsystem subsystem) {
     @Override
     public void initialize() {
         subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE);
+        if (Robot.isSimulation()) {
+            SmartDashboard.putBoolean("[SIM] Holding Coral", true);
+        }
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
index e8728c7..6d68113 100644
--- a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java
@@ -1,8 +1,10 @@
 package frc.robot.commands.coral;
 
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.Robot;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets;
 
@@ -19,6 +21,9 @@ public ScoreCoralCommand(CoralSubsystem subsystem) {
     @Override
     public void initialize() {
         subsystem.setCoralIntakePreset(CoralIntakePresets.SCORE);
+        if (Robot.isSimulation()) {
+            SmartDashboard.putBoolean("[SIM] Holding Coral", false);
+        }
     }
 
     @Override
@@ -28,11 +33,16 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        if (!subsystem.isHolding() && coralExitTime == 0) {
-            coralExitTime = Timer.getFPGATimestamp();
-        }
-        return !subsystem.isHolding()
-                && ((Timer.getFPGATimestamp() - coralExitTime) > Constants.Coral.Intake.SCORE_EXTRA_SECONDS);
+        // if (!subsystem.isHolding() && coralExitTime == 0) {
+        // coralExitTime = Timer.getFPGATimestamp();
+        // }
+        // return !subsystem.isHolding()
+        // && ((Timer.getFPGATimestamp() - coralExitTime) >
+        // Constants.Coral.Intake.SCORE_EXTRA_SECONDS);
+
+        // Just keep going at it!
+        // For autos, might want a different command?
+        return Robot.isSimulation();
     }
 
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
index 69ae85c..2a5b9c5 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
@@ -10,10 +10,16 @@
 
 public class MoveElevator extends Command {
     private CoralSubsystem coralSub;
+    private Supplier<CoralPresets> presetSupplier;
 
     public MoveElevator(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
         this.coralSub = coralSub;
+        this.presetSupplier = presetSupplier;
         addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Elevator");
         coralSub.setCoralPresetElevator(presetSupplier.get());
     }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
index 65b472f..e9773cf 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
@@ -10,11 +10,17 @@
 
 public class MovePitch extends Command {
     private CoralSubsystem coralSub;
+    private Supplier<CoralPresets> presetSupplier;
 
     public MovePitch(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
         this.coralSub = coralSub;
+        this.presetSupplier = presetSupplier;
         // HACK: This is much sketch
         // addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Pitch");
         coralSub.setCoralPresetPitch(presetSupplier.get());
     }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
index 21c6bf1..48887fd 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -10,10 +10,16 @@
 
 public class MovePivot extends Command {
     private CoralSubsystem coralSub;
+    private Supplier<CoralPresets> presetSupplier;
 
     public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
         this.coralSub = coralSub;
+        this.presetSupplier = presetSupplier;
         addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Pivot");
         coralSub.setCoralPresetPivot(presetSupplier.get());
     }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
index ce865a6..f473bb8 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
@@ -10,10 +10,16 @@
 
 public class MoveRoll extends Command {
     private CoralSubsystem coralSub;
+    private Supplier<CoralPresets> presetSupplier;
 
     public MoveRoll(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
         this.coralSub = coralSub;
         // addRequirements(coralSub);
+        this.presetSupplier = presetSupplier;
+    }
+
+    @Override
+    public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Roll");
         coralSub.setCoralPresetRoll(presetSupplier.get());
     }
diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
index 76fd343..9a73749 100644
--- a/src/main/java/frc/robot/commands/coral/motion/StowArm.java
+++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java
@@ -12,6 +12,10 @@ public class StowArm extends Command {
     public StowArm(CoralSubsystem coralSub) {
         this.coralSub = coralSub;
         addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Stowing Arm");
         coralSub.setCoralPresetPitch(CoralPresets.STOW);
         coralSub.setCoralPresetRoll(CoralPresets.STOW);
@@ -20,8 +24,7 @@ public StowArm(CoralSubsystem coralSub) {
 
     @Override
     public boolean isFinished() {
-        // Intentionally not worrying about pitch because it doesn't really matter
-        // anyways
+        // TODO: For the future... might want to wait? or not.
         return true;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 3f0f721..1e4b9ee 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -360,4 +360,16 @@ public double readPivotEncoderPosition() {
                 : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
                         * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
     }
+
+    public boolean isPitchSupposedToBeInPosition() {
+        return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
+    }
+
+    public boolean isPivotSupposedToBeInPosition() {
+        return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
+    }
+
+    public boolean isRollSupposedToBeInPosition() {
+        return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 15fa1e1..a9e6b65 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Robot;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 7bf95a9..b1bd64d 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.coral;
 
+import java.util.function.BooleanSupplier;
+
 // import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -7,6 +9,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Robot;
 
 public class CoralSubsystem extends SubsystemBase {
 
@@ -26,8 +29,8 @@ public enum CoralPresets {
         LEVEL_2(0.237, 19.032, 90, 105.968),
         LEVEL_3(0.640, 19.032, 90, 105.968),
         LEVEL_4(1.342, 23.238, 90, 111.762),
-        INTAKE(0.0, 9.559, 90, 50.44),
-        STOW(0.02, 0.0, 0.0, 0.0),
+        INTAKE(0.01, 9.559, 90, 50.44),
+        STOW(0.01, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 
@@ -168,7 +171,16 @@ public double getPitchGoalDegrees() {
     }
 
     public boolean isHolding() {
-        return intake.isHolding();
+        return Robot.isSimulation() ? SmartDashboard.getBoolean("[SIM] Holding Coral", false) : intake.isHolding();
+    }
+
+    public BooleanSupplier isHoldingSupplier() {
+        return new BooleanSupplier() {
+            @Override
+            public boolean getAsBoolean() {
+                return isHolding();
+            }
+        };
     }
 
     public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) {
@@ -232,4 +244,21 @@ public double getPivotPositionDegrees() {
     public boolean isElevatorSupposedToBeInPosition() {
         return elevator.isSupposedToBeInPosition();
     }
+
+    public boolean isPitchSupposedToBeInPosition() {
+        return arm.isPitchSupposedToBeInPosition();
+    }
+
+    public boolean isRollSupposedToBeInPosition() {
+        return arm.isRollSupposedToBeInPosition();
+    }
+
+    public boolean isPivotSupposedToBeInPosition() {
+        return arm.isPivotSupposedToBeInPosition();
+    }
+
+    public boolean isSupposedToBeInPosition() {
+        return isPivotSupposedToBeInPosition() && isRollSupposedToBeInPosition() && isElevatorSupposedToBeInPosition()
+                && isPitchSupposedToBeInPosition();
+    }
 }

From bdca571cfb0d21f9e9a2daa419fd52f3ab8e68d1 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 19:50:21 -0600
Subject: [PATCH 52/71] Works, but flings coral.

---
 src/main/java/frc/robot/Constants.java        | 10 ++---
 src/main/java/frc/robot/RobotContainer.java   | 24 +++++++----
 .../commands/coral/IntakeCoralCommand.java    |  2 +
 .../commands/coral/motion/MoveElevator.java   |  8 +++-
 .../commands/coral/motion/MovePitch.java      |  9 +++-
 .../commands/coral/motion/MovePivot.java      |  9 +++-
 .../robot/commands/coral/motion/MoveRoll.java |  9 +++-
 .../frc/robot/subsystems/algae/AlgaeArm.java  | 42 +++++++++----------
 .../robot/subsystems/algae/AlgaeIntake.java   | 32 +++++++-------
 .../frc/robot/subsystems/coral/CoralArm.java  | 38 +++++++++++++----
 .../robot/subsystems/coral/CoralElevator.java |  6 ++-
 .../robot/subsystems/coral/CoralIntake.java   | 10 ++++-
 .../subsystems/coral/CoralSubsystem.java      |  8 ++--
 13 files changed, 134 insertions(+), 73 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 90044c4..c4ccbad 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -182,16 +182,16 @@ public static class Pivot {
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          15.0,
+          18.0,
           0.0,
           0.1,
-          new TrapezoidProfile.Constraints(7.0, 20.0));
+          new TrapezoidProfile.Constraints(7.0, 15.0));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
           0.0,
           0.4, // V
-          0.4, // 1.0, // V*s/rad
+          0.35, // 1.0, // V*s/rad
           0.00// V*s^2/rad
       );
 
@@ -216,7 +216,7 @@ public static class Roll {
           2.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(20.0, 150.0));
+          new TrapezoidProfile.Constraints(20.0, 50.0));
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.2);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
@@ -243,7 +243,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 200.0)); // Radians
+          new TrapezoidProfile.Constraints(12.0, 50.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5996303..859eb95 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -120,7 +120,10 @@ private Command getGoToLockedPresetCommand() {
                 .andThen(new ParallelCommandGroup(
                         new MovePivot(coralSubsystem, currentLockedPresetSupplier),
                         new MoveRoll(coralSubsystem, currentLockedPresetSupplier),
-                        new MovePitch(coralSubsystem, currentLockedPresetSupplier)));
+                        new MovePitch(coralSubsystem, currentLockedPresetSupplier)))
+                .andThen(new InstantCommand(() -> {
+                    SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done");
+                }));
     }
 
     private Command getStowCommand() {
@@ -177,7 +180,8 @@ private void configureBindings() {
         operatorXbox.rightTrigger().whileTrue((new InstantCommand(() -> {
             // coralSubsystem.setCoralPreset(currentCoralPreset);
             lockCoralArmPreset(selectedScoringPreset);
-            isScoring = true;
+            if (!coralSubsystem.isHolding())
+                isScoring = true;
         }).andThen(getGoToLockedPresetCommand().andThen(
                 new InstantCommand(() -> {
                     operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
@@ -200,12 +204,16 @@ private void configureBindings() {
         // }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox));
 
         // Score coral
-        operatorXbox.rightBumper().and(new BooleanSupplier() {
-            @Override
-            public boolean getAsBoolean() {
-                return coralSubsystem.isHolding() && isScoring;
-            }
-        }).whileTrue(new ScoreCoralCommand(coralSubsystem));
+        /*
+         * .and(new BooleanSupplier() {
+         * 
+         * @Override
+         * public boolean getAsBoolean() {
+         * return coralSubsystem.isHolding() && isScoring;
+         * }
+         * })
+         */
+        operatorXbox.rightBumper().whileTrue(new ScoreCoralCommand(coralSubsystem));
         operatorXbox.rightBumper().whileFalse(getStowCommand());
 
         // Intake coral
diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
index ce4b319..6d77952 100644
--- a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
+++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java
@@ -17,6 +17,8 @@ public IntakeCoralCommand(CoralSubsystem subsystem) {
     @Override
     public void initialize() {
         subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE);
+        SmartDashboard.putString("Intake Command", "Started");
+
         if (Robot.isSimulation()) {
             SmartDashboard.putBoolean("[SIM] Holding Coral", true);
         }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
index 2a5b9c5..cb01ef6 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java
@@ -21,11 +21,17 @@ public MoveElevator(CoralSubsystem coralSub, Supplier<CoralPresets> presetSuppli
     @Override
     public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Elevator");
+        SmartDashboard.putString("Move Elevator", "Started");
         coralSub.setCoralPresetElevator(presetSupplier.get());
     }
 
+    @Override
+    public void end(boolean interrupted) {
+        SmartDashboard.putString("Move Elevator", "End");
+    }
+
     @Override
     public boolean isFinished() {
-        return Constants.Elevator.DBG_DISABLED || coralSub.isElevatorSupposedToBeInPosition();
+        return coralSub.isElevatorSupposedToBeInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
index e9773cf..5694dcc 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java
@@ -22,11 +22,18 @@ public MovePitch(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
     @Override
     public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Pitch");
+        SmartDashboard.putString("Move Pitch", "Start");
+
         coralSub.setCoralPresetPitch(presetSupplier.get());
     }
 
+    @Override
+    public void end(boolean interrupted) {
+        SmartDashboard.putString("Move Pitch", "End");
+    }
+
     @Override
     public boolean isFinished() {
-        return coralSub.isPitchInPosition();
+        return coralSub.isPitchSupposedToBeInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
index 48887fd..51b2904 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -21,11 +21,18 @@ public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
     @Override
     public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Pivot");
+        SmartDashboard.putString("Move Pivot", "Start");
+
         coralSub.setCoralPresetPivot(presetSupplier.get());
     }
 
+    @Override
+    public void end(boolean interrupted) {
+        SmartDashboard.putString("Move Pivot", "End");
+    }
+
     @Override
     public boolean isFinished() {
-        return coralSub.isPivotInPosition();
+        return coralSub.isPivotSupposedToBeInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
index f473bb8..e380020 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java
@@ -21,11 +21,18 @@ public MoveRoll(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier)
     @Override
     public void initialize() {
         SmartDashboard.putString("Arm Sequence", "Moving Roll");
+        SmartDashboard.putString("Move Roll", "Start");
+
         coralSub.setCoralPresetRoll(presetSupplier.get());
     }
 
+    @Override
+    public void end(boolean interrupted) {
+        SmartDashboard.putString("Move Roll", "End");
+    }
+
     @Override
     public boolean isFinished() {
-        return coralSub.isRollInPosition();
+        return coralSub.isRollSupposedToBeInPosition();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
index 2b5afad..cfe5410 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java
@@ -23,20 +23,19 @@ public class AlgaeArm extends SubsystemBase {
     private final SparkFlex pivotMotor = new SparkFlex(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless);
     // sim motor
     private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Algae.Pivot.PhysicalConstants.MOTOR);
-    
+
     public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT);
 
     // physics simulation
     private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-        Algae.Pivot.PhysicalConstants.MOTOR,
-        Algae.Pivot.PhysicalConstants.GEARING,
-        Algae.Pivot.PhysicalConstants.MOI,
-        Algae.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-        Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES),
-        Units.degreesToRadians(Algae.Pivot.EXTENDED_LIMIT_DEGREES),
-        true,
-        Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES)
-    );
+            Algae.Pivot.PhysicalConstants.MOTOR,
+            Algae.Pivot.PhysicalConstants.GEARING,
+            Algae.Pivot.PhysicalConstants.MOI,
+            Algae.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+            Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES),
+            Units.degreesToRadians(Algae.Pivot.EXTENDED_LIMIT_DEGREES),
+            true,
+            Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES));
 
     private final SparkMaxConfig pivotConfig = new SparkMaxConfig();
 
@@ -51,26 +50,25 @@ public AlgaeArm() {
     @Override
     public void periodic() {
         double output = pivotPID.calculate(
-            pivotEncoder.getAbsolutePosition().getValueAsDouble()
-            + pivotFeedforward.calculate(Units.degreesToRadians(this.getGoalDegrees()), 0.0)
-        );
+                pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                        + pivotFeedforward.calculate(Units.degreesToRadians(this.getGoalDegrees()), 0.0));
 
         pivotMotor.set(output);
-        simPivotMotor.setAppliedOutput(output);
+        if (Robot.isSimulation())
+            simPivotMotor.setAppliedOutput(output);
     }
 
     @Override
     public void simulationPeriodic() {
-        // update physics 
+        // update physics
         simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage());
         simPivotPhysics.update(0.02);
 
         // update sim objects
         simPivotMotor.iterate(
-            Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
+                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+                RoboRioSim.getVInVoltage(),
+                0.02);
     }
 
     public void setGoalDegrees(double goal) {
@@ -82,8 +80,8 @@ public double getGoalDegrees() {
     }
 
     public double getPositionDegrees() {
-        return Robot.isReal() 
-            ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
-            : Units.radiansToDegrees(simPivotPhysics.getAngleRads());
+        return Robot.isReal()
+                ? pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                : Units.radiansToDegrees(simPivotPhysics.getAngleRads());
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
index b294051..66e6419 100644
--- a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
+++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Algae;
+import frc.robot.Robot;
 
 public class AlgaeIntake extends SubsystemBase {
     private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless);
@@ -20,21 +21,18 @@ public class AlgaeIntake extends SubsystemBase {
 
     // physics simulation
     private final FlywheelSim simIntakePhysics = new FlywheelSim(
-        LinearSystemId.createFlywheelSystem(
-            Algae.Intake.PhysicalConstants.MOTOR,
-            Algae.Intake.PhysicalConstants.MOI,
-            Algae.Intake.PhysicalConstants.GEARING
-        ),
-        Algae.Intake.PhysicalConstants.MOTOR
-    );
+            LinearSystemId.createFlywheelSystem(
+                    Algae.Intake.PhysicalConstants.MOTOR,
+                    Algae.Intake.PhysicalConstants.MOI,
+                    Algae.Intake.PhysicalConstants.GEARING),
+            Algae.Intake.PhysicalConstants.MOTOR);
 
     private final DigitalInput intakeBeambreak = new DigitalInput(Algae.Intake.BEAMBREAK_PORT);
 
     private final SlewRateLimiter intakeProfile = new SlewRateLimiter(
-        Algae.Intake.POSITIVE_RATE_LIMIT,
-        Algae.Intake.NEGATIVE_RATE_LIMIT, 
-        0
-    );
+            Algae.Intake.POSITIVE_RATE_LIMIT,
+            Algae.Intake.NEGATIVE_RATE_LIMIT,
+            0);
 
     private double outputPercentage = 0.0;
 
@@ -50,9 +48,10 @@ public void periodic() {
         }
 
         intakeMotor.set(output);
-        simIntakeMotor.setAppliedOutput(output);
+        if (Robot.isSimulation())
+            simIntakeMotor.setAppliedOutput(output);
     }
-    
+
     @Override
     public void simulationPeriodic() {
         // update physics
@@ -61,10 +60,9 @@ public void simulationPeriodic() {
 
         // update sim objects
         simIntakeMotor.iterate(
-            simIntakePhysics.getAngularVelocityRPM(),
-            RoboRioSim.getVInVoltage(),
-            0.02
-        );
+                simIntakePhysics.getAngularVelocityRPM(),
+                RoboRioSim.getVInVoltage(),
+                0.02);
     }
 
     public void setOutputPercentage(double outputPercentage) {
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 1e4b9ee..d9b7faf 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -138,6 +138,12 @@ public CoralArm() {
         pivotPID.setTolerance(Units.degreesToRadians(3.0));
         rollPID.setTolerance(Units.degreesToRadians(3.0));
         pitchPID.setTolerance(Units.degreesToRadians(3.0));
+
+        // Warm up the filters
+        for (int i = 0; i < 3; ++i) {
+            readPitchEncoderPosition();
+            readRollEncoderPosition();
+        }
     }
 
     @Override
@@ -174,7 +180,8 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
         if (!Constants.Coral.Pivot.DBG_DISABLED)
             pivotMotor.setVoltage(pivotPIDout + pivotFFout);
-        simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
+        if (Robot.isSimulation())
+            simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
 
         double rollPIDout = rollPID.calculate(readRollEncoderPosition());
         double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
@@ -186,7 +193,8 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
         if (!Constants.Coral.Roll.DBG_DISABLED)
             rollMotor.setVoltage(rollPIDout + rollFFout);
-        simRollMotor.setAppliedOutput(rollPIDout);
+        if (Robot.isSimulation())
+            simRollMotor.setAppliedOutput(rollPIDout);
 
         double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
         double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
@@ -198,18 +206,22 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
         if (!Constants.Coral.Pitch.DBG_DISABLED)
             pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-        simPitchMotor.setAppliedOutput(pitchPIDout);
+        if (Robot.isSimulation())
+            simPitchMotor.setAppliedOutput(pitchPIDout);
     }
 
     @Override
     public void simulationPeriodic() {
         // update physics
         simPivotPhysics
-                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
         simRollPhysics
-                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
         simPitchPhysics
-                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0));
+                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
         SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
                 * RoboRioSim.getVInVoltage());
 
@@ -340,8 +352,9 @@ public boolean isPivotInPosition() {
 
     public double readPitchEncoderPosition() {
         return MathUtil
-                .angleModulus(((pitchEncFilter.calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
-                        : pitchEncoder.getPosition())
+                .angleModulus(((pitchEncFilter
+                        .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
+                                : pitchEncoder.getPosition())
                         + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
                         / 3.3)
                         * 2 * Math.PI);
@@ -350,7 +363,8 @@ public double readPitchEncoderPosition() {
     public double readRollEncoderPosition() {
         return MathUtil
                 .angleModulus(((rollEncFilter
-                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition() : rollEncoder.getPosition())
+                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
+                                : rollEncoder.getPosition())
                         + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
                         * 2 * Math.PI);
     }
@@ -362,14 +376,20 @@ public double readPivotEncoderPosition() {
     }
 
     public boolean isPitchSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
+        // this.pitchPID.getGoal().position, 0.01);
         return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
     }
 
     public boolean isPivotSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
+        // this.pivotPID.getGoal().position, 0.01);
         return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
     }
 
     public boolean isRollSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.rollPID.getSetpoint().position,
+        // this.rollPID.getGoal().position, 0.01);
         return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
index 79dc061..cfab666 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java
@@ -129,8 +129,10 @@ public void periodic() {
                 followerMotor.setVoltage(output);
             }
 
-            simLeaderMotor.setAppliedOutput(output);
-            simFollowerMotor.setAppliedOutput(output);
+            if (Robot.isSimulation()) {
+                simLeaderMotor.setAppliedOutput(output);
+                simFollowerMotor.setAppliedOutput(output);
+            }
         } else {
             if (!Constants.Elevator.DBG_DISABLED) {
                 // slowly move down to zero
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index a9e6b65..cc57f45 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.coral;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -38,11 +39,14 @@ public class CoralIntake extends SubsystemBase {
 
     public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
-        intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(20.0));
+        intakeMotor.getConfigurator()
+                .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(15.0).withStatorCurrentLimitEnable(true));
         intakeMotor.getConfigurator()
                 .apply(new MotorOutputConfigs()
                         .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive
                                 : InvertedValue.CounterClockwise_Positive));
+        intakeMotor.getConfigurator()
+                .apply(new HardwareLimitSwitchConfigs().withForwardLimitEnable(false).withReverseLimitEnable(false));
     }
 
     private double outputPercentage = 0.0;
@@ -53,7 +57,7 @@ public void periodic() {
         boolean curHolding = isHolding();
         // if no coral
         if (!curHolding) {
-            double output = intakeProfile.calculate(outputPercentage);
+            double output = outputPercentage;// intakeProfile.calculate(outputPercentage);
             if (!Constants.Coral.Intake.DBG_DISABLED)
                 intakeMotor.set(output);
         } else {
@@ -72,6 +76,8 @@ public void periodic() {
         // }
 
         // lastHolding = curHolding;
+
+        SmartDashboard.putBoolean("Holding Coral", curHolding);
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index b1bd64d..9a41307 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -26,9 +26,9 @@ public class CoralSubsystem extends SubsystemBase {
 
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
-        LEVEL_2(0.237, 19.032, 90, 105.968),
-        LEVEL_3(0.640, 19.032, 90, 105.968),
-        LEVEL_4(1.342, 23.238, 90, 111.762),
+        LEVEL_2(0.237, 19.032, 90, 102.968),
+        LEVEL_3(0.640, 19.032, 90, 102.968),
+        LEVEL_4(1.342, 21.238, 90, 111.762),
         INTAKE(0.01, 9.559, 90, 50.44),
         STOW(0.01, 0.0, 0.0, 0.0),
 
@@ -68,7 +68,7 @@ private MirrorPresets(boolean isMirrored) {
 
     public enum CoralIntakePresets {
         INTAKE(1),
-        HOLD(0.05),
+        HOLD(0.4),
         PURGE(-1),
         SCORE(-1),
         STOP(0),

From 4d99369139aeed2a7a5570499e3644cc0146f710 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 21:04:15 -0600
Subject: [PATCH 53/71] Scoring is OK, intake is OK. not perfect, but it works
 alright.

---
 src/main/java/frc/robot/Constants.java               | 12 ++++++------
 src/main/java/frc/robot/RobotContainer.java          |  4 ++++
 .../java/frc/robot/subsystems/coral/CoralIntake.java |  2 +-
 .../frc/robot/subsystems/coral/CoralSubsystem.java   |  8 ++++----
 4 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c4ccbad..0852fd4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -213,11 +213,11 @@ public static class Roll {
       public static boolean DBG_DISABLED = false;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          2.5,
+          3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(20.0, 50.0));
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.2);
+          new TrapezoidProfile.Constraints(10.0, 25.0));
+      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
@@ -243,7 +243,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 50.0)); // Radians
+          new TrapezoidProfile.Constraints(10.0, 35.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
@@ -346,10 +346,10 @@ public static class PID {
       public static double kP = 28.0;
       public static double kI = 0.0;
       public static double kD = 0.01;
-      public static double MAX_VELOCITY = 3.00;
+      public static double MAX_VELOCITY = 2.75;
       // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
       // the elevator can make or exceed this
-      public static double MAX_ACCELERATION = 13.0;
+      public static double MAX_ACCELERATION = 10.0;
     }
 
     // TODO: PAD THE ELEVATOR!!!!!!!
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 859eb95..fe59f81 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -237,6 +237,10 @@ public boolean getAsBoolean() {
             operatorXbox.setRumble(RumbleType.kBothRumble, 0.0);
         })));
 
+        operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
+            coralSubsystem.mirrorArm();
+        }));
+
         /*
          * driver
          */
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index cc57f45..9609a24 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -40,7 +40,7 @@ public class CoralIntake extends SubsystemBase {
     public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
         intakeMotor.getConfigurator()
-                .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(15.0).withStatorCurrentLimitEnable(true));
+                .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(12.5).withStatorCurrentLimitEnable(true));
         intakeMotor.getConfigurator()
                 .apply(new MotorOutputConfigs()
                         .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 9a41307..8df61a9 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -26,11 +26,11 @@ public class CoralSubsystem extends SubsystemBase {
 
     public enum CoralPresets {
         LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
-        LEVEL_2(0.237, 19.032, 90, 102.968),
-        LEVEL_3(0.640, 19.032, 90, 102.968),
+        LEVEL_2(0.247, 19.032, 90, 98.968),
+        LEVEL_3(0.650, 19.032, 90, 98.968),
         LEVEL_4(1.342, 21.238, 90, 111.762),
-        INTAKE(0.01, 9.559, 90, 50.44),
-        STOW(0.01, 0.0, 0.0, 0.0),
+        INTAKE(0.05, 18.0, 90, 35.0),
+        STOW(0.05, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
 

From 71cfc62235af898640fab59e5fd030e66288cb1e Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Tue, 11 Feb 2025 23:07:24 -0600
Subject: [PATCH 54/71] Pitch tweak, move current limit too constants.

---
 src/main/java/frc/robot/Constants.java                      | 6 ++----
 src/main/java/frc/robot/RobotContainer.java                 | 4 ++--
 src/main/java/frc/robot/subsystems/coral/CoralIntake.java   | 3 ++-
 .../java/frc/robot/subsystems/coral/CoralSubsystem.java     | 5 +++++
 4 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 0852fd4..e8f436e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -226,7 +226,7 @@ public static class PhysicalConstants {
         public static final double NET_REDUCTION = 45.0;
         public static final double MASS_KG = 2.85; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.083;
-        public static final double MOI = 0.00403605447; // Kg*m^2
+        public static final double MOI = 0.0403605447; // Kg*m^2
       }
     }
 
@@ -267,9 +267,7 @@ public static class Intake {
 
       public static final double SCORE_EXTRA_SECONDS = 1.0;
 
-      public static final double IN_OUT_CURRENT_LIMIT = 60.0; // Stator limit
-      public static final double HOLD_CURRENT_LIMIT = 5.0; // Stator, TODO: Test this!
-      // TODO: ################### PLACHOLDERS ###################
+      public static final double CURRENT_LIMIT = 12.5; // Stator
 
       public static final class PhysicalConstants {
         public static final DCMotor MOTOR = DCMotor.getFalcon500(1);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fe59f81..5e11b10 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -119,8 +119,8 @@ private Command getGoToLockedPresetCommand() {
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
                 .andThen(new ParallelCommandGroup(
                         new MovePivot(coralSubsystem, currentLockedPresetSupplier),
-                        new MoveRoll(coralSubsystem, currentLockedPresetSupplier),
-                        new MovePitch(coralSubsystem, currentLockedPresetSupplier)))
+                        new MoveRoll(coralSubsystem, currentLockedPresetSupplier)))
+                .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier))
                 .andThen(new InstantCommand(() -> {
                     SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done");
                 }));
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 9609a24..55d0882 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -40,7 +40,8 @@ public class CoralIntake extends SubsystemBase {
     public CoralIntake() {
         intakeMotor.setNeutralMode(NeutralModeValue.Brake);
         intakeMotor.getConfigurator()
-                .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(12.5).withStatorCurrentLimitEnable(true));
+                .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Coral.Intake.CURRENT_LIMIT)
+                        .withStatorCurrentLimitEnable(true));
         intakeMotor.getConfigurator()
                 .apply(new MotorOutputConfigs()
                         .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 8df61a9..8e18d75 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -94,6 +94,11 @@ public void periodic() {
         SmartDashboard.putBoolean("Roll in position", isRollInPosition());
         SmartDashboard.putBoolean("Pitch in position", isPitchInPosition());
         SmartDashboard.putBoolean("Pivot in position", isPivotInPosition());
+
+        SmartDashboard.putBoolean("Elevator SUPPOSED to be in position", isElevatorSupposedToBeInPosition());
+        SmartDashboard.putBoolean("Roll SUPPOSED to be in position", isRollSupposedToBeInPosition());
+        SmartDashboard.putBoolean("Pitch SUPPOSED to be in position", isPitchSupposedToBeInPosition());
+        SmartDashboard.putBoolean("Pivot SUPPOSED to be in position", isPivotSupposedToBeInPosition());
     }
 
     private CoralPresets currentPreset = CoralPresets.STOW;

From 66f4b5aeb95dd83f0e9083b0834051f70af913ab Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Wed, 12 Feb 2025 18:26:19 -0600
Subject: [PATCH 55/71] Assisted scoring init, based on LL

---
 src/main/java/frc/robot/LimelightHelpers.java | 811 ++++++++++++++++--
 src/main/java/frc/robot/RobotContainer.java   |  11 +
 .../java/frc/robot/subsystems/Limelight.java  | 201 +++++
 .../subsystems/coral/CoralSubsystem.java      |   7 +-
 .../frc/robot/util/LimelightContainer.java    | 241 ++++++
 5 files changed, 1190 insertions(+), 81 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/Limelight.java
 create mode 100644 src/main/java/frc/robot/util/LimelightContainer.java

diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java
index 3f0b9af..854968f 100644
--- a/src/main/java/frc/robot/LimelightHelpers.java
+++ b/src/main/java/frc/robot/LimelightHelpers.java
@@ -1,10 +1,14 @@
-//LimelightHelpers v1.5.0 (March 27, 2024)
+//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER)
 
 package frc.robot;
 
+import edu.wpi.first.networktables.DoubleArrayEntry;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.TimestampedDoubleArray;
+import frc.robot.LimelightHelpers.LimelightResults;
+import frc.robot.LimelightHelpers.PoseEstimate;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -17,6 +21,7 @@
 import java.net.HttpURLConnection;
 import java.net.MalformedURLException;
 import java.net.URL;
+import java.util.Map;
 import java.util.concurrent.CompletableFuture;
 
 import com.fasterxml.jackson.annotation.JsonFormat;
@@ -25,9 +30,19 @@
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.DeserializationFeature;
 import com.fasterxml.jackson.databind.ObjectMapper;
+import java.util.concurrent.ConcurrentHashMap;
 
+/**
+ * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC.
+ * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking.
+ */
 public class LimelightHelpers {
 
+    private static final Map<String, DoubleArrayEntry> doubleArrayEntries = new ConcurrentHashMap<>();
+
+    /**
+     * Represents a Color/Retroreflective Target Result extracted from JSON Output
+     */
     public static class LimelightTarget_Retro {
 
         @JsonProperty("t6c_ts")
@@ -92,16 +107,22 @@ public Pose2d getTargetPose_RobotSpace2D()
 
         @JsonProperty("tx")
         public double tx;
+        
+        @JsonProperty("ty")
+        public double ty;
 
         @JsonProperty("txp")
         public double tx_pixels;
 
-        @JsonProperty("ty")
-        public double ty;
-
         @JsonProperty("typ")
         public double ty_pixels;
 
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
         @JsonProperty("ts")
         public double ts;
 
@@ -115,6 +136,9 @@ public LimelightTarget_Retro() {
 
     }
 
+    /**
+     * Represents an AprilTag/Fiducial Target Result extracted from JSON Output
+     */
     public static class LimelightTarget_Fiducial {
 
         @JsonProperty("fID")
@@ -186,15 +210,21 @@ public Pose2d getTargetPose_RobotSpace2D()
         @JsonProperty("tx")
         public double tx;
 
-        @JsonProperty("txp")
-        public double tx_pixels;
-
         @JsonProperty("ty")
         public double ty;
 
+        @JsonProperty("txp")
+        public double tx_pixels;
+
         @JsonProperty("typ")
         public double ty_pixels;
 
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
         @JsonProperty("ts")
         public double ts;
         
@@ -207,10 +237,58 @@ public LimelightTarget_Fiducial() {
         }
     }
 
+    /**
+     * Represents a Barcode Target Result extracted from JSON Output
+     */
     public static class LimelightTarget_Barcode {
 
+        /**
+         * Barcode family type (e.g. "QR", "DataMatrix", etc.)
+         */
+        @JsonProperty("fam")
+        public String family;
+
+        /**
+         * Gets the decoded data content of the barcode
+         */
+        @JsonProperty("data") 
+        public String data;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("pts")
+        public double[][] corners;
+
+        public LimelightTarget_Barcode() {
+        }
+
+        public String getFamily() {
+            return family;
+        }
     }
 
+    /**
+     * Represents a Neural Classifier Pipeline Result extracted from JSON Output
+     */
     public static class LimelightTarget_Classifier {
 
         @JsonProperty("class")
@@ -241,6 +319,9 @@ public  LimelightTarget_Classifier() {
         }
     }
 
+    /**
+     * Represents a Neural Detector Pipeline Result extracted from JSON Output
+     */
     public static class LimelightTarget_Detector {
 
         @JsonProperty("class")
@@ -258,21 +339,32 @@ public static class LimelightTarget_Detector {
         @JsonProperty("tx")
         public double tx;
 
-        @JsonProperty("txp")
-        public double tx_pixels;
-
         @JsonProperty("ty")
         public double ty;
 
+        @JsonProperty("txp")
+        public double tx_pixels;
+
         @JsonProperty("typ")
         public double ty_pixels;
 
+        @JsonProperty("tx_nocross")
+        public double tx_nocrosshair;
+
+        @JsonProperty("ty_nocross")
+        public double ty_nocrosshair;
+
         public LimelightTarget_Detector() {
         }
     }
 
-    public static class Results {
-
+    /**
+     * Limelight Results object, parsed from a Limelight's JSON results output.
+     */
+    public static class LimelightResults {
+        
+        public String error;
+        
         @JsonProperty("pID")
         public double pipelineID;
 
@@ -357,7 +449,7 @@ public Pose2d getBotPose2d_wpiBlue() {
         @JsonProperty("Barcode")
         public LimelightTarget_Barcode[] targets_Barcode;
 
-        public Results() {
+        public LimelightResults() {
             botpose = new double[6];
             botpose_wpired = new double[6];
             botpose_wpiblue = new double[6];
@@ -369,30 +461,21 @@ public Results() {
             targets_Barcode = new LimelightTarget_Barcode[0];
 
         }
-    }
-
-    public static class LimelightResults {
-        @JsonProperty("Results")
-        public Results targetingResults;
-        
-        public String error;
-
-        public LimelightResults() {
-            targetingResults = new Results();
-            error = "";
-        }
 
 
     }
 
+    /**
+     * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output.
+     */
     public static class RawFiducial {
-        public int id;
-        public double txnc;
-        public double tync;
-        public double ta;
-        public double distToCamera;
-        public double distToRobot;
-        public double ambiguity;
+        public int id = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double distToCamera = 0;
+        public double distToRobot = 0;
+        public double ambiguity = 0;
 
 
         public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
@@ -406,6 +489,47 @@ public RawFiducial(int id, double txnc, double tync, double ta, double distToCam
         }
     }
 
+    /**
+     * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output.
+     */
+    public static class RawDetection {
+        public int classId = 0;
+        public double txnc = 0;
+        public double tync = 0;
+        public double ta = 0;
+        public double corner0_X = 0;
+        public double corner0_Y = 0;
+        public double corner1_X = 0;
+        public double corner1_Y = 0;
+        public double corner2_X = 0;
+        public double corner2_Y = 0;
+        public double corner3_X = 0;
+        public double corner3_Y = 0;
+
+
+        public RawDetection(int classId, double txnc, double tync, double ta, 
+            double corner0_X, double corner0_Y, 
+            double corner1_X, double corner1_Y, 
+            double corner2_X, double corner2_Y, 
+            double corner3_X, double corner3_Y ) {
+            this.classId = classId;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.corner0_X = corner0_X;
+            this.corner0_Y = corner0_Y;
+            this.corner1_X = corner1_X;
+            this.corner1_Y = corner1_Y;
+            this.corner2_X = corner2_X;
+            this.corner2_Y = corner2_Y;
+            this.corner3_X = corner3_X;
+            this.corner3_Y = corner3_Y;
+        }
+    }
+    
+    /**
+     * Represents a 3D Pose Estimate.
+     */
     public static class PoseEstimate {
         public Pose2d pose;
         public double timestampSeconds;
@@ -414,11 +538,28 @@ public static class PoseEstimate {
         public double tagSpan;
         public double avgTagDist;
         public double avgTagArea;
+
         public RawFiducial[] rawFiducials; 
+        public boolean isMegaTag2;
+
+        /**
+         * Instantiates a PoseEstimate object with default values
+         */
+        public PoseEstimate() {
+            this.pose = new Pose2d();
+            this.timestampSeconds = 0;
+            this.latency = 0;
+            this.tagCount = 0;
+            this.tagSpan = 0;
+            this.avgTagDist = 0;
+            this.avgTagArea = 0;
+            this.rawFiducials = new RawFiducial[]{};
+            this.isMegaTag2 = false;
+        }
 
         public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
             int tagCount, double tagSpan, double avgTagDist, 
-            double avgTagArea, RawFiducial[] rawFiducials) {
+            double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) {
 
             this.pose = pose;
             this.timestampSeconds = timestampSeconds;
@@ -428,9 +569,45 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency,
             this.avgTagDist = avgTagDist;
             this.avgTagArea = avgTagArea;
             this.rawFiducials = rawFiducials;
+            this.isMegaTag2 = isMegaTag2;
         }
+
     }
 
+    /**
+     * Encapsulates the state of an internal Limelight IMU.
+     */
+    public static class IMUData {
+        public double robotYaw = 0.0;
+        public double Roll = 0.0;
+        public double Pitch = 0.0;
+        public double Yaw = 0.0;
+        public double gyroX = 0.0;
+        public double gyroY = 0.0;
+        public double gyroZ = 0.0;
+        public double accelX = 0.0;
+        public double accelY = 0.0;
+        public double accelZ = 0.0;
+
+        public IMUData() {}
+
+        public IMUData(double[] imuData) {
+            if (imuData != null && imuData.length >= 10) {
+                this.robotYaw = imuData[0];
+                this.Roll = imuData[1];
+                this.Pitch = imuData[2];
+                this.Yaw = imuData[3];
+                this.gyroX = imuData[4];
+                this.gyroY = imuData[5];
+                this.gyroZ = imuData[6];
+                this.accelX = imuData[7];
+                this.accelY = imuData[8];
+                this.accelZ = imuData[9];
+            }
+        }
+    }
+
+
     private static ObjectMapper mapper;
 
     /**
@@ -445,7 +622,13 @@ static final String sanitizeName(String name) {
         return name;
     }
 
-    private static Pose3d toPose3D(double[] inData){
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose3d object.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose3d object representing the pose, or empty Pose3d if invalid data
+     */
+    public static Pose3d toPose3D(double[] inData){
         if(inData.length < 6)
         {
             //System.err.println("Bad LL 3D Pose Data!");
@@ -457,7 +640,14 @@ private static Pose3d toPose3D(double[] inData){
                     Units.degreesToRadians(inData[5])));
     }
 
-    private static Pose2d toPose2D(double[] inData){
+    /**
+     * Takes a 6-length array of pose data and converts it to a Pose2d object.
+     * Uses only x, y, and yaw components, ignoring z, roll, and pitch.
+     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
+     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+     * @return Pose2d object representing the pose, or empty Pose2d if invalid data
+     */
+    public static Pose2d toPose2D(double[] inData){
         if(inData.length < 6)
         {
             //System.err.println("Bad LL 2D Pose Data!");
@@ -468,7 +658,44 @@ private static Pose2d toPose2D(double[] inData){
         return new Pose2d(tran2d, r2d);
     }
 
-    private static double extractBotPoseEntry(double[] inData, int position){
+    /**
+     * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * 
+     * @param pose The Pose3d object to convert
+     * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
+     */
+    public static double[] pose3dToArray(Pose3d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = pose.getTranslation().getZ();
+        result[3] = Units.radiansToDegrees(pose.getRotation().getX());
+        result[4] = Units.radiansToDegrees(pose.getRotation().getY());
+        result[5] = Units.radiansToDegrees(pose.getRotation().getZ());
+        return result;
+    }
+
+    /**
+     * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+     * Translation components are in meters, rotation components are in degrees.
+     * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw.
+     * 
+     * @param pose The Pose2d object to convert
+     * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
+     */
+    public static double[] pose2dToArray(Pose2d pose) {
+        double[] result = new double[6];
+        result[0] = pose.getTranslation().getX();
+        result[1] = pose.getTranslation().getY();
+        result[2] = 0;
+        result[3] = Units.radiansToDegrees(0);
+        result[4] = Units.radiansToDegrees(0);
+        result[5] = Units.radiansToDegrees(pose.getRotation().getRadians());
+        return result;
+    }
+
+    private static double extractArrayEntry(double[] inData, int position){
         if(inData.length < position+1)
         {
             return 0;
@@ -476,27 +703,35 @@ private static double extractBotPoseEntry(double[] inData, int position){
         return inData[position];
     }
 
-    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
-        var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName);
-        var poseArray = poseEntry.getDoubleArray(new double[0]);
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) {
+        DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
+        
+        TimestampedDoubleArray tsValue = poseEntry.getAtomic();
+        double[] poseArray = tsValue.value;
+        long timestamp = tsValue.timestamp;
+        
+        if (poseArray.length == 0) {
+            // Handle the case where no data is available
+            return null; // or some default PoseEstimate
+        }
+    
         var pose = toPose2D(poseArray);
-        double latency = extractBotPoseEntry(poseArray,6);
-        int tagCount = (int)extractBotPoseEntry(poseArray,7);
-        double tagSpan = extractBotPoseEntry(poseArray,8);
-        double tagDist = extractBotPoseEntry(poseArray,9);
-        double tagArea = extractBotPoseEntry(poseArray,10);
-        //getlastchange() in microseconds, ll latency in milliseconds
-        var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0);
-
-
+        double latency = extractArrayEntry(poseArray, 6);
+        int tagCount = (int)extractArrayEntry(poseArray, 7);
+        double tagSpan = extractArrayEntry(poseArray, 8);
+        double tagDist = extractArrayEntry(poseArray, 9);
+        double tagArea = extractArrayEntry(poseArray, 10);
+        
+        // Convert server timestamp from microseconds to seconds and adjust for latency
+        double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0);
+    
         RawFiducial[] rawFiducials = new RawFiducial[tagCount];
         int valsPerFiducial = 7;
-        int expectedTotalVals = 11 + valsPerFiducial*tagCount;
-
+        int expectedTotalVals = 11 + valsPerFiducial * tagCount;
+    
         if (poseArray.length != expectedTotalVals) {
             // Don't populate fiducials
-        }
-        else{
+        } else {
             for(int i = 0; i < tagCount; i++) {
                 int baseIndex = 11 + (i * valsPerFiducial);
                 int id = (int)poseArray[baseIndex];
@@ -509,11 +744,89 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr
                 rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
             }
         }
+    
+        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2);
+    }
 
-        return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
+    /**
+     * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawFiducial objects containing detection details
+     */
+    public static RawFiducial[] getRawFiducials(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
+        var rawFiducialArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 7;
+        if (rawFiducialArray.length % valsPerEntry != 0) {
+            return new RawFiducial[0];
+        }
+    
+        int numFiducials = rawFiducialArray.length / valsPerEntry;
+        RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
+    
+        for (int i = 0; i < numFiducials; i++) {
+            int baseIndex = i * valsPerEntry;
+            int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
+            double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
+            double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
+            double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
+            double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
+            
+            rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+        }
+    
+        return rawFiducials;
     }
 
-    private static void printPoseEstimate(PoseEstimate pose) {
+    /**
+     * Gets the latest raw neural detector results from NetworkTables
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @return Array of RawDetection objects containing detection details
+     */
+    public static RawDetection[] getRawDetections(String limelightName) {
+        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
+        var rawDetectionArray = entry.getDoubleArray(new double[0]);
+        int valsPerEntry = 12;
+        if (rawDetectionArray.length % valsPerEntry != 0) {
+            return new RawDetection[0];
+        }
+    
+        int numDetections = rawDetectionArray.length / valsPerEntry;
+        RawDetection[] rawDetections = new RawDetection[numDetections];
+    
+        for (int i = 0; i < numDetections; i++) {
+            int baseIndex = i * valsPerEntry; // Starting index for this detection's data
+            int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex);
+            double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1);
+            double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2);
+            double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3);
+            double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4);
+            double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5);
+            double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6);
+            double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7);
+            double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8);
+            double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9);
+            double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10);
+            double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11);
+            
+            rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y);
+        }
+    
+        return rawDetections;
+    }
+
+    /**
+     * Prints detailed information about a PoseEstimate to standard output.
+     * Includes timestamp, latency, tag count, tag span, average tag distance,
+     * average tag area, and detailed information about each detected fiducial.
+     *
+     * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
+     */
+    public static void printPoseEstimate(PoseEstimate pose) {
         if (pose == null) {
             System.out.println("No PoseEstimate available.");
             return;
@@ -526,6 +839,7 @@ private static void printPoseEstimate(PoseEstimate pose) {
         System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
         System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
         System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
         System.out.println();
     
         if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
@@ -548,14 +862,30 @@ private static void printPoseEstimate(PoseEstimate pose) {
         }
     }
 
+    public static Boolean validPoseEstimate(PoseEstimate pose) {
+        return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+    }
+
     public static NetworkTable getLimelightNTTable(String tableName) {
         return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
     }
 
+    public static void Flush() {
+        NetworkTableInstance.getDefault().flush();
+    }
+
     public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
         return getLimelightNTTable(tableName).getEntry(entryName);
     }
 
+    public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
+        String key = tableName + "/" + entryName;
+        return doubleArrayEntries.computeIfAbsent(key, k -> {
+            NetworkTable table = getLimelightNTTable(tableName);
+            return table.getDoubleArrayTopic(entryName).getEntry(new double[0]);
+        });
+    }
+    
     public static double getLimelightNTDouble(String tableName, String entryName) {
         return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
     }
@@ -572,10 +902,16 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN
         return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
     }
 
+
     public static String getLimelightNTString(String tableName, String entryName) {
         return getLimelightNTTableEntry(tableName, entryName).getString("");
     }
 
+    public static String[] getLimelightNTStringArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
+    }
+
+
     public static URL getLimelightURLString(String tableName, String request) {
         String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
         URL url;
@@ -590,30 +926,171 @@ public static URL getLimelightURLString(String tableName, String request) {
     /////
     /////
 
+    /**
+     * Does the Limelight have a valid target?
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return True if a valid target is present, false otherwise
+     */
+    public static boolean getTV(String limelightName) {
+        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+    }
+
+    /**
+     * Gets the horizontal offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
     public static double getTX(String limelightName) {
         return getLimelightNTDouble(limelightName, "tx");
     }
 
+    /**
+     * Gets the vertical offset from the crosshair to the target in degrees.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
     public static double getTY(String limelightName) {
         return getLimelightNTDouble(limelightName, "ty");
     }
 
+    /**
+     * Gets the horizontal offset from the principal pixel/point to the target in degrees.  This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Horizontal offset angle in degrees
+     */
+    public static double getTXNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "txnc");
+    }
+
+    /**
+     * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
+     * @param limelightName Name of the Limelight camera ("" for default)
+     * @return Vertical offset angle in degrees
+     */
+    public static double getTYNC(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tync");
+    }
+
+    /**
+     * Gets the target area as a percentage of the image (0-100%).
+     * @param limelightName Name of the Limelight camera ("" for default) 
+     * @return Target area percentage (0-100)
+     */
     public static double getTA(String limelightName) {
         return getLimelightNTDouble(limelightName, "ta");
     }
 
+    /**
+     * T2D is an array that contains several targeting metrcis
+     * @param limelightName Name of the Limelight camera
+     * @return Array containing  [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, 
+     * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees]
+     */
+    public static double[] getT2DArray(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "t2d");
+    }
+    
+    /**
+     * Gets the number of targets currently detected.
+     * @param limelightName Name of the Limelight camera
+     * @return Number of detected targets
+     */
+    public static int getTargetCount(String limelightName) {
+      double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[1];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the classifier class index from the currently running neural classifier pipeline
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from classifier pipeline
+     */
+    public static int getClassifierClassIndex (String limelightName) {
+    double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[10];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the detector class index from the primary result of the currently running neural detector pipeline.
+     * @param limelightName Name of the Limelight camera
+     * @return Class index from detector pipeline
+     */
+    public static int getDetectorClassIndex (String limelightName) {
+     double[] t2d = getT2DArray(limelightName);
+      if(t2d.length == 17)
+      {
+        return (int)t2d[11];
+      }
+      return 0;
+    }
+
+    /**
+     * Gets the current neural classifier result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from classifier pipeline
+     */
+    public static String getClassifierClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tcclass");
+    }
+
+    /**
+     * Gets the primary neural detector result class name.
+     * @param limelightName Name of the Limelight camera
+     * @return Class name string from detector pipeline
+     */
+    public static String getDetectorClass (String limelightName) {
+        return getLimelightNTString(limelightName, "tdclass");
+    }
+
+    /**
+     * Gets the pipeline's processing latency contribution.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline latency in milliseconds
+     */
     public static double getLatency_Pipeline(String limelightName) {
         return getLimelightNTDouble(limelightName, "tl");
     }
 
+    /**
+     * Gets the capture latency.
+     * @param limelightName Name of the Limelight camera
+     * @return Capture latency in milliseconds
+     */
     public static double getLatency_Capture(String limelightName) {
         return getLimelightNTDouble(limelightName, "cl");
     }
 
+    /**
+     * Gets the active pipeline index.
+     * @param limelightName Name of the Limelight camera
+     * @return Current pipeline index (0-9)
+     */
     public static double getCurrentPipelineIndex(String limelightName) {
         return getLimelightNTDouble(limelightName, "getpipe");
     }
 
+    /**
+     * Gets the current pipeline type.
+     * @param limelightName Name of the Limelight camera
+     * @return Pipeline type string (e.g. "retro", "apriltag", etc)
+     */
+    public static String getCurrentPipelineType(String limelightName) {
+        return getLimelightNTString(limelightName, "getpipetype");
+    }
+
+    /**
+     * Gets the full JSON results dump.
+     * @param limelightName Name of the Limelight camera
+     * @return JSON string containing all current results
+     */
     public static String getJSONDump(String limelightName) {
         return getLimelightNTString(limelightName, "json");
     }
@@ -691,6 +1168,10 @@ public static String getNeuralClassID(String limelightName) {
         return getLimelightNTString(limelightName, "tclass");
     }
 
+    public static String[] getRawBarcodeData(String limelightName) {
+        return getLimelightNTStringArray(limelightName, "rawbarcodes");
+    }
+
     /////
     /////
 
@@ -699,36 +1180,71 @@ public static Pose3d getBotPose3d(String limelightName) {
         return toPose3D(poseArray);
     }
 
+    /**
+     * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Red Alliance field space
+     */
     public static Pose3d getBotPose3d_wpiRed(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
         return toPose3D(poseArray);
     }
 
+    /**
+     * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space
+     */
     public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
         return toPose3D(poseArray);
     }
 
+    /**
+     * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the robot's position and orientation relative to the target
+     */
     public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
         return toPose3D(poseArray);
     }
 
+    /**
+     * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the target
+     */
     public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
         return toPose3D(poseArray);
     }
 
+    /**
+     * Gets the target's 3D pose with respect to the camera's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the camera
+     */
     public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
         return toPose3D(poseArray);
     }
 
+    /**
+     * Gets the target's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the target's position and orientation relative to the robot
+     */
     public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
         return toPose3D(poseArray);
     }
 
+    /**
+     * Gets the camera's 3D pose with respect to the robot's coordinate system.
+     * @param limelightName Name/identifier of the Limelight
+     * @return Pose3d object representing the camera's position and orientation relative to the robot
+     */
     public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
         double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
         return toPose3D(poseArray);
@@ -748,25 +1264,24 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
     }
 
     /**
-     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
-     * alliance
+     * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
      * 
      * @param limelightName
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpiblue");
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
     }
 
     /**
-     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
-     * alliance
+     * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+     * Make sure you are calling setRobotOrientation() before calling this method.
      * 
      * @param limelightName
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
     }
 
     /**
@@ -790,7 +1305,7 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpired");
+        return getBotPoseEstimate(limelightName, "botpose_wpired", false);
     }
 
     /**
@@ -800,7 +1315,7 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
     }
 
     /**
@@ -816,9 +1331,21 @@ public static Pose2d getBotPose2d(String limelightName) {
         return toPose2D(result);
 
     }
-
-    public static boolean getTV(String limelightName) {
-        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+   
+    /**
+     * Gets the current IMU data from NetworkTables.
+     * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ].
+     * Returns all zeros if data is invalid or unavailable.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @return IMUData object containing all current IMU data
+     */
+    public static IMUData getIMUData(String limelightName) {
+        double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
+        if (imuData == null || imuData.length < 10) {
+            return new IMUData();  // Returns object with all zeros
+        }
+        return new IMUData(imuData);
     }
 
     /////
@@ -834,8 +1361,8 @@ public static void setPriorityTagID(String limelightName, int ID) {
     }
 
     /**
-     * The LEDs will be controlled by Limelight pipeline settings, and not by robot
-     * code.
+     * Sets LED mode to be controlled by the current pipeline.
+     * @param limelightName Name of the Limelight camera
      */
     public static void setLEDMode_PipelineControl(String limelightName) {
         setLimelightNTDouble(limelightName, "ledMode", 0);
@@ -853,29 +1380,38 @@ public static void setLEDMode_ForceOn(String limelightName) {
         setLimelightNTDouble(limelightName, "ledMode", 3);
     }
 
+    /**
+     * Enables standard side-by-side stream mode.
+     * @param limelightName Name of the Limelight camera
+     */
     public static void setStreamMode_Standard(String limelightName) {
         setLimelightNTDouble(limelightName, "stream", 0);
     }
 
+    /**
+     * Enables Picture-in-Picture mode with secondary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
     public static void setStreamMode_PiPMain(String limelightName) {
         setLimelightNTDouble(limelightName, "stream", 1);
     }
 
+    /**
+     * Enables Picture-in-Picture mode with primary stream in the corner.
+     * @param limelightName Name of the Limelight camera
+     */
     public static void setStreamMode_PiPSecondary(String limelightName) {
         setLimelightNTDouble(limelightName, "stream", 2);
     }
 
-    public static void setCameraMode_Processor(String limelightName) {
-        setLimelightNTDouble(limelightName, "camMode", 0);
-    }
-    public static void setCameraMode_Driver(String limelightName) {
-        setLimelightNTDouble(limelightName, "camMode", 1);
-    }
-
 
     /**
-     * Sets the crop window. The crop window in the UI must be completely open for
-     * dynamic cropping to work.
+     * Sets the crop window for the camera. The crop window in the UI must be completely open.
+     * @param limelightName Name of the Limelight camera
+     * @param cropXMin Minimum X value (-1 to 1)
+     * @param cropXMax Maximum X value (-1 to 1)
+     * @param cropYMin Minimum Y value (-1 to 1)
+     * @param cropYMax Maximum Y value (-1 to 1)
      */
     public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
         double[] entries = new double[4];
@@ -885,10 +1421,44 @@ public static void setCropWindow(String limelightName, double cropXMin, double c
         entries[3] = cropYMax;
         setLimelightNTDoubleArray(limelightName, "crop", entries);
     }
+   
+    /**
+     * Sets 3D offset point for easy 3D targeting.
+     */
+    public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) {
+        double[] entries = new double[3];
+        entries[0] = offsetX;
+        entries[1] = offsetY;
+        entries[2] = offsetZ;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
 
+    /**
+     * Sets robot orientation values used by MegaTag2 localization algorithm.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
+     * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
+     * @param pitch (Unnecessary) Robot pitch in degrees 
+     * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
+     * @param roll (Unnecessary) Robot roll in degrees
+     * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
+     */
     public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, 
         double pitch, double pitchRate, 
         double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
+    }
+
+    public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
+    }
+
+    private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate, boolean flush) {
 
         double[] entries = new double[6];
         entries[0] = yaw;
@@ -898,8 +1468,48 @@ public static void SetRobotOrientation(String limelightName, double yaw, double
         entries[4] = roll;
         entries[5] = rollRate;
         setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+        if(flush)
+        {
+            Flush();
+        }
+    }
+   
+    /**
+     * Configures the IMU mode for MegaTag2 Localization
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param mode IMU mode.
+     */
+    public static void SetIMUMode(String limelightName, int mode) {
+        setLimelightNTDouble(limelightName, "imumode_set", mode);
     }
 
+    /**
+     * Sets the 3D point-of-interest offset for the current fiducial pipeline. 
+     * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param x X offset in meters
+     * @param y Y offset in meters
+     * @param z Z offset in meters
+     */
+    public static void SetFidcuial3DOffset(String limelightName, double x, double y, 
+        double z) {
+
+        double[] entries = new double[3];
+        entries[0] = x;
+        entries[1] = y;
+        entries[2] = z;
+        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+    }
+
+    /**
+     * Overrides the valid AprilTag IDs that will be used for localization.
+     * Tags not in this list will be ignored for robot pose estimation.
+     *
+     * @param limelightName Name/identifier of the Limelight
+     * @param validIDs Array of valid AprilTag IDs to track
+     */
     public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
         double[] validIDsDouble = new double[validIDs.length];
         for (int i = 0; i < validIDs.length; i++) {
@@ -907,7 +1517,50 @@ public static void SetFiducialIDFiltersOverride(String limelightName, int[] vali
         }        
         setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
     }
+
+    /**
+     * Sets the downscaling factor for AprilTag detection.
+     * Increasing downscale can improve performance at the cost of potentially reduced detection range.
+     * 
+     * @param limelightName Name/identifier of the Limelight
+     * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control.
+     */
+    public static void SetFiducialDownscalingOverride(String limelightName, float downscale) 
+    {
+        int d = 0; // pipeline
+        if (downscale == 1.0)
+        {
+            d = 1;
+        }
+        if (downscale == 1.5)
+        {
+            d = 2;
+        }
+        if (downscale == 2)
+        {
+            d = 3;
+        }
+        if (downscale == 3)
+        {
+            d = 4;
+        }
+        if (downscale == 4)
+        {
+            d = 5;
+        }
+        setLimelightNTDouble(limelightName, "fiducial_downscale_set", d);
+    }
     
+    /**
+     * Sets the camera pose relative to the robot.
+     * @param limelightName Name of the Limelight camera
+     * @param forward Forward offset in meters
+     * @param side Side offset in meters
+     * @param up Up offset in meters
+     * @param roll Roll angle in degrees
+     * @param pitch Pitch angle in degrees
+     * @param yaw Yaw angle in degrees
+     */
     public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
         double[] entries = new double[6];
         entries[0] = forward;
@@ -964,7 +1617,9 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName)
     }
 
     /**
-     * Parses Limelight's JSON results dump into a LimelightResults Object
+     * Gets the latest JSON results output and returns a LimelightResults object.
+     * @param limelightName Name of the Limelight camera
+     * @return LimelightResults object containing all current target data
      */
     public static LimelightResults getLatestResults(String limelightName) {
 
@@ -982,7 +1637,7 @@ public static LimelightResults getLatestResults(String limelightName) {
 
         long end = System.nanoTime();
         double millis = (end - start) * .000001;
-        results.targetingResults.latency_jsonParse = millis;
+        results.latency_jsonParse = millis;
         if (profileJSON) {
             System.out.printf("lljson: %.2f\r\n", millis);
         }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5e11b10..3778db9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -34,6 +34,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.*;
+import frc.robot.util.LimelightContainer;
+import frc.robot.subsystems.Limelight.LimelightType;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -46,6 +48,15 @@
  */
 public class RobotContainer {
 
+    private static final Limelight LL_FR = new Limelight(LimelightType.LL4, "limelight-fr", true, true);
+    private static final Limelight LL_FL = new Limelight(LimelightType.LL4, "limelight-fl", true, true);
+    private static final Limelight LL_BR  = new Limelight(LimelightType.LL4, "limelight-br", true, true);
+    private static final Limelight LL_BL = new Limelight(LimelightType.LL4, "limelight-bl", true, true);
+
+    public static final LimelightContainer LLContainer = new LimelightContainer(LL_FR, LL_FL, LL_BR, LL_BL);
+
+
+
     private final CommandXboxController driverXbox = new CommandXboxController(
             ControllerConstants.DRIVER_CONTROLLER_PORT);
     private final CommandXboxController operatorXbox = new CommandXboxController(
diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
new file mode 100644
index 0000000..403db7f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -0,0 +1,201 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.LimelightHelpers;
+import frc.robot.LimelightHelpers.RawFiducial;
+import frc.robot.util.LimelightContainer;
+
+public class Limelight extends SubsystemBase {
+    public enum LimelightType {
+        // LL1 doesn't have specs listed, so these could be incorrect
+        LL1(54, 41),
+        LL2(62.5, 48.9),
+        LL2Plus(62.5, 48.9),
+        LL3(62.5, 48.9),
+        LL3G(82, 56.2),
+        LL4(82, 56.2);
+
+        private double HFOV;
+        private double VFOV;
+
+        private LimelightType(double HFOV, double VFOV) {
+            this.HFOV = HFOV;
+            this.VFOV = VFOV;
+        }
+    }
+
+    private final LimelightType limelightType;
+    private final String name;
+    private boolean isEnabled;
+    private boolean cropEnabled;
+    private double lastPoseEstimate = 0;
+    private int counter = 0;
+    private double lastFrame = 0;
+
+    public Limelight(LimelightType limelightType, String name, boolean isEnabled, boolean cropEnabled) {
+        this.limelightType = limelightType;
+        this.name = name;
+        this.isEnabled = isEnabled;
+        this.cropEnabled = cropEnabled;
+    }
+
+    @Override
+    public void periodic() {
+        if (isEnabled) {
+            if (cropEnabled) {
+                if (numTargets() > 0) {
+                    smartCrop();
+                } else {
+                    restoreCrop();
+                }
+            }
+        }
+    }
+
+    public int numTargets() {
+        return LimelightHelpers.getRawFiducials(name).length;
+    }
+
+    public void smartCrop() {
+        LimelightHelpers.PoseEstimate poseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name);
+        if(poseEstimate == null){return;}
+        if (lastPoseEstimate != poseEstimate.timestampSeconds){
+            counter = ++counter % 50;
+            if(counter > 40){ // For every 5 frames, out of 50, check the entire screen for apriltags
+                restoreCrop(); // 45 will be cropped onto whichever limelights they find
+                return;
+            }
+            lastPoseEstimate = poseEstimate.timestampSeconds; //making sure we're only running this process on new frames
+            RawFiducial[] targets = LimelightHelpers.getRawFiducials(name); //get our targets
+            boolean useCrop = true; //we are currently using it, more changes will be implemented
+            Translation2d[] targetTranslations = new Translation2d[targets.length]; 
+            double area = 0;
+
+            for (int i = 0; i < targetTranslations.length; ++i) {
+                targetTranslations[i] = new Translation2d(targets[i].txnc, targets[i].tync); 
+                area = Math.max(area, targets[i].ta); //Finding the largest area of the targets
+            }
+
+            double xc = 0, yc = 0;
+
+            for (int i = 0; i < targetTranslations.length; ++i) { // Add to the width, depending on the locations of the other targets
+                xc += targetTranslations[i].getX(); //If one is at (2, 1), the width of our box increases on each size by 2, and the height by 1
+                yc += targetTranslations[i].getY(); //xc & xy increase per target
+            }
+
+            xc /= targetTranslations.length; //Then, average depending on the number of targets
+            yc /= targetTranslations.length; // (2,1), (.05, .25), (-1, 2) -> xc =.35, yc= .75
+
+            xc = xc / (limelightType.HFOV / 2); // scales it to FOV
+            yc = yc / (limelightType.VFOV / 2);
+
+            double borderx = 0, bordery = 0; 
+
+            if (targetTranslations.length > 1) { // for more than one tag
+                borderx = (area + 0.75) * 0.22 * targetTranslations.length + .2; // relatively randomly generated border
+                bordery = (area + 0.5) * 0.22 * targetTranslations.length;
+            } 
+
+            //todo optimize
+
+            else {
+                if (area > 0.75){
+                    borderx = 1;
+                    bordery = .5;
+                }
+                else if (area > 0.5) {  //Remember that "area" only refers to the area of the largest apriltag
+                    borderx = 0.8; 
+                    bordery = 0.5;
+                } 
+                else if (area > .015){ 
+                    borderx = 1; 
+                    bordery = 0.25;
+                }
+                else if (area > .005){
+                    borderx = 1;
+                    bordery = .5;
+                }
+                else { 
+                    borderx = 1;
+                    bordery = .5;
+                }
+            }
+
+            /*else {
+                if (area < 0.015) {
+                    borderx = 0.5;
+                    bordery = 0.25;
+                } 
+                else if (area > .005){
+                    borderx = 1;
+                    bordery = 0.5;
+                }
+                else {
+                    borderx = 0.5;
+                    bordery = 0.5;
+                }
+            }
+            */
+            //borderx = 1;
+            //bordery = .25;
+            // Doesn't let borders extend beyond -1, 1 for x & y
+            double xlim = (xc - borderx < -1) ? -1 : xc - borderx;
+            double xlim2 = (xc - borderx > 1) ? 1 : xc + borderx;
+            double ylim = (yc - bordery < -1) ? -1 : yc - borderx;
+            double ylim2 = (yc - bordery > 1) ? 1 : xc + borderx;
+
+            if (useCrop) {
+                LimelightHelpers.setCropWindow(name, xlim, xlim2, ylim, ylim2);
+                // Finds the center of the targets, tries to build a big enough box from there
+            }
+        }
+    }
+
+    public void restoreCrop() {
+        LimelightHelpers.setCropWindow(name, -1, 1, -1, 1);
+    }
+
+    
+    public void setEnabled(boolean enabled) {
+        this.isEnabled = enabled;
+    }
+
+    public void setCropEnabled(boolean enabled) {
+        this.cropEnabled = enabled;
+    }
+
+    public boolean isEnabled() {
+        return isEnabled;
+    }
+
+    public boolean isCropEnabled() {
+        return cropEnabled;
+    }
+
+    public double getLastFrameTime(){
+        return lastFrame;
+    }
+
+    public void setLastFrame(double lastFrameTime){
+        lastFrame = lastFrameTime;
+    }
+
+    public double getVFOV() {
+        return limelightType.VFOV;
+    }
+
+    public double getHFOV() {
+        return limelightType.HFOV;
+    }
+
+    public LimelightType getLimelightType() {
+        return limelightType;
+    }
+
+    public String getName() {
+        return name;
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 8e18d75..0aebe3c 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.util.LimelightContainer;
 
 public class CoralSubsystem extends SubsystemBase {
 
@@ -218,10 +219,10 @@ public void setCustomPitchDegrees(double pitchAngle) {
     }
 
     public void mirrorArm() {
-        if (mirrorSetting.isMirrored) {
-            mirrorSetting = MirrorPresets.RIGHT;
-        } else {
+        if (LimelightContainer.isOnLeft()) {
             mirrorSetting = MirrorPresets.LEFT;
+        } else {
+            mirrorSetting = MirrorPresets.RIGHT;
         }
     }
 
diff --git a/src/main/java/frc/robot/util/LimelightContainer.java b/src/main/java/frc/robot/util/LimelightContainer.java
new file mode 100644
index 0000000..81edf12
--- /dev/null
+++ b/src/main/java/frc/robot/util/LimelightContainer.java
@@ -0,0 +1,241 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util;
+
+import java.util.ArrayList;
+
+import com.studica.frc.AHRS;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.LimelightHelpers;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.subsystems.Limelight;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+
+public class LimelightContainer {
+  static int SIMCOUNTER = 0;
+  static int RLCOUNTER = 0;
+  static int RLCountermt1 = 0;
+  private static ArrayList<Limelight> limelights = new ArrayList<Limelight>();
+
+
+  public static boolean isOnLeft(){
+    int[] validIds = new int[6]; //change these to be all reef tags, for given alliance (check if they red or blue!!)
+    
+    if(FieldConstants.getAlliance() == Alliance.Blue){
+        for(int i = 0; i < 6; i++){
+        validIds[i] = i+17;
+        }
+    }
+    else if(FieldConstants.getAlliance() == Alliance.Red){
+        for(int i = 0; i < 6; i++){
+            validIds[i] = i +6;
+        }
+    }
+
+    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", validIds);
+    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", validIds);
+
+    if(LimelightHelpers.getTA("limelight-fl")>5){
+        return true;
+    }
+    else if(LimelightHelpers.getTA("limelight-fr")>5){
+        return false;
+    }
+
+    int[] allIDs = new int[22];
+    for(int i = 1; i <= 22; i++){
+        allIDs[i-1] = i; // sets allIDs to be {1, 2, 3, etc.}
+    }
+
+    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", allIDs);
+    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", allIDs);
+
+    else return true;
+  }
+
+  public LimelightContainer(Limelight... limelights) {
+    for (Limelight limelight : limelights) {
+      LimelightContainer.limelights.add(limelight);
+    }
+    enableLimelights(true);
+  }
+
+  public void enableLimelights(boolean enable) {
+    for (Limelight limelight : limelights) {
+      if(limelight != null){
+        limelight.setEnabled(enable);
+      }
+    }
+  }
+
+  public static void estimateSimMT1() {
+    for (Limelight limelight : limelights) {
+      boolean doRejectUpdate = false;
+      LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName());
+      if (mt1 == null) { // in case not all limelights are connected
+        continue;
+      }
+      if (mt1.tagCount == 0) {
+        doRejectUpdate = true;
+      }
+      if (!doRejectUpdate) {
+        SmartDashboard.putString("Simulated Pos MT1", mt1.pose.toString() + SIMCOUNTER);
+        SIMCOUNTER++;
+      }
+    }
+  }
+
+  public void estimateMT1OdometryPrelim(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx,
+      SwerveModulePosition[] swerveModulePositions) {
+    int framesChecked = 0;
+    ArrayList<Pose2d> validPoses = new ArrayList<>();
+
+    for (int i = 0; (i < 100) && (framesChecked < 10); i++) {
+      for (Limelight limelight : limelights) {
+
+        LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName());
+
+        if (mt1 == null) {
+          continue;
+        }
+
+        double currtime = mt1.timestampSeconds;
+        if (currtime != limelight.getLastFrameTime()) {
+          framesChecked++;
+          boolean doRejectUpdate = false;
+
+          if (mt1.tagCount == 0) {
+            doRejectUpdate = true;
+          }
+
+          if (Math.abs(navx.getRate()) > 720) {
+            doRejectUpdate = true;
+          }
+
+          if (!doRejectUpdate) {
+            validPoses.add(mt1.pose);
+            SmartDashboard.putString("Pos MT1 prelim: ", mt1.pose.toString() + " " + RLCountermt1);
+          }
+
+          RLCountermt1++;
+          limelight.setLastFrame(currtime);
+        }
+      }
+    }
+
+    if (!validPoses.isEmpty()) {
+      double avgX = 0, avgY = 0, avgRotation = 0;
+      for (Pose2d pose : validPoses) {
+        avgX += pose.getX();
+        avgY += pose.getY();
+        avgRotation += pose.getRotation().getDegrees();
+      }
+      avgX /= validPoses.size();
+      avgY /= validPoses.size();
+      avgRotation /= validPoses.size();
+
+      odometry.resetPosition(Rotation2d.fromDegrees(avgRotation), swerveModulePositions, new Pose2d(avgX, avgY, Rotation2d.fromDegrees(avgRotation)));
+    }
+  }
+
+  public void estimateMT1Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx) {
+    for (Limelight limelight : limelights) {
+      boolean doRejectUpdate = false;
+
+      LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName());
+
+      if (mt1 == null) {
+        continue;
+      }
+
+      if (mt1.tagCount == 0) {
+        doRejectUpdate = true;
+      }
+
+      if (Math.abs(navx.getRate()) > 720) {
+        doRejectUpdate = true;
+      }
+      
+      if (Math.abs(odometry.getEstimatedPosition().getX() - mt1.pose.getX()) > .4) {
+        doRejectUpdate = true; 
+        SmartDashboard.putBoolean("Rejected due to too-far pose", true);
+       }
+
+      if (!doRejectUpdate) {
+        odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.9, .9, .2));
+        odometry.addVisionMeasurement(
+            mt1.pose,
+            mt1.timestampSeconds);
+      }
+
+      RLCountermt1++;
+    }
+  }
+
+  public void estimateMT2Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx) {
+
+    for (Limelight limelight : limelights) {
+      boolean doRejectUpdate = false;
+
+
+      LimelightHelpers.SetRobotOrientation(limelight.getName(),
+          odometry.getEstimatedPosition().getRotation().getDegrees(), navx.getRate(), 0, 0, 0, 0);
+
+
+      LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName());
+
+      if (mt2 == null) {
+        continue;
+      }
+
+      if (Math.abs(navx.getRate()) > 720) {
+        doRejectUpdate = true;
+      }
+
+      if (mt2.tagCount == 0) {
+        doRejectUpdate = true;
+      }
+
+      if (mt2.pose.getX() == 8.77 && mt2.pose.getY() == 4.03) {
+        doRejectUpdate = true;
+      }
+
+      if (Math.abs(odometry.getEstimatedPosition().getX() - mt2.pose.getX()) > .4) {
+        doRejectUpdate = true; 
+        SmartDashboard.putBoolean("Rejected due to too-far pose", true);
+      }
+
+      if (!doRejectUpdate) {
+
+        SmartDashboard.putString("Pos (mt2): ", mt2.pose.toString() + " " + RLCOUNTER);
+
+        if (mt2.tagCount == 1) {
+          odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.4, .4, .3));
+          odometry.addVisionMeasurement(
+              mt2.pose,
+              mt2.timestampSeconds);
+        } else if (mt2.tagCount >= 2) {
+          odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.1, .1, .1));
+          odometry.addVisionMeasurement(
+              mt2.pose,
+              mt2.timestampSeconds);
+
+        }
+
+        SmartDashboard.putNumber("Dist", mt2.avgTagDist);
+      }
+      RLCOUNTER++;
+    }
+  }
+
+}

From cec0c9ee68837be884561668d975345705ad8403 Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Wed, 12 Feb 2025 18:30:41 -0600
Subject: [PATCH 56/71] small tweaks type

---
 .../frc/robot/util/LimelightContainer.java    | 30 +++++++++++--------
 1 file changed, 18 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/util/LimelightContainer.java b/src/main/java/frc/robot/util/LimelightContainer.java
index 81edf12..6311591 100644
--- a/src/main/java/frc/robot/util/LimelightContainer.java
+++ b/src/main/java/frc/robot/util/LimelightContainer.java
@@ -45,22 +45,28 @@ else if(FieldConstants.getAlliance() == Alliance.Red){
     LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", validIds);
     LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", validIds);
 
-    if(LimelightHelpers.getTA("limelight-fl")>5){
+
+    if(LimelightHelpers.getTA("limelight-fl")>1){
+      int[] allIDs = new int[22];
+      for(int i = 1; i <= 22; i++){
+          allIDs[i-1] = i; // sets allIDs to be {1, 2, 3, etc.}
+      }
+      LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", allIDs);
+      LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", allIDs);
         return true;
     }
-    else if(LimelightHelpers.getTA("limelight-fr")>5){
-        return false;
-    }
+    else if(LimelightHelpers.getTA("limelight-fr")>1){
+      int[] allIDs = new int[22];
+      for(int i = 1; i <= 22; i++){
+          allIDs[i-1] = i; // sets allIDs to be {1, 2, 3, etc.}
+      }
+      LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", allIDs);
+      LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", allIDs);
 
-    int[] allIDs = new int[22];
-    for(int i = 1; i <= 22; i++){
-        allIDs[i-1] = i; // sets allIDs to be {1, 2, 3, etc.}
+        return false;
     }
-
-    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", allIDs);
-    LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", allIDs);
-
-    else return true;
+    else return false;
+    
   }
 
   public LimelightContainer(Limelight... limelights) {

From 0c32d01086880254f3fcdb46c861f4a4527017a3 Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Wed, 12 Feb 2025 21:07:40 -0600
Subject: [PATCH 57/71] Accounting for intake

---
 .../frc/robot/subsystems/coral/CoralSubsystem.java  |  2 +-
 .../java/frc/robot/util/LimelightContainer.java     | 13 ++++++++-----
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 0aebe3c..5a24905 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -219,7 +219,7 @@ public void setCustomPitchDegrees(double pitchAngle) {
     }
 
     public void mirrorArm() {
-        if (LimelightContainer.isOnLeft()) {
+        if (LimelightContainer.isReefOnLeft()) {
             mirrorSetting = MirrorPresets.LEFT;
         } else {
             mirrorSetting = MirrorPresets.RIGHT;
diff --git a/src/main/java/frc/robot/util/LimelightContainer.java b/src/main/java/frc/robot/util/LimelightContainer.java
index 6311591..3e7b005 100644
--- a/src/main/java/frc/robot/util/LimelightContainer.java
+++ b/src/main/java/frc/robot/util/LimelightContainer.java
@@ -28,18 +28,22 @@ public class LimelightContainer {
   private static ArrayList<Limelight> limelights = new ArrayList<Limelight>();
 
 
-  public static boolean isOnLeft(){
-    int[] validIds = new int[6]; //change these to be all reef tags, for given alliance (check if they red or blue!!)
+  public static boolean isReefOnLeft(){
+    int[] validIds = new int[8]; 
     
     if(FieldConstants.getAlliance() == Alliance.Blue){
         for(int i = 0; i < 6; i++){
         validIds[i] = i+17;
         }
+        validIds[6] = 13;
+        validIds[7] = 12;
     }
     else if(FieldConstants.getAlliance() == Alliance.Red){
         for(int i = 0; i < 6; i++){
             validIds[i] = i +6;
         }
+        validIds[6] = 1;
+        validIds[7] = 2;
     }
 
     LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", validIds);
@@ -65,10 +69,9 @@ else if(LimelightHelpers.getTA("limelight-fr")>1){
 
         return false;
     }
-    else return false;
-    
+    else return false; //ideally this wouldn't be called, should add some error thing handling and such
   }
-
+ 
   public LimelightContainer(Limelight... limelights) {
     for (Limelight limelight : limelights) {
       LimelightContainer.limelights.add(limelight);

From 43335e30dbc5dada2ae27fc48965df6fcc51f7fb Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Thu, 13 Feb 2025 09:47:12 -0600
Subject: [PATCH 58/71] Finn's constants

---
 src/main/java/frc/robot/Constants.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index e8f436e..6daeae9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -216,7 +216,7 @@ public static class Roll {
           3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(10.0, 25.0));
+          new TrapezoidProfile.Constraints(7.5, 18.0));
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
@@ -243,7 +243,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(10.0, 35.0)); // Radians
+          new TrapezoidProfile.Constraints(7.5, 25.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 

From 26fc514ecdd31aef5dd6fdbf37d8a8a0321c831a Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Thu, 13 Feb 2025 16:22:01 -0600
Subject: [PATCH 59/71] Intaking and constants tweak

---
 src/main/java/frc/robot/Constants.java      |  4 ++--
 src/main/java/frc/robot/RobotContainer.java | 22 +++++++++++++++++----
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6daeae9..315fc2e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -216,7 +216,7 @@ public static class Roll {
           3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(7.5, 18.0));
+          new TrapezoidProfile.Constraints(18.0, 25.0));
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
@@ -243,7 +243,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(7.5, 25.0)); // Radians
+          new TrapezoidProfile.Constraints(10.0, 35.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3778db9..5a20ca8 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -50,13 +50,11 @@ public class RobotContainer {
 
     private static final Limelight LL_FR = new Limelight(LimelightType.LL4, "limelight-fr", true, true);
     private static final Limelight LL_FL = new Limelight(LimelightType.LL4, "limelight-fl", true, true);
-    private static final Limelight LL_BR  = new Limelight(LimelightType.LL4, "limelight-br", true, true);
+    private static final Limelight LL_BR = new Limelight(LimelightType.LL4, "limelight-br", true, true);
     private static final Limelight LL_BL = new Limelight(LimelightType.LL4, "limelight-bl", true, true);
 
     public static final LimelightContainer LLContainer = new LimelightContainer(LL_FR, LL_FL, LL_BR, LL_BL);
 
-
-
     private final CommandXboxController driverXbox = new CommandXboxController(
             ControllerConstants.DRIVER_CONTROLLER_PORT);
     private final CommandXboxController operatorXbox = new CommandXboxController(
@@ -137,6 +135,22 @@ private Command getGoToLockedPresetCommand() {
                 }));
     }
 
+    // Goes to a preset more quickly by moving pitch+pivot+roll at the same time,
+    // but can throw coral. Good for intaking
+    private Command getGoToLockedPresetFASTCommand() {
+        return new InstantCommand(() -> {
+            SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
+        }).andThen(new StowArm(coralSubsystem))
+                .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
+                .andThen(new ParallelCommandGroup(
+                        new MovePivot(coralSubsystem, currentLockedPresetSupplier),
+                        new MoveRoll(coralSubsystem, currentLockedPresetSupplier),
+                        new MovePitch(coralSubsystem, currentLockedPresetSupplier)))
+                .andThen(new InstantCommand(() -> {
+                    SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done");
+                }));
+    }
+
     private Command getStowCommand() {
         return new InstantCommand(() -> {
             lockCoralArmPreset(CoralPresets.STOW);
@@ -236,7 +250,7 @@ public boolean getAsBoolean() {
         }).whileTrue(new InstantCommand(() -> {
             System.out.println("Intaking");
             lockCoralArmPreset(CoralPresets.INTAKE);
-        }).andThen(getGoToLockedPresetCommand()).andThen(new IntakeCoralCommand(coralSubsystem))
+        }).andThen(getGoToLockedPresetFASTCommand()).andThen(new IntakeCoralCommand(coralSubsystem))
                 .andThen(getStowCommand())).whileFalse(getStowCommand());
 
         // purge coral

From 38442977691a6882ba121210ee7b3ca3bc2b071c Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Thu, 13 Feb 2025 21:43:02 -0600
Subject: [PATCH 60/71] Climber support

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../robot/subsystems/ClimberSubsystem.java    |  8 ++-
 .../frc/robot/subsystems/SwerveSubsystem.java | 67 ++++++++++---------
 4 files changed, 43 insertions(+), 36 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 315fc2e..ced03af 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -165,7 +165,7 @@ public static class Climber {
         0.0,
         new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
 
-    public static boolean DBG_DISABLED = true;
+    public static boolean DBG_DISABLED = false;
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5a20ca8..4dc87c3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -76,7 +76,7 @@ public class RobotContainer {
 
     private final AlgaeSubsystem algaeSubsystem = new AlgaeSubsystem();
 
-    private final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
+    private final ClimberSubsystem climberSubsystem = new ClimberSubsystem(operatorXbox.getHID());
 
     /*
      * The container for the robot. Contains subsystems, OI devices, and commands.
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index e45b262..038389b 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -7,6 +7,7 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.Climber;
@@ -14,8 +15,10 @@
 public class ClimberSubsystem extends SubsystemBase {
     private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
     private final SparkMaxConfig climberConfig = new SparkMaxConfig();
+    XboxController operator;
 
-    public ClimberSubsystem() {
+    public ClimberSubsystem(XboxController operator) {
+        this.operator = operator;
         climberConfig.idleMode(IdleMode.kBrake);
 
         climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -26,7 +29,8 @@ public ClimberSubsystem() {
     @Override
     public void periodic() {
         if (!Constants.Climber.DBG_DISABLED)
-            climberMotor.set(output);
+            climberMotor.set(operator.getLeftBumper() ? (operator.getLeftY() * 0.4) : 0.0);
+
     }
 
     // TODO: limit the output somehow
diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index 45338f4..08c1527 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -111,7 +111,8 @@ public SwerveSubsystem() {
                 this::getPose, // Robot pose supplier
                 this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose)
                 this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+                (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT
+                                                                       // RELATIVE ChassisSpeeds
                 PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER,
                 PathPlannerConstants.ROBOT_CONFIG,
                 () -> {
@@ -143,22 +144,23 @@ public SwerveSubsystem() {
     @Override
     public void periodic() {
 
-        if (!isalliancereset && DriverStation.getAlliance().isPresent()) {
-            Translation2d pospose = getPose().getTranslation();
-            odometry.resetPosition(getRotation2d(), getModulePositions(),
-                    new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() == Alliance.Blue ? 0.0 : Math.PI)));
-            isalliancereset = true;
-        }
+        // if (!isalliancereset && DriverStation.getAlliance().isPresent()) {
+        // Translation2d pospose = getPose().getTranslation();
+        // odometry.resetPosition(getRotation2d(), getModulePositions(),
+        // new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() ==
+        // Alliance.Blue ? 0.0 : Math.PI)));
+        // isalliancereset = true;
+        // }
 
         // TODO: Test
         // WARNING: REMOVE IF USING TAG FOLLOW!!!
         // updateVisionOdometry();
 
-        if (DriverStation.isTeleopEnabled()) {
-            updateMegaTagOdometry();
-        } else {
-            updateVisionOdometry();
-        }
+        // if (DriverStation.isTeleopEnabled()) {
+        // updateMegaTagOdometry();
+        // } else {
+        // updateVisionOdometry();
+        // }
 
         odometry.update(getRotation2d(), getModulePositions());
         // if (DriverStation.getAlliance().isPresent()) {
@@ -327,27 +329,28 @@ public Command loadPath(String name) {
     public Command followPathCommand(String pathName) {
         try {
             PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);
-            
-            return new FollowPathCommand(
-                path,
-                this::getPose, // Robot pose supplier
-                this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
-                PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER,
-                PathPlannerConstants.ROBOT_CONFIG,
-                () -> {
-                    // Boolean supplier that controls when the path will be mirrored for the red
-                    // alliance
-                    // This will flip the path being followed to the red side of the field.
-                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
 
-                    var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
-                },
-                this // Reference to this subsystem to set requirements
+            return new FollowPathCommand(
+                    path,
+                    this::getPose, // Robot pose supplier
+                    this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                    (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given
+                                                                           // ROBOT RELATIVE ChassisSpeeds
+                    PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER,
+                    PathPlannerConstants.ROBOT_CONFIG,
+                    () -> {
+                        // Boolean supplier that controls when the path will be mirrored for the red
+                        // alliance
+                        // This will flip the path being followed to the red side of the field.
+                        // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                        var alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                    },
+                    this // Reference to this subsystem to set requirements
             );
         } catch (Exception exception) {
             return Commands.none();

From 623a88d4b160bc8cf372010a57769adccaafb0e2 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Thu, 13 Feb 2025 23:51:37 -0600
Subject: [PATCH 61/71] Rudimentary support for ultrasonic auto-mirroring.

---
 src/main/java/frc/robot/Constants.java            |  3 +++
 src/main/java/frc/robot/RobotContainer.java       |  2 ++
 .../robot/subsystems/coral/CoralSubsystem.java    | 15 ++++++++++++++-
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ced03af..a94dbe2 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -170,6 +170,9 @@ public static class Climber {
 
   // TODO: ##################### PLACEHOLDERS #####################
   public static class Coral {
+    public static int LEFT_ULTRASONIC_PORT = 0;
+    public static int RIGHT_ULTRASONIC_PORT = 1;
+
     public static class Pivot {
       public static final int MOTOR_PORT = 14;
       public static final int ENCODER_PORT = 28;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4dc87c3..c56846e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -123,6 +123,7 @@ public CoralPresets get() {
     // 4. Rotate wrist
     private Command getGoToLockedPresetCommand() {
         return new InstantCommand(() -> {
+            coralSubsystem.autoSetMirror();
             SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
         }).andThen(new StowArm(coralSubsystem))
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
@@ -139,6 +140,7 @@ private Command getGoToLockedPresetCommand() {
     // but can throw coral. Good for intaking
     private Command getGoToLockedPresetFASTCommand() {
         return new InstantCommand(() -> {
+            coralSubsystem.autoSetMirror();
             SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
         }).andThen(new StowArm(coralSubsystem))
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 5a24905..0c3fa66 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -2,6 +2,9 @@
 
 import java.util.function.BooleanSupplier;
 
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Ultrasonic;
 // import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -25,8 +28,13 @@ public class CoralSubsystem extends SubsystemBase {
     private final MechanismLigament2d pivotMechanism = elevatorMechanism.append(
             new MechanismLigament2d("Coral", Constants.Coral.Pivot.PhysicalConstants.JOINT_LENGTH_METERS, 0));
 
+    private final AnalogPotentiometer leftUltrasonic = new AnalogPotentiometer(Constants.Coral.LEFT_ULTRASONIC_PORT,
+            254.0, 0.0);
+    private final AnalogPotentiometer rightUltrasonic = new AnalogPotentiometer(Constants.Coral.RIGHT_ULTRASONIC_PORT,
+            254.0, 0.0);
+
     public enum CoralPresets {
-        LEVEL_1(1.0, 20.0, 0.0, 0.0), // TODO: Figure out level 1, TBD
+        LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(0.87), Units.radiansToDegrees(1.636)),
         LEVEL_2(0.247, 19.032, 90, 98.968),
         LEVEL_3(0.650, 19.032, 90, 98.968),
         LEVEL_4(1.342, 21.238, 90, 111.762),
@@ -230,6 +238,11 @@ public void mirrorArm(MirrorPresets preset) {
         mirrorSetting = preset;
     }
 
+    public void autoSetMirror() {
+        this.mirrorSetting = (this.leftUltrasonic.get() < this.rightUltrasonic.get()) ? MirrorPresets.LEFT
+                : MirrorPresets.RIGHT;
+    }
+
     public void setCoralIntakePreset(CoralIntakePresets preset) {
         SmartDashboard.putString("Coral Intake Preset", preset.toString());
         if (preset != currentIntakePreset) {

From 77e8c5fda0fef6c4d36804d742d3398397b21840 Mon Sep 17 00:00:00 2001
From: voidReq <jhanstedtas@gmail.com>
Date: Fri, 14 Feb 2025 19:05:09 -0600
Subject: [PATCH 62/71] Removed (commented) limelight/odometry

---
 .../frc/robot/subsystems/SwerveSubsystem.java  | 18 +-----------------
 1 file changed, 1 insertion(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
index 08c1527..51a9870 100644
--- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -144,23 +144,7 @@ public SwerveSubsystem() {
     @Override
     public void periodic() {
 
-        // if (!isalliancereset && DriverStation.getAlliance().isPresent()) {
-        // Translation2d pospose = getPose().getTranslation();
-        // odometry.resetPosition(getRotation2d(), getModulePositions(),
-        // new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() ==
-        // Alliance.Blue ? 0.0 : Math.PI)));
-        // isalliancereset = true;
-        // }
-
-        // TODO: Test
-        // WARNING: REMOVE IF USING TAG FOLLOW!!!
-        // updateVisionOdometry();
-
-        // if (DriverStation.isTeleopEnabled()) {
-        // updateMegaTagOdometry();
-        // } else {
-        // updateVisionOdometry();
-        // }
+        
 
         odometry.update(getRotation2d(), getModulePositions());
         // if (DriverStation.getAlliance().isPresent()) {

From 7d322684233c478ffa0b7eb06ceeac8f19922c8b Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sat, 15 Feb 2025 11:44:30 -0600
Subject: [PATCH 63/71] After filler matches.

---
 src/main/java/frc/robot/Constants.java        |   4 +-
 src/main/java/frc/robot/RobotContainer.java   |  14 +-
 .../robot/subsystems/ClimberSubsystem.java    |   2 +-
 .../frc/robot/subsystems/coral/CoralArm.java  | 716 +++++++++---------
 .../subsystems/coral/CoralSubsystem.java      |  27 +-
 5 files changed, 388 insertions(+), 375 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a94dbe2..ec5a5ac 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -219,7 +219,7 @@ public static class Roll {
           3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(18.0, 25.0));
+          new TrapezoidProfile.Constraints(12.0, 20.0));
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
@@ -246,7 +246,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(10.0, 35.0)); // Radians
+          new TrapezoidProfile.Constraints(13.0, 35.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c56846e..975b75f 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -22,6 +22,7 @@
 import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+import frc.robot.subsystems.coral.CoralSubsystem.MirrorPresets;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
@@ -219,6 +220,13 @@ private void configureBindings() {
                     isScoring = false;
                 })));
 
+        // operatorXbox.povLeft().onTrue(new InstantCommand(() -> {
+        // coralSubsystem.mirrorArm(MirrorPresets.LEFT);
+        // }));
+        // operatorXbox.povRight().onTrue(new InstantCommand(() -> {
+        // coralSubsystem.mirrorArm(MirrorPresets.LEFT);
+        // }));
+
         // wrist adjustment
         // Hold for now, until everything else is working
         // operatorXbox.rightStick().and(new BooleanSupplier() {
@@ -264,9 +272,9 @@ public boolean getAsBoolean() {
             operatorXbox.setRumble(RumbleType.kBothRumble, 0.0);
         })));
 
-        operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
-            coralSubsystem.mirrorArm();
-        }));
+        // operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
+        // coralSubsystem.mirrorArm();
+        // }));
 
         /*
          * driver
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 038389b..94092a8 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -29,7 +29,7 @@ public ClimberSubsystem(XboxController operator) {
     @Override
     public void periodic() {
         if (!Constants.Climber.DBG_DISABLED)
-            climberMotor.set(operator.getLeftBumper() ? (operator.getLeftY() * 0.4) : 0.0);
+            climberMotor.set(operator.getLeftBumper() ? (operator.getLeftY()) : 0.0);
 
     }
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index d9b7faf..bfcb6a7 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -32,364 +32,366 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase {
-    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-    // sim motors
-    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
-
-    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-    private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
-    private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
-    // sim encoders
-    private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
-    private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
-    private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
-
-    // physics simulations
-    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-            Coral.Pivot.PhysicalConstants.MOTOR,
-            Coral.Pivot.PhysicalConstants.NET_REDUCTION,
-            Coral.Pivot.PhysicalConstants.MOI,
-            Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-            0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
-            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
-    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-            Coral.Roll.PhysicalConstants.MOTOR,
-            Coral.Roll.PhysicalConstants.NET_REDUCTION,
-            Coral.Roll.PhysicalConstants.MOI,
-            Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-            Coral.Roll.MAXIMUM_ANGLE * -1,
-            Coral.Roll.MAXIMUM_ANGLE,
-            false,
-            0);
-    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-            Coral.Pitch.PhysicalConstants.MOTOR,
-            Coral.Pitch.PhysicalConstants.NET_REDUCTION,
-            Coral.Pitch.PhysicalConstants.MOI,
-            Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-            Coral.Pitch.MAXIMUM_ANGLE * -1,
-            Coral.Pitch.MAXIMUM_ANGLE,
-            false,
-            0);
-
-    private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
-    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-    private final ProfiledPIDController rollPID = Coral.Roll.PID;
-    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-
-    private double pivotGoal = 0.0; // Rads
-    private double rollGoal = 0.0; // Rads
-    private double pitchGoal = 0.0; // Rads
-
-    LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
-    LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
-
-    public CoralArm() {
-        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
-
-        pivotMotor.configure(
-                pivotConfig.idleMode(IdleMode.kBrake).apply(
-                        new EncoderConfig()
-                                .positionConversionFactor(
-                                        (1.0 * 2.0 * Math.PI)
-                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
-                                .velocityConversionFactor(
-                                        (60.0 * 2.0 * Math.PI)
-                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
-                                                                                                           // ->
-                // Rotations &
-                // Seconds
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        rollMotor.configure(
-                rollConfig.idleMode(IdleMode.kBrake)
-                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
-                        .apply(new SparkMaxConfig().inverted(
-                                Constants.Coral.Pitch.MOTOR_INVERTED)),
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        pitchMotor.configure(pitchConfig
-                .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
-                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
-
-        /*
-         * .apply(new EncoderConfig()
-         * .positionConversionFactor(
-         * (1.0 * 2.0 * Math.PI)
-         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
-         * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
-         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
-         */
-        // Not sure how to get
-
-        // Disabling this for now because we don't want the wrist wire bundle to explode
-        // rollPID.enableContinuousInput(-180, 180);
-
-        // TODO: Are these reasonable? who knows.
-        pivotPID.setTolerance(Units.degreesToRadians(3.0));
-        rollPID.setTolerance(Units.degreesToRadians(3.0));
-        pitchPID.setTolerance(Units.degreesToRadians(3.0));
-
-        // Warm up the filters
-        for (int i = 0; i < 3; ++i) {
-            readPitchEncoderPosition();
-            readRollEncoderPosition();
+        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+        // sim motors
+        private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
+
+        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
+        // sim encoders
+        private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
+        private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
+        private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
+
+        // physics simulations
+        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+                        Coral.Pivot.PhysicalConstants.MOTOR,
+                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pivot.PhysicalConstants.MOI,
+                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+                        0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
+                        0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
+        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+                        Coral.Roll.PhysicalConstants.MOTOR,
+                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
+                        Coral.Roll.PhysicalConstants.MOI,
+                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+                        Coral.Roll.MAXIMUM_ANGLE * -1,
+                        Coral.Roll.MAXIMUM_ANGLE,
+                        false,
+                        0);
+        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+                        Coral.Pitch.PhysicalConstants.MOTOR,
+                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pitch.PhysicalConstants.MOI,
+                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+                        Coral.Pitch.MAXIMUM_ANGLE * -1,
+                        Coral.Pitch.MAXIMUM_ANGLE,
+                        false,
+                        0);
+
+        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+        private final ProfiledPIDController rollPID = Coral.Roll.PID;
+        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+        private double pivotGoal = 0.0; // Rads
+        private double rollGoal = 0.0; // Rads
+        private double pitchGoal = 0.0; // Rads
+
+        LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
+        LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
+
+        public CoralArm() {
+                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
+
+                pivotMotor.configure(
+                                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                                                new EncoderConfig()
+                                                                .positionConversionFactor(
+                                                                                (1.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                                                .velocityConversionFactor(
+                                                                                (60.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                                                                           // ->
+                                // Rotations &
+                                // Seconds
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                rollMotor.configure(
+                                rollConfig.idleMode(IdleMode.kBrake)
+                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
+                                                .apply(new SparkMaxConfig().inverted(
+                                                                Constants.Coral.Pitch.MOTOR_INVERTED)),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                pitchMotor.configure(pitchConfig
+                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
+                                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+                pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
+
+                /*
+                 * .apply(new EncoderConfig()
+                 * .positionConversionFactor(
+                 * (1.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+                 */
+                // Not sure how to get
+
+                // Disabling this for now because we don't want the wrist wire bundle to explode
+                // rollPID.enableContinuousInput(-180, 180);
+
+                // TODO: Are these reasonable? who knows.
+                pivotPID.setTolerance(Units.degreesToRadians(3.0));
+                rollPID.setTolerance(Units.degreesToRadians(3.0));
+                pitchPID.setTolerance(Units.degreesToRadians(3.0));
+
+                // Warm up the filters
+                for (int i = 0; i < 3; ++i) {
+                        readPitchEncoderPosition();
+                        readRollEncoderPosition();
+                }
         }
-    }
-
-    @Override
-    public void periodic() {
-        // if entering the frame border
-        // && wrist is at neutral
-        if ((pivotPID.getGoal().position != pivotGoal)
-                && (rollPID.atGoal() && pitchPID.atGoal())) {
-            // allow arm to enter frame border
-            pivotPID.setGoal(pivotGoal);
-        } else if ( // if exiting the frame border && has exited the frame border
-        ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
-            // set roll and pitch back to their goals
-            rollPID.setGoal(rollGoal);
-            pitchPID.setGoal(pitchGoal);
+
+        @Override
+        public void periodic() {
+                // if entering the frame border
+                // && wrist is at neutral
+                if ((pivotPID.getGoal().position != pivotGoal)
+                                && (rollPID.atGoal() && pitchPID.atGoal())) {
+                        // allow arm to enter frame border
+                        pivotPID.setGoal(pivotGoal);
+                } else if ( // if exiting the frame border && has exited the frame border
+                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
+                        // set roll and pitch back to their goals
+                        rollPID.setGoal(rollGoal);
+                        pitchPID.setGoal(pitchGoal);
+                }
+
+                /*
+                 * run the motors
+                 */
+
+                double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
+                                : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
+                // double pivotPosition = readPivotEncoderPosition();
+
+                double pivotFFout = pivotFeedforward.calculate(
+                                Math.PI * 0.5 + pivotPosition,
+                                pivotPID.getSetpoint().velocity);
+                double pivotPIDout = pivotPID.calculate(pivotPosition);
+                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+                if (!Constants.Coral.Pivot.DBG_DISABLED)
+                        pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+                if (Robot.isSimulation())
+                        simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
+
+                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+                double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
+                if (!Constants.Coral.Roll.DBG_DISABLED)
+                        rollMotor.setVoltage(rollPIDout + rollFFout);
+                if (Robot.isSimulation())
+                        simRollMotor.setAppliedOutput(rollPIDout);
+
+                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+                double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
+                if (!Constants.Coral.Pitch.DBG_DISABLED)
+                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+                if (Robot.isSimulation())
+                        simPitchMotor.setAppliedOutput(pitchPIDout);
+        }
+
+        @Override
+        public void simulationPeriodic() {
+                // update physics
+                simPivotPhysics
+                                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                simRollPhysics
+                                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                simPitchPhysics
+                                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                                * RoboRioSim.getVInVoltage());
+
+                simPivotPhysics.update(0.02);
+                simRollPhysics.update(0.02);
+                simPitchPhysics.update(0.02);
+
+                // update sim objects
+                simPivotMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simRollMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simPitchMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+
+                simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
+                                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
+                simRollEncoder.iterate(
+                                simRollPhysics.getVelocityRadPerSec(),
+                                0.02);
+                simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
+                                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
+                simPitchEncoder.iterate(
+                                simPitchPhysics.getVelocityRadPerSec(),
+                                0.02);
+
+                simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
+                simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
+        }
+
+        // this should be relative to straight upwards.
+        // i.e. 0 should be straight vertical,
+        // 20 would be to the right, -20 to the left
+        public void setPivotGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pivotGoal = MathUtil.clamp(goalRads,
+                                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
+                // if passing through the frame border
+                /*
+                 * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+                 * && (Math.abs(this.getPivotPositionDegrees()) <
+                 * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+                 * // set the pivot to stop at the frame border to allow the wrist to move
+                 * // neutral
+                 * pivotPID.setGoal(
+                 * Coral.Pivot.FRAME_BORDER_ANGLE
+                 * (pivotGoal < 0 ? -1 : 1));
+                 * rollPID.setGoal(0);
+                 * pitchPID.setGoal(0);
+                 * } else {
+                 * pivotPID.setGoal(pivotGoal);
+                 * }
+                 */
+
+                pivotPID.setGoal(pivotGoal);
+        }
+
+        // similarly, this ranges from -180 to 180
+        public void setRollGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+                /*
+                 * // if outside of frame border
+                 * if (Math.abs(this.getPivotPositionDegrees()) >
+                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
+                 * // start moving to goal
+                 * rollPID.setGoal(rollGoal);
+                 * }
+                 */
+                rollPID.setGoal(rollGoal);
+        }
+
+        public void setPitchGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+                /*
+                 * // if outside the frame border
+                 * if (Math.abs(this.getPivotPositionDegrees()) >
+                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
+                 * // start moving to goal
+                 * pitchPID.setGoal(pitchGoal);
+                 * }
+                 */
+                pitchPID.setGoal(pitchGoal);
+        }
+
+        public double getPivotGoalDegrees() {
+                return Units.radiansToDegrees(pivotGoal);
+        }
+
+        public double getRollGoalDegrees() {
+                return Units.radiansToDegrees(rollGoal);
+        }
+
+        public double getPitchGoalDegrees() {
+                return Units.radiansToDegrees(pitchGoal);
+        }
+
+        public double getPivotPositionDegrees() {
+                return Units.radiansToDegrees(readPivotEncoderPosition());
+        }
+
+        public double getRollPositionDegrees() {
+                return Units.radiansToDegrees(readRollEncoderPosition());
         }
 
-        /*
-         * run the motors
-         */
-
-        double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
-                : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
-        double pivotFFout = pivotFeedforward.calculate(
-                Math.PI * 0.5 + pivotPosition,
-                pivotPID.getSetpoint().velocity);
-        double pivotPIDout = pivotPID.calculate(pivotPosition);
-        SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
-        SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
-        SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
-        SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
-        SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-        if (!Constants.Coral.Pivot.DBG_DISABLED)
-            pivotMotor.setVoltage(pivotPIDout + pivotFFout);
-        if (Robot.isSimulation())
-            simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
-
-        double rollPIDout = rollPID.calculate(readRollEncoderPosition());
-        double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
-        SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-        SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
-        SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
-        SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
-        if (!Constants.Coral.Roll.DBG_DISABLED)
-            rollMotor.setVoltage(rollPIDout + rollFFout);
-        if (Robot.isSimulation())
-            simRollMotor.setAppliedOutput(rollPIDout);
-
-        double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
-        double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
-        SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-        SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
-        SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
-        SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
-        if (!Constants.Coral.Pitch.DBG_DISABLED)
-            pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-        if (Robot.isSimulation())
-            simPitchMotor.setAppliedOutput(pitchPIDout);
-    }
-
-    @Override
-    public void simulationPeriodic() {
-        // update physics
-        simPivotPhysics
-                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        simRollPhysics
-                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        simPitchPhysics
-                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
-                * RoboRioSim.getVInVoltage());
-
-        simPivotPhysics.update(0.02);
-        simRollPhysics.update(0.02);
-        simPitchPhysics.update(0.02);
-
-        // update sim objects
-        simPivotMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-        simRollMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-        simPitchMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-
-        simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
-                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
-        simRollEncoder.iterate(
-                simRollPhysics.getVelocityRadPerSec(),
-                0.02);
-        simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
-                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
-        simPitchEncoder.iterate(
-                simPitchPhysics.getVelocityRadPerSec(),
-                0.02);
-
-        simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
-        simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
-    }
-
-    // this should be relative to straight upwards.
-    // i.e. 0 should be straight vertical,
-    // 20 would be to the right, -20 to the left
-    public void setPivotGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        pivotGoal = MathUtil.clamp(goalRads,
-                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
-        // if passing through the frame border
-        /*
-         * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
-         * && (Math.abs(this.getPivotPositionDegrees()) <
-         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
-         * // set the pivot to stop at the frame border to allow the wrist to move
-         * // neutral
-         * pivotPID.setGoal(
-         * Coral.Pivot.FRAME_BORDER_ANGLE
-         * (pivotGoal < 0 ? -1 : 1));
-         * rollPID.setGoal(0);
-         * pitchPID.setGoal(0);
-         * } else {
-         * pivotPID.setGoal(pivotGoal);
-         * }
-         */
-
-        pivotPID.setGoal(pivotGoal);
-    }
-
-    // similarly, this ranges from -180 to 180
-    public void setRollGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-        /*
-         * // if outside of frame border
-         * if (Math.abs(this.getPivotPositionDegrees()) >
-         * Coral.Pivot.FRAME_BORDER_ANGLE) {
-         * // start moving to goal
-         * rollPID.setGoal(rollGoal);
-         * }
-         */
-        rollPID.setGoal(rollGoal);
-    }
-
-    public void setPitchGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-        /*
-         * // if outside the frame border
-         * if (Math.abs(this.getPivotPositionDegrees()) >
-         * Coral.Pivot.FRAME_BORDER_ANGLE) {
-         * // start moving to goal
-         * pitchPID.setGoal(pitchGoal);
-         * }
-         */
-        pitchPID.setGoal(pitchGoal);
-    }
-
-    public double getPivotGoalDegrees() {
-        return Units.radiansToDegrees(pivotGoal);
-    }
-
-    public double getRollGoalDegrees() {
-        return Units.radiansToDegrees(rollGoal);
-    }
-
-    public double getPitchGoalDegrees() {
-        return Units.radiansToDegrees(pitchGoal);
-    }
-
-    public double getPivotPositionDegrees() {
-        return Units.radiansToDegrees(readPivotEncoderPosition());
-    }
-
-    public double getRollPositionDegrees() {
-        return Units.radiansToDegrees(readRollEncoderPosition());
-    }
-
-    public double getPitchPositionDegrees() {
-        return Units.radiansToDegrees(readPitchEncoderPosition());
-    }
-
-    public boolean isPitchInPosition() {
-        return pitchPID.atGoal();
-    }
-
-    public boolean isRollInPosition() {
-        return rollPID.atGoal();
-    }
-
-    public boolean isPivotInPosition() {
-        return pivotPID.atGoal();
-    }
-
-    public double readPitchEncoderPosition() {
-        return MathUtil
-                .angleModulus(((pitchEncFilter
-                        .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
-                                : pitchEncoder.getPosition())
-                        + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
-                        / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readRollEncoderPosition() {
-        return MathUtil
-                .angleModulus(((rollEncFilter
-                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
-                                : rollEncoder.getPosition())
-                        + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readPivotEncoderPosition() {
-        return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
-                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-    }
-
-    public boolean isPitchSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
-        // this.pitchPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
-    }
-
-    public boolean isPivotSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
-        // this.pivotPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
-    }
-
-    public boolean isRollSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.rollPID.getSetpoint().position,
-        // this.rollPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
-    }
+        public double getPitchPositionDegrees() {
+                return Units.radiansToDegrees(readPitchEncoderPosition());
+        }
+
+        public boolean isPitchInPosition() {
+                return pitchPID.atGoal();
+        }
+
+        public boolean isRollInPosition() {
+                return rollPID.atGoal();
+        }
+
+        public boolean isPivotInPosition() {
+                return pivotPID.atGoal();
+        }
+
+        public double readPitchEncoderPosition() {
+                return MathUtil
+                                .angleModulus(((pitchEncFilter
+                                                .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
+                                                                : pitchEncoder.getPosition())
+                                                + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
+                                                / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readRollEncoderPosition() {
+                return MathUtil
+                                .angleModulus(((rollEncFilter
+                                                .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
+                                                                : rollEncoder.getPosition())
+                                                + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readPivotEncoderPosition() {
+                return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
+                                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+        }
+
+        public boolean isPitchSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
+                // this.pitchPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
+        }
+
+        public boolean isPivotSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
+                // this.pivotPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
+        }
+
+        public boolean isRollSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.rollPID.getSetpoint().position,
+                // this.rollPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
+        }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 0c3fa66..92dfc84 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -34,11 +34,11 @@ public class CoralSubsystem extends SubsystemBase {
             254.0, 0.0);
 
     public enum CoralPresets {
-        LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(0.87), Units.radiansToDegrees(1.636)),
-        LEVEL_2(0.247, 19.032, 90, 98.968),
-        LEVEL_3(0.650, 19.032, 90, 98.968),
-        LEVEL_4(1.342, 21.238, 90, 111.762),
-        INTAKE(0.05, 18.0, 90, 35.0),
+        LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(1.0), Units.radiansToDegrees(1.636)),
+        LEVEL_2(0.247, 19.032, 90, 96.968),
+        LEVEL_3(0.650, 19.032, 90, 96.968),
+        LEVEL_4(1.342, 21.238, 90, 108.762),
+        INTAKE(0.05, 18.0, 90, 30.0),
         STOW(0.05, 0.0, 0.0, 0.0),
 
         CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
@@ -93,6 +93,9 @@ private CoralIntakePresets(double intakePercentage) {
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber("Ultra Left", this.leftUltrasonic.get());
+        SmartDashboard.putNumber("Ultra Right", this.rightUltrasonic.get());
+
         // i have no idea what any of the getPositions output
         elevatorMechanism
                 .setLength(elevator.getPosition() + Constants.Elevator.PhysicalParameters.CORAL_PIVOT_VERTICAL_OFFSET);
@@ -226,13 +229,13 @@ public void setCustomPitchDegrees(double pitchAngle) {
         arm.setPitchGoalDegrees(pitchAngle);
     }
 
-    public void mirrorArm() {
-        if (LimelightContainer.isReefOnLeft()) {
-            mirrorSetting = MirrorPresets.LEFT;
-        } else {
-            mirrorSetting = MirrorPresets.RIGHT;
-        }
-    }
+    // public void mirrorArm() {
+    // if (LimelightContainer.isReefOnLeft()) {
+    // mirrorSetting = MirrorPresets.LEFT;
+    // } else {
+    // mirrorSetting = MirrorPresets.RIGHT;
+    // }
+    // }
 
     public void mirrorArm(MirrorPresets preset) {
         mirrorSetting = preset;

From 71649f4ccda91262ce4ecf34e6a15dbafcef6aaf Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sat, 15 Feb 2025 18:08:51 -0600
Subject: [PATCH 64/71] Tweaks at the end of week 0

---
 src/main/deploy/pathplanner/navgrid.json      |  1 +
 src/main/deploy/pathplanner/settings.json     | 32 +++++++++++++++++++
 src/main/java/frc/robot/Constants.java        | 15 ++++-----
 src/main/java/frc/robot/RobotContainer.java   | 11 ++++---
 .../frc/robot/subsystems/SwerveModule.java    | 10 +++---
 .../frc/robot/subsystems/coral/CoralArm.java  | 10 +++---
 .../subsystems/coral/CoralSubsystem.java      |  6 ++--
 7 files changed, 60 insertions(+), 25 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 src/main/deploy/pathplanner/settings.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..e54abce
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 0.5,
+  "defaultMaxAccel": 0.5,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ec5a5ac..362d3fe 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -126,7 +126,7 @@ public static class DriveConstants {
     public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec
 
     // TODO: ############## REPLACE PLACEHOLDERS ##############
-    public static final double MAX_MODULE_CURRENT = 10;
+    public static final double MAX_MODULE_CURRENT = 30;
 
     public static final double TRACK_WIDTH = Units.inchesToMeters(19.75);
     public static final double WHEEL_BASE = Units.inchesToMeters(19.75);
@@ -219,7 +219,7 @@ public static class Roll {
           3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 20.0));
+          new TrapezoidProfile.Constraints(20.0, 20.0));
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
@@ -246,7 +246,7 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(13.0, 35.0)); // Radians
+          new TrapezoidProfile.Constraints(12.0, 30.0)); // Radians
 
       public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
 
@@ -347,10 +347,10 @@ public static class PID {
       public static double kP = 28.0;
       public static double kI = 0.0;
       public static double kD = 0.01;
-      public static double MAX_VELOCITY = 2.75;
+      public static double MAX_VELOCITY = 2.80;
       // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if
       // the elevator can make or exceed this
-      public static double MAX_ACCELERATION = 10.0;
+      public static double MAX_ACCELERATION = 15.0;
     }
 
     // TODO: PAD THE ELEVATOR!!!!!!!
@@ -408,10 +408,7 @@ public static final class PathPlannerConstants {
             DCMotor.getKrakenX60(1),
             DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ##############
             1),
-        new Translation2d(-0.5, 0.5),
-        new Translation2d(0.5, 0.5),
-        new Translation2d(-0.5, -0.5),
-        new Translation2d(0.5, -0.5));
+        DriveConstants.KINEMATICS.getModules());
   }
 
   public static final class PoseConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 975b75f..06dcb3e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -124,14 +124,15 @@ public CoralPresets get() {
     // 4. Rotate wrist
     private Command getGoToLockedPresetCommand() {
         return new InstantCommand(() -> {
-            coralSubsystem.autoSetMirror();
             SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
         }).andThen(new StowArm(coralSubsystem))
                 .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
-                .andThen(new ParallelCommandGroup(
+                .andThen((new InstantCommand(() -> {
+                    coralSubsystem.autoSetMirror();
+                })).andThen(new ParallelCommandGroup(
                         new MovePivot(coralSubsystem, currentLockedPresetSupplier),
                         new MoveRoll(coralSubsystem, currentLockedPresetSupplier)))
-                .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier))
+                        .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier)))
                 .andThen(new InstantCommand(() -> {
                     SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done");
                 }));
@@ -352,9 +353,9 @@ public boolean getAsBoolean() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
+        // return autoChooser.getSelected();
         // return new PathPlannerAuto("4-close-middle");
-
+        return new PrintCommand("Haha auto go brrr");
     }
 
     public SwerveSubsystem getSwerveSubsystem() {
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 7da0d6c..8c11f9b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -55,7 +55,8 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou
         driveMotor = new TalonFX(driveCanID);
         driveConfigurator = driveMotor.getConfigurator();
         driveConfig = new MotorOutputConfigs();
-        driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
+        driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive
+                : InvertedValue.CounterClockwise_Positive;
         driveConfig.NeutralMode = NeutralModeValue.Brake;
         driveConfigurator.apply(driveConfig);
 
@@ -109,12 +110,12 @@ public double getDrivePosition() {
             return driveEncSim;
         // return driveMotorEncoder.getPosition();
         // TODO: Do the conversion in the motor
-        return driveMotor.getPosition().getValueAsDouble()* SwerveModuleConstants.DRIVE_ROTATION_TO_METER;
+        return driveMotor.getPosition().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER;
     }
 
     public double getDriveVelocity() {
         // return driveMotorEncoder.getVelocity();
-        return driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER;
+        return driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_METERS_PER_MINUTE;
     }
 
     public double getSteerPosition() {
@@ -152,7 +153,8 @@ public SwerveModulePosition getModulePosition() {
     public void setModuleStateRaw(SwerveModuleState state) {
         state.optimize(new Rotation2d(getSteerPosition()));
         double drive_command = state.speedMetersPerSecond / DriveConstants.MAX_MODULE_VELOCITY;
-        // SmartDashboard.putNumber("Module " + Integer.toString(this.thisModuleNumber) + " Drive", drive_command);
+        // SmartDashboard.putNumber("Module " + Integer.toString(this.thisModuleNumber)
+        // + " Drive", drive_command);
         driveMotor.set(drive_command * (motor_inv ? -1.0 : 1.0));
 
         // This is stupid
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index bfcb6a7..0952bdc 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -166,9 +166,9 @@ public void periodic() {
                  * run the motors
                  */
 
-                double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
-                                : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
-                // double pivotPosition = readPivotEncoderPosition();
+                // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
+                // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
+                double pivotPosition = readPivotEncoderPosition();
 
                 double pivotFFout = pivotFeedforward.calculate(
                                 Math.PI * 0.5 + pivotPosition,
@@ -181,7 +181,9 @@ public void periodic() {
                 SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
                 SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
                 if (!Constants.Coral.Pivot.DBG_DISABLED)
-                        pivotMotor.setVoltage(pivotPIDout + pivotFFout);
+                        pivotMotor.setVoltage(
+                                        (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband(
+                                                        pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout);
                 if (Robot.isSimulation())
                         simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
 
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 92dfc84..98c38be 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -35,9 +35,9 @@ public class CoralSubsystem extends SubsystemBase {
 
     public enum CoralPresets {
         LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(1.0), Units.radiansToDegrees(1.636)),
-        LEVEL_2(0.247, 19.032, 90, 96.968),
-        LEVEL_3(0.650, 19.032, 90, 96.968),
-        LEVEL_4(1.342, 21.238, 90, 108.762),
+        LEVEL_2(0.247, 19.032, 90, 98.068),
+        LEVEL_3(0.650, 19.032, 90, 98.068),
+        LEVEL_4(1.342, 21.238, 90, 110.062),
         INTAKE(0.05, 18.0, 90, 30.0),
         STOW(0.05, 0.0, 0.0, 0.0),
 

From a0467f01e2cad16c255558fa00bfcc8ad9b421e0 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sat, 15 Feb 2025 20:30:33 -0600
Subject: [PATCH 65/71] Climber ready-ish for soft limits.

---
 src/main/java/frc/robot/Constants.java        |  4 +++
 .../robot/subsystems/ClimberSubsystem.java    | 31 ++++++++++++++++---
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 362d3fe..9be63d5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -166,6 +166,10 @@ public static class Climber {
         new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE));
 
     public static boolean DBG_DISABLED = false;
+    public static double GEAR_RATIO = 45.0;
+
+    public static double DEPLOY_SOFT_LIMIT = 0.0;
+    public static double CLIMB_SOFT_LIMIT = 0.0;
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 94092a8..0fd9ac2 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -1,39 +1,60 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SoftLimitConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.Climber;
 
 public class ClimberSubsystem extends SubsystemBase {
     private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMaxConfig climberConfig = new SparkMaxConfig();
+    private SparkMaxConfig climberConfig = new SparkMaxConfig();
+    private RelativeEncoder climberEncoder;
     XboxController operator;
 
     public ClimberSubsystem(XboxController operator) {
         this.operator = operator;
-        climberConfig.idleMode(IdleMode.kBrake);
+        climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake)
+                // .apply(new
+                // SoftLimitConfig().forwardSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT)
+                // .forwardSoftLimitEnabled(true).reverseSoftLimit(Constants.Climber.CLIMB_SOFT_LIMIT))
+                .apply(
+                        new EncoderConfig().positionConversionFactor(1.0 / Constants.Climber.GEAR_RATIO)
+                                .velocityConversionFactor(
+                                        1.0 / Constants.Climber.GEAR_RATIO)),
+                ResetMode.kResetSafeParameters,
+                PersistMode.kPersistParameters);
 
-        climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        climberEncoder = climberMotor.getEncoder();
     }
 
     private double output = 0.0;
+    private boolean deployed = false;
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition());
+        // if (climberEncoder.getPosition() > Constants.Climber.CLIMB_SOFT_LIMIT &&
+        // !deployed) {
+        // deployed = true;
+        // climberMotor.configure(climberConfig.apply(climberConfig.softLimit.reverseSoftLimitEnabled(true)),
+        // ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+        // }
+
         if (!Constants.Climber.DBG_DISABLED)
             climberMotor.set(operator.getLeftBumper() ? (operator.getLeftY()) : 0.0);
-
     }
 
-    // TODO: limit the output somehow
     public void setOutput(double output) {
         this.output = output;
     }

From 9033eab26e9ea7093a6d3c9ea2fdce82ecba556b Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sat, 15 Feb 2025 20:48:38 -0600
Subject: [PATCH 66/71] Smoother constants, PID tuning utilities.

---
 src/main/java/frc/robot/Constants.java        |  12 +-
 .../frc/robot/subsystems/coral/CoralArm.java  | 739 +++++++++---------
 2 files changed, 391 insertions(+), 360 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 9be63d5..602a943 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -177,6 +177,8 @@ public static class Coral {
     public static int LEFT_ULTRASONIC_PORT = 0;
     public static int RIGHT_ULTRASONIC_PORT = 1;
 
+    public static boolean DEBUG_PIDS = true;
+
     public static class Pivot {
       public static final int MOTOR_PORT = 14;
       public static final int ENCODER_PORT = 28;
@@ -223,8 +225,9 @@ public static class Roll {
           3.5,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(20.0, 20.0));
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.1);
+          new TrapezoidProfile.Constraints(12.5, 20.0));
+      // public static final SimpleMotorFeedforward FEEDFORWARD = new
+      // SimpleMotorFeedforward(0.0, 0.1);
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90);
 
@@ -250,9 +253,10 @@ public static class Pitch {
           5.0,
           0.0,
           0.0,
-          new TrapezoidProfile.Constraints(12.0, 30.0)); // Radians
+          new TrapezoidProfile.Constraints(10.0, 30.0)); // Radians
 
-      public static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(0.0, 0.45);
+      // public static final SimpleMotorFeedforward FEEDFORWARD = new
+      // SimpleMotorFeedforward(0.0, 0.45);
 
       public static class PhysicalConstants {
         public static DCMotor MOTOR = DCMotor.getNeo550(1);
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 0952bdc..20bc1d4 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -32,368 +32,395 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase {
-        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-        // sim motors
-        private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
-        private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
-        private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
-
-        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
-        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
-        // sim encoders
-        private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
-        private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
-        private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
-
-        // physics simulations
-        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-                        Coral.Pivot.PhysicalConstants.MOTOR,
-                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pivot.PhysicalConstants.MOI,
-                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-                        0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
-                        0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
-        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-                        Coral.Roll.PhysicalConstants.MOTOR,
-                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
-                        Coral.Roll.PhysicalConstants.MOI,
-                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Roll.MAXIMUM_ANGLE * -1,
-                        Coral.Roll.MAXIMUM_ANGLE,
-                        false,
-                        0);
-        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-                        Coral.Pitch.PhysicalConstants.MOTOR,
-                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
-                        Coral.Pitch.PhysicalConstants.MOI,
-                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-                        Coral.Pitch.MAXIMUM_ANGLE * -1,
-                        Coral.Pitch.MAXIMUM_ANGLE,
-                        false,
-                        0);
-
-        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
-        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-        private final ProfiledPIDController rollPID = Coral.Roll.PID;
-        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-
-        private double pivotGoal = 0.0; // Rads
-        private double rollGoal = 0.0; // Rads
-        private double pitchGoal = 0.0; // Rads
-
-        LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
-        LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
-
-        public CoralArm() {
-                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
-
-                pivotMotor.configure(
-                                pivotConfig.idleMode(IdleMode.kBrake).apply(
-                                                new EncoderConfig()
-                                                                .positionConversionFactor(
-                                                                                (1.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
-                                                                .velocityConversionFactor(
-                                                                                (60.0 * 2.0 * Math.PI)
-                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
-                                                                                                                                                           // ->
-                                // Rotations &
-                                // Seconds
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                rollMotor.configure(
-                                rollConfig.idleMode(IdleMode.kBrake)
-                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
-                                                .apply(new SparkMaxConfig().inverted(
-                                                                Constants.Coral.Pitch.MOTOR_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-                pitchMotor.configure(pitchConfig
-                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
-                                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
-                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-                pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
-
-                /*
-                 * .apply(new EncoderConfig()
-                 * .positionConversionFactor(
-                 * (1.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
-                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
-                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
-                 */
-                // Not sure how to get
-
-                // Disabling this for now because we don't want the wrist wire bundle to explode
-                // rollPID.enableContinuousInput(-180, 180);
-
-                // TODO: Are these reasonable? who knows.
-                pivotPID.setTolerance(Units.degreesToRadians(3.0));
-                rollPID.setTolerance(Units.degreesToRadians(3.0));
-                pitchPID.setTolerance(Units.degreesToRadians(3.0));
-
-                // Warm up the filters
-                for (int i = 0; i < 3; ++i) {
-                        readPitchEncoderPosition();
-                        readRollEncoderPosition();
-                }
+    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+    // sim motors
+    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
+
+    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+    private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+    private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
+    // sim encoders
+    private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
+    private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
+    private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
+
+    // physics simulations
+    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+            Coral.Pivot.PhysicalConstants.MOTOR,
+            Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+            Coral.Pivot.PhysicalConstants.MOI,
+            Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+            0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
+            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
+    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+            Coral.Roll.PhysicalConstants.MOTOR,
+            Coral.Roll.PhysicalConstants.NET_REDUCTION,
+            Coral.Roll.PhysicalConstants.MOI,
+            Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Roll.MAXIMUM_ANGLE * -1,
+            Coral.Roll.MAXIMUM_ANGLE,
+            false,
+            0);
+    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+            Coral.Pitch.PhysicalConstants.MOTOR,
+            Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+            Coral.Pitch.PhysicalConstants.MOI,
+            Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+            Coral.Pitch.MAXIMUM_ANGLE * -1,
+            Coral.Pitch.MAXIMUM_ANGLE,
+            false,
+            0);
+
+    private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+    private final ProfiledPIDController rollPID = Coral.Roll.PID;
+    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+    private double pivotGoal = 0.0; // Rads
+    private double rollGoal = 0.0; // Rads
+    private double pitchGoal = 0.0; // Rads
+
+    LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
+    LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
+
+    public CoralArm() {
+        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
+
+        pivotMotor.configure(
+                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                        new EncoderConfig()
+                                .positionConversionFactor(
+                                        (1.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                .velocityConversionFactor(
+                                        (60.0 * 2.0 * Math.PI)
+                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                           // ->
+                // Rotations &
+                // Seconds
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        rollMotor.configure(
+                rollConfig.idleMode(IdleMode.kBrake)
+                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
+                        .apply(new SparkMaxConfig().inverted(
+                                Constants.Coral.Pitch.MOTOR_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        pitchMotor.configure(pitchConfig
+                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
+                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
+                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
+
+        /*
+         * .apply(new EncoderConfig()
+         * .positionConversionFactor(
+         * (1.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+         * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+         */
+        // Not sure how to get
+
+        // Disabling this for now because we don't want the wrist wire bundle to explode
+        // rollPID.enableContinuousInput(-180, 180);
+
+        // TODO: Are these reasonable? who knows.
+        pivotPID.setTolerance(Units.degreesToRadians(3.0));
+        rollPID.setTolerance(Units.degreesToRadians(3.0));
+        pitchPID.setTolerance(Units.degreesToRadians(3.0));
+
+        if (Constants.Coral.DEBUG_PIDS) {
+            SmartDashboard.putNumber("Coral/Pivot/PID/P", pivotPID.getP());
+            SmartDashboard.putNumber("Coral/Pivot/PID/I", pivotPID.getI());
+            SmartDashboard.putNumber("Coral/Pivot/PID/D", pivotPID.getD());
+
+            SmartDashboard.putNumber("Coral/Roll/PID/P", rollPID.getP());
+            SmartDashboard.putNumber("Coral/Roll/PID/I", rollPID.getI());
+            SmartDashboard.putNumber("Coral/Roll/PID/D", rollPID.getD());
+
+            SmartDashboard.putNumber("Coral/Pitch/PID/P", pitchPID.getP());
+            SmartDashboard.putNumber("Coral/Pitch/PID/I", pitchPID.getI());
+            SmartDashboard.putNumber("Coral/Pitch/PID/D", pitchPID.getD());
         }
 
-        @Override
-        public void periodic() {
-                // if entering the frame border
-                // && wrist is at neutral
-                if ((pivotPID.getGoal().position != pivotGoal)
-                                && (rollPID.atGoal() && pitchPID.atGoal())) {
-                        // allow arm to enter frame border
-                        pivotPID.setGoal(pivotGoal);
-                } else if ( // if exiting the frame border && has exited the frame border
-                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                        // set roll and pitch back to their goals
-                        rollPID.setGoal(rollGoal);
-                        pitchPID.setGoal(pitchGoal);
-                }
-
-                /*
-                 * run the motors
-                 */
-
-                // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
-                // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
-                double pivotPosition = readPivotEncoderPosition();
-
-                double pivotFFout = pivotFeedforward.calculate(
-                                Math.PI * 0.5 + pivotPosition,
-                                pivotPID.getSetpoint().velocity);
-                double pivotPIDout = pivotPID.calculate(pivotPosition);
-                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
-                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
-                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
-                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-                if (!Constants.Coral.Pivot.DBG_DISABLED)
-                        pivotMotor.setVoltage(
-                                        (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband(
-                                                        pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout);
-                if (Robot.isSimulation())
-                        simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
-
-                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
-                double rollFFout = Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
-                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
-                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
-                if (!Constants.Coral.Roll.DBG_DISABLED)
-                        rollMotor.setVoltage(rollPIDout + rollFFout);
-                if (Robot.isSimulation())
-                        simRollMotor.setAppliedOutput(rollPIDout);
-
-                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
-                double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
-                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
-                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
-                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
-                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
-                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
-                if (!Constants.Coral.Pitch.DBG_DISABLED)
-                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-                if (Robot.isSimulation())
-                        simPitchMotor.setAppliedOutput(pitchPIDout);
+        // Warm up the filters
+        for (int i = 0; i < 3; ++i) {
+            readPitchEncoderPosition();
+            readRollEncoderPosition();
         }
-
-        @Override
-        public void simulationPeriodic() {
-                // update physics
-                simPivotPhysics
-                                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                                                -12.0, 12.0));
-                simRollPhysics
-                                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                                                -12.0, 12.0));
-                simPitchPhysics
-                                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                                                -12.0, 12.0));
-                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
-                                * RoboRioSim.getVInVoltage());
-
-                simPivotPhysics.update(0.02);
-                simRollPhysics.update(0.02);
-                simPitchPhysics.update(0.02);
-
-                // update sim objects
-                simPivotMotor.iterate(
-                                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
-                simRollMotor.iterate(
-                                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
-                simPitchMotor.iterate(
-                                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
-                                RoboRioSim.getVInVoltage(),
-                                0.02);
-
-                simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
-                                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
-                simRollEncoder.iterate(
-                                simRollPhysics.getVelocityRadPerSec(),
-                                0.02);
-                simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
-                                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
-                simPitchEncoder.iterate(
-                                simPitchPhysics.getVelocityRadPerSec(),
-                                0.02);
-
-                simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
-                simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
+    }
+
+    @Override
+    public void periodic() {
+        // if entering the frame border
+        // && wrist is at neutral
+        if ((pivotPID.getGoal().position != pivotGoal)
+                && (rollPID.atGoal() && pitchPID.atGoal())) {
+            // allow arm to enter frame border
+            pivotPID.setGoal(pivotGoal);
+        } else if ( // if exiting the frame border && has exited the frame border
+        ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
+            // set roll and pitch back to their goals
+            rollPID.setGoal(rollGoal);
+            pitchPID.setGoal(pitchGoal);
         }
 
-        // this should be relative to straight upwards.
-        // i.e. 0 should be straight vertical,
-        // 20 would be to the right, -20 to the left
-        public void setPivotGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pivotGoal = MathUtil.clamp(goalRads,
-                                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
-                // if passing through the frame border
-                /*
-                 * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
-                 * && (Math.abs(this.getPivotPositionDegrees()) <
-                 * Coral.Pivot.FRAME_BORDER_ANGLE)) {
-                 * // set the pivot to stop at the frame border to allow the wrist to move
-                 * // neutral
-                 * pivotPID.setGoal(
-                 * Coral.Pivot.FRAME_BORDER_ANGLE
-                 * (pivotGoal < 0 ? -1 : 1));
-                 * rollPID.setGoal(0);
-                 * pitchPID.setGoal(0);
-                 * } else {
-                 * pivotPID.setGoal(pivotGoal);
-                 * }
-                 */
-
-                pivotPID.setGoal(pivotGoal);
-        }
+        if (Constants.Coral.DEBUG_PIDS) {
+            pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP()));
+            pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI()));
+            pivotPID.setD(SmartDashboard.getNumber("Coral/Pivot/PID/D", pivotPID.getD()));
 
-        // similarly, this ranges from -180 to 180
-        public void setRollGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-                /*
-                 * // if outside of frame border
-                 * if (Math.abs(this.getPivotPositionDegrees()) >
-                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
-                 * // start moving to goal
-                 * rollPID.setGoal(rollGoal);
-                 * }
-                 */
-                rollPID.setGoal(rollGoal);
-        }
-
-        public void setPitchGoalDegrees(double goal) {
-                double goalRads = Units.degreesToRadians(goal);
-                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-                /*
-                 * // if outside the frame border
-                 * if (Math.abs(this.getPivotPositionDegrees()) >
-                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
-                 * // start moving to goal
-                 * pitchPID.setGoal(pitchGoal);
-                 * }
-                 */
-                pitchPID.setGoal(pitchGoal);
-        }
-
-        public double getPivotGoalDegrees() {
-                return Units.radiansToDegrees(pivotGoal);
-        }
-
-        public double getRollGoalDegrees() {
-                return Units.radiansToDegrees(rollGoal);
-        }
-
-        public double getPitchGoalDegrees() {
-                return Units.radiansToDegrees(pitchGoal);
-        }
-
-        public double getPivotPositionDegrees() {
-                return Units.radiansToDegrees(readPivotEncoderPosition());
-        }
-
-        public double getRollPositionDegrees() {
-                return Units.radiansToDegrees(readRollEncoderPosition());
-        }
-
-        public double getPitchPositionDegrees() {
-                return Units.radiansToDegrees(readPitchEncoderPosition());
-        }
-
-        public boolean isPitchInPosition() {
-                return pitchPID.atGoal();
-        }
-
-        public boolean isRollInPosition() {
-                return rollPID.atGoal();
-        }
-
-        public boolean isPivotInPosition() {
-                return pivotPID.atGoal();
-        }
-
-        public double readPitchEncoderPosition() {
-                return MathUtil
-                                .angleModulus(((pitchEncFilter
-                                                .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
-                                                                : pitchEncoder.getPosition())
-                                                + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
-                                                / 3.3)
-                                                * 2 * Math.PI);
-        }
-
-        public double readRollEncoderPosition() {
-                return MathUtil
-                                .angleModulus(((rollEncFilter
-                                                .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
-                                                                : rollEncoder.getPosition())
-                                                + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
-                                                * 2 * Math.PI);
-        }
-
-        public double readPivotEncoderPosition() {
-                return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
-                                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-        }
-
-        public boolean isPitchSupposedToBeInPosition() {
-                // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
-                // this.pitchPID.getGoal().position, 0.01);
-                return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
-        }
-
-        public boolean isPivotSupposedToBeInPosition() {
-                // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
-                // this.pivotPID.getGoal().position, 0.01);
-                return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
-        }
+            rollPID.setP(SmartDashboard.getNumber("Coral/Roll/PID/P", rollPID.getP()));
+            rollPID.setI(SmartDashboard.getNumber("Coral/Roll/PID/I", rollPID.getI()));
+            rollPID.setD(SmartDashboard.getNumber("Coral/Roll/PID/D", rollPID.getD()));
 
-        public boolean isRollSupposedToBeInPosition() {
-                // return MathUtil.isNear(this.rollPID.getSetpoint().position,
-                // this.rollPID.getGoal().position, 0.01);
-                return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
+            pitchPID.setP(SmartDashboard.getNumber("Coral/Pitch/PID/P", pitchPID.getP()));
+            pitchPID.setI(SmartDashboard.getNumber("Coral/Pitch/PID/I", pitchPID.getI()));
+            pitchPID.setD(SmartDashboard.getNumber("Coral/Pitch/PID/D", pitchPID.getD()));
         }
+        /*
+         * run the motors
+         */
+
+        // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
+        // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
+        double pivotPosition = readPivotEncoderPosition();
+
+        double pivotFFout = pivotFeedforward.calculate(
+                Math.PI * 0.5 + pivotPosition,
+                pivotPID.getSetpoint().velocity);
+        double pivotPIDout = pivotPID.calculate(pivotPosition);
+        SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+        SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+        SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+        SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+        if (!Constants.Coral.Pivot.DBG_DISABLED)
+            pivotMotor.setVoltage(
+                    (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband(
+                            pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout);
+        if (Robot.isSimulation())
+            simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
+
+        double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+        double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+        SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+        SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
+        if (!Constants.Coral.Roll.DBG_DISABLED)
+            rollMotor.setVoltage(rollPIDout + rollFFout);
+        if (Robot.isSimulation())
+            simRollMotor.setAppliedOutput(rollPIDout);
+
+        double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+        double pitchFFout = 0.0;// Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+        SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+        SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+        SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+        SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
+        if (!Constants.Coral.Pitch.DBG_DISABLED)
+            pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+        if (Robot.isSimulation())
+            simPitchMotor.setAppliedOutput(pitchPIDout);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // update physics
+        simPivotPhysics
+                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
+        simRollPhysics
+                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
+        simPitchPhysics
+                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                        -12.0, 12.0));
+        SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                * RoboRioSim.getVInVoltage());
+
+        simPivotPhysics.update(0.02);
+        simRollPhysics.update(0.02);
+        simPitchPhysics.update(0.02);
+
+        // update sim objects
+        simPivotMotor.iterate(
+                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+                RoboRioSim.getVInVoltage(),
+                0.02);
+        simRollMotor.iterate(
+                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
+                RoboRioSim.getVInVoltage(),
+                0.02);
+        simPitchMotor.iterate(
+                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
+                RoboRioSim.getVInVoltage(),
+                0.02);
+
+        simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
+                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
+        simRollEncoder.iterate(
+                simRollPhysics.getVelocityRadPerSec(),
+                0.02);
+        simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
+                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
+        simPitchEncoder.iterate(
+                simPitchPhysics.getVelocityRadPerSec(),
+                0.02);
+
+        simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
+        simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
+    }
+
+    // this should be relative to straight upwards.
+    // i.e. 0 should be straight vertical,
+    // 20 would be to the right, -20 to the left
+    public void setPivotGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pivotGoal = MathUtil.clamp(goalRads,
+                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
+        // if passing through the frame border
+        /*
+         * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+         * && (Math.abs(this.getPivotPositionDegrees()) <
+         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+         * // set the pivot to stop at the frame border to allow the wrist to move
+         * // neutral
+         * pivotPID.setGoal(
+         * Coral.Pivot.FRAME_BORDER_ANGLE
+         * (pivotGoal < 0 ? -1 : 1));
+         * rollPID.setGoal(0);
+         * pitchPID.setGoal(0);
+         * } else {
+         * pivotPID.setGoal(pivotGoal);
+         * }
+         */
+
+        pivotPID.setGoal(pivotGoal);
+    }
+
+    // similarly, this ranges from -180 to 180
+    public void setRollGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+        /*
+         * // if outside of frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * rollPID.setGoal(rollGoal);
+         * }
+         */
+        rollPID.setGoal(rollGoal);
+    }
+
+    public void setPitchGoalDegrees(double goal) {
+        double goalRads = Units.degreesToRadians(goal);
+        pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+        /*
+         * // if outside the frame border
+         * if (Math.abs(this.getPivotPositionDegrees()) >
+         * Coral.Pivot.FRAME_BORDER_ANGLE) {
+         * // start moving to goal
+         * pitchPID.setGoal(pitchGoal);
+         * }
+         */
+        pitchPID.setGoal(pitchGoal);
+    }
+
+    public double getPivotGoalDegrees() {
+        return Units.radiansToDegrees(pivotGoal);
+    }
+
+    public double getRollGoalDegrees() {
+        return Units.radiansToDegrees(rollGoal);
+    }
+
+    public double getPitchGoalDegrees() {
+        return Units.radiansToDegrees(pitchGoal);
+    }
+
+    public double getPivotPositionDegrees() {
+        return Units.radiansToDegrees(readPivotEncoderPosition());
+    }
+
+    public double getRollPositionDegrees() {
+        return Units.radiansToDegrees(readRollEncoderPosition());
+    }
+
+    public double getPitchPositionDegrees() {
+        return Units.radiansToDegrees(readPitchEncoderPosition());
+    }
+
+    public boolean isPitchInPosition() {
+        return pitchPID.atGoal();
+    }
+
+    public boolean isRollInPosition() {
+        return rollPID.atGoal();
+    }
+
+    public boolean isPivotInPosition() {
+        return pivotPID.atGoal();
+    }
+
+    public double readPitchEncoderPosition() {
+        return MathUtil
+                .angleModulus(((pitchEncFilter
+                        .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
+                                : pitchEncoder.getPosition())
+                        + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
+                        / 3.3)
+                        * 2 * Math.PI);
+    }
+
+    public double readRollEncoderPosition() {
+        return MathUtil
+                .angleModulus(((rollEncFilter
+                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
+                                : rollEncoder.getPosition())
+                        + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
+                        * 2 * Math.PI);
+    }
+
+    public double readPivotEncoderPosition() {
+        return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
+                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+    }
+
+    public boolean isPitchSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
+        // this.pitchPID.getGoal().position, 0.01);
+        return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
+    }
+
+    public boolean isPivotSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
+        // this.pivotPID.getGoal().position, 0.01);
+        return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
+    }
+
+    public boolean isRollSupposedToBeInPosition() {
+        // return MathUtil.isNear(this.rollPID.getSetpoint().position,
+        // this.rollPID.getGoal().position, 0.01);
+        return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
+    }
 }

From 1efbda92d590f350c66013e8b06e7f6ee93f10bd Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sat, 15 Feb 2025 22:19:48 -0600
Subject: [PATCH 67/71] Velocity display for debugging.

---
 .../frc/robot/subsystems/coral/CoralArm.java  | 44 ++++++++++++++++++-
 1 file changed, 42 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 20bc1d4..791d117 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -160,6 +160,9 @@ public CoralArm() {
         }
     }
 
+    double lastPitchReading = 0.0;
+    double lastRollReading = 0.0;
+
     @Override
     public void periodic() {
         // if entering the frame border
@@ -176,6 +179,13 @@ public void periodic() {
             pitchPID.setGoal(pitchGoal);
         }
 
+        double rollPosition = readRollEncoderPosition();
+        double pitchPosition = readPitchEncoderPosition();
+        double dRollDt = (rollPosition - lastRollReading) / 0.02;
+        double dPitchDt = (pitchPosition - lastPitchReading) / 0.02;
+        lastPitchReading = pitchPosition;
+        lastRollReading = rollPosition;
+
         if (Constants.Coral.DEBUG_PIDS) {
             pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP()));
             pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI()));
@@ -206,6 +216,8 @@ public void periodic() {
         SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
         SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
         SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pivot/velocity_target", pivotPID.getSetpoint().velocity);
+        SmartDashboard.putNumber("Coral/Pivot/velocity", readPivotEncoderVelocity());
         SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
         if (!Constants.Coral.Pivot.DBG_DISABLED)
             pivotMotor.setVoltage(
@@ -217,7 +229,9 @@ public void periodic() {
         double rollPIDout = rollPID.calculate(readRollEncoderPosition());
         double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
         SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+        SmartDashboard.putNumber("Coral/Roll/velocity", dRollDt);
         SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Roll/velocity_target", rollPID.getSetpoint().velocity);
         SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
         SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
         SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
@@ -230,7 +244,9 @@ public void periodic() {
         double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
         double pitchFFout = 0.0;// Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
         SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+        SmartDashboard.putNumber("Coral/Pitch/velocity", dPitchDt);
         SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+        SmartDashboard.putNumber("Coral/Pitch/velocity_target", pitchPID.getSetpoint().velocity);
         SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
         SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
         SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
@@ -277,12 +293,12 @@ public void simulationPeriodic() {
         simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
                 - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
         simRollEncoder.iterate(
-                simRollPhysics.getVelocityRadPerSec(),
+                Units.radiansToRotations(simRollPhysics.getVelocityRadPerSec()) * 3.3,
                 0.02);
         simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
                 - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
         simPitchEncoder.iterate(
-                simPitchPhysics.getVelocityRadPerSec(),
+                Units.radiansToRotations(simPitchPhysics.getVelocityRadPerSec()) * 3.3,
                 0.02);
 
         simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
@@ -391,6 +407,15 @@ public double readPitchEncoderPosition() {
                         * 2 * Math.PI);
     }
 
+    public double readPitchEncoderVelocity() {
+        return MathUtil
+                .angleModulus(((pitchEncFilter
+                        .calculate(Robot.isSimulation() ? simPitchEncoder.getVelocity()
+                                : pitchEncoder.getVelocity()))
+                        / 3.3)
+                        * 2 * Math.PI);
+    }
+
     public double readRollEncoderPosition() {
         return MathUtil
                 .angleModulus(((rollEncFilter
@@ -400,12 +425,27 @@ public double readRollEncoderPosition() {
                         * 2 * Math.PI);
     }
 
+    public double readRollEncoderVelocity() {
+        return MathUtil
+                .angleModulus(((rollEncFilter
+                        .calculate(Robot.isSimulation() ? simRollEncoder.getVelocity()
+                                : rollEncoder.getVelocity()))
+                        / 3.3)
+                        * 2 * Math.PI);
+    }
+
     public double readPivotEncoderPosition() {
         return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
                 : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
                         * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
     }
 
+    public double readPivotEncoderVelocity() {
+        return Robot.isSimulation() ? (simPivotPhysics.getVelocityRadPerSec())
+                : Units.rotationsToRadians((pivotEncoder.getVelocity().getValueAsDouble()
+                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+    }
+
     public boolean isPitchSupposedToBeInPosition() {
         // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
         // this.pitchPID.getGoal().position, 0.01);

From 882f239147eadb517ed61349c1afbb95c91cec72 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 16 Feb 2025 12:28:29 -0600
Subject: [PATCH 68/71] V2 sequncnign????

---
 src/main/java/frc/robot/Constants.java        |   3 +-
 src/main/java/frc/robot/RobotContainer.java   |  25 +-
 .../commands/coral/motion/MovePivot.java      |   2 +-
 .../coral/motion/WaitArmClearance.java        |  28 +
 .../coral/motion/WaitElevatorApproach.java    |  28 +
 .../coral/motion/WaitRollFinished.java        |  27 +
 .../frc/robot/subsystems/coral/CoralArm.java  | 863 +++++++++---------
 .../subsystems/coral/CoralSubsystem.java      |  24 +-
 8 files changed, 570 insertions(+), 430 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java
 create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 602a943..6b25762 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -187,7 +187,7 @@ public static class Pivot {
       public static final boolean ENCODER_INVERTED = false;
 
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(80);
-      public static final double FRAME_BORDER_ANGLE = Units.degreesToRadians(30);
+      public static final double ELEVATOR_BORDER_ANGLE = Units.degreesToRadians(10);
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
@@ -236,6 +236,7 @@ public static class PhysicalConstants {
         public static final double NET_REDUCTION = 45.0;
         public static final double MASS_KG = 2.85; // Includes a coral
         public static final double ARM_LENGTH_METERS = 0.083;
+        public static final double JOINT_LENGTH_METERS = 0.10;
         public static final double MOI = 0.0403605447; // Kg*m^2
       }
     }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 06dcb3e..84721a5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -15,6 +15,9 @@
 import frc.robot.commands.coral.motion.MovePivot;
 import frc.robot.commands.coral.motion.MoveRoll;
 import frc.robot.commands.coral.motion.StowArm;
+import frc.robot.commands.coral.motion.WaitArmClearance;
+import frc.robot.commands.coral.motion.WaitElevatorApproach;
+import frc.robot.commands.coral.motion.WaitRollFinished;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import frc.robot.subsystems.*;
@@ -138,6 +141,24 @@ private Command getGoToLockedPresetCommand() {
                 }));
     }
 
+    private Command getGoToLockedPresetCommandV2() {
+        return new InstantCommand(() -> {
+            coralSubsystem.autoSetMirror();
+
+            SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
+        }).andThen(new StowArm(coralSubsystem))
+                .andThen(new ParallelCommandGroup(
+                        new MoveElevator(coralSubsystem, currentLockedPresetSupplier),
+                        new MovePivot(coralSubsystem, currentLockedPresetSupplier),
+                        new WaitArmClearance(coralSubsystem)
+                                .andThen(new MoveRoll(coralSubsystem, currentLockedPresetSupplier)),
+                        new WaitRollFinished(coralSubsystem).andThen(new WaitElevatorApproach(coralSubsystem, 0.4))
+                                .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier))))
+                .andThen(new InstantCommand(() -> {
+                    SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done");
+                }));
+    }
+
     // Goes to a preset more quickly by moving pitch+pivot+roll at the same time,
     // but can throw coral. Good for intaking
     private Command getGoToLockedPresetFASTCommand() {
@@ -211,7 +232,7 @@ private void configureBindings() {
             lockCoralArmPreset(selectedScoringPreset);
             if (!coralSubsystem.isHolding())
                 isScoring = true;
-        }).andThen(getGoToLockedPresetCommand().andThen(
+        }).andThen(getGoToLockedPresetCommandV2().andThen(
                 new InstantCommand(() -> {
                     operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
                 }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> {
@@ -269,8 +290,10 @@ public boolean getAsBoolean() {
 
         coralAquisition.onChange(new InstantCommand(() -> {
             operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
+            driverXbox.setRumble(RumbleType.kBothRumble, 1.0);
         }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> {
             operatorXbox.setRumble(RumbleType.kBothRumble, 0.0);
+            driverXbox.setRumble(RumbleType.kBothRumble, 0.0);
         })));
 
         // operatorXbox.leftBumper().onTrue(new InstantCommand(() -> {
diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
index 51b2904..f4dddfc 100644
--- a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
+++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java
@@ -15,7 +15,7 @@ public class MovePivot extends Command {
     public MovePivot(CoralSubsystem coralSub, Supplier<CoralPresets> presetSupplier) {
         this.coralSub = coralSub;
         this.presetSupplier = presetSupplier;
-        addRequirements(coralSub);
+        // addRequirements(coralSub);
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java
new file mode 100644
index 0000000..54402d6
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java
@@ -0,0 +1,28 @@
+package frc.robot.commands.coral.motion;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class WaitArmClearance extends Command {
+    private CoralSubsystem coralSub;
+
+    public WaitArmClearance(CoralSubsystem coralSub) {
+        this.coralSub = coralSub;
+        // addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(coralSub.getPivotPositionDegrees()) > Units
+                .radiansToDegrees(Constants.Coral.Pivot.ELEVATOR_BORDER_ANGLE);
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java
new file mode 100644
index 0000000..0df8c59
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java
@@ -0,0 +1,28 @@
+package frc.robot.commands.coral.motion;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class WaitElevatorApproach extends Command {
+    private CoralSubsystem coralSub;
+    private double metersBefore;
+
+    public WaitElevatorApproach(CoralSubsystem coralSub, double metersBefore) {
+        this.coralSub = coralSub;
+        this.metersBefore = metersBefore;
+    }
+
+    @Override
+    public void initialize() {
+        System.out.printf("Finishing %f meters before\n", metersBefore);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return coralSub.getElevator().getPosition() > (coralSub.getElevator().getGoalPosition() - metersBefore);
+    }
+}
diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java
new file mode 100644
index 0000000..3489ad5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.coral.motion;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets;
+
+public class WaitRollFinished extends Command {
+    private CoralSubsystem coralSub;
+
+    public WaitRollFinished(CoralSubsystem coralSub) {
+        this.coralSub = coralSub;
+        // addRequirements(coralSub);
+    }
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return (Math.abs(coralSub.getRollGoalDegrees()) > 10) && coralSub.isRollSupposedToBeInPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
index 791d117..3516a2a 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralArm.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java
@@ -32,435 +32,448 @@
 import frc.robot.Constants.Coral;
 
 public class CoralArm extends SubsystemBase {
-    private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
-    private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
-    // sim motors
-    private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
-    private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
-
-    private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
-    private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
-    private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
-    // sim encoders
-    private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
-    private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
-    private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
-
-    // physics simulations
-    private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
-            Coral.Pivot.PhysicalConstants.MOTOR,
-            Coral.Pivot.PhysicalConstants.NET_REDUCTION,
-            Coral.Pivot.PhysicalConstants.MOI,
-            Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
-            0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
-            0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
-    private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
-            Coral.Roll.PhysicalConstants.MOTOR,
-            Coral.Roll.PhysicalConstants.NET_REDUCTION,
-            Coral.Roll.PhysicalConstants.MOI,
-            Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
-            Coral.Roll.MAXIMUM_ANGLE * -1,
-            Coral.Roll.MAXIMUM_ANGLE,
-            false,
-            0);
-    private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
-            Coral.Pitch.PhysicalConstants.MOTOR,
-            Coral.Pitch.PhysicalConstants.NET_REDUCTION,
-            Coral.Pitch.PhysicalConstants.MOI,
-            Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
-            Coral.Pitch.MAXIMUM_ANGLE * -1,
-            Coral.Pitch.MAXIMUM_ANGLE,
-            false,
-            0);
-
-    private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
-    private final SparkMaxConfig rollConfig = new SparkMaxConfig();
-    private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
-
-    private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
-    private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
-    private final ProfiledPIDController rollPID = Coral.Roll.PID;
-    private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
-
-    private double pivotGoal = 0.0; // Rads
-    private double rollGoal = 0.0; // Rads
-    private double pitchGoal = 0.0; // Rads
-
-    LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
-    LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
-
-    public CoralArm() {
-        AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
-
-        pivotMotor.configure(
-                pivotConfig.idleMode(IdleMode.kBrake).apply(
-                        new EncoderConfig()
-                                .positionConversionFactor(
-                                        (1.0 * 2.0 * Math.PI)
-                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
-                                .velocityConversionFactor(
-                                        (60.0 * 2.0 * Math.PI)
-                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
-                                                                                                           // ->
-                // Rotations &
-                // Seconds
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        rollMotor.configure(
-                rollConfig.idleMode(IdleMode.kBrake)
-                        .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
-                        .apply(new SparkMaxConfig().inverted(
-                                Constants.Coral.Pitch.MOTOR_INVERTED)),
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        pitchMotor.configure(pitchConfig
-                .idleMode(IdleMode.kBrake).apply(wristEncConfig
-                        .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
-                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
-                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
-
-        /*
-         * .apply(new EncoderConfig()
-         * .positionConversionFactor(
-         * (1.0 * 2.0 * Math.PI)
-         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
-         * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
-         * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
-         */
-        // Not sure how to get
-
-        // Disabling this for now because we don't want the wrist wire bundle to explode
-        // rollPID.enableContinuousInput(-180, 180);
-
-        // TODO: Are these reasonable? who knows.
-        pivotPID.setTolerance(Units.degreesToRadians(3.0));
-        rollPID.setTolerance(Units.degreesToRadians(3.0));
-        pitchPID.setTolerance(Units.degreesToRadians(3.0));
-
-        if (Constants.Coral.DEBUG_PIDS) {
-            SmartDashboard.putNumber("Coral/Pivot/PID/P", pivotPID.getP());
-            SmartDashboard.putNumber("Coral/Pivot/PID/I", pivotPID.getI());
-            SmartDashboard.putNumber("Coral/Pivot/PID/D", pivotPID.getD());
-
-            SmartDashboard.putNumber("Coral/Roll/PID/P", rollPID.getP());
-            SmartDashboard.putNumber("Coral/Roll/PID/I", rollPID.getI());
-            SmartDashboard.putNumber("Coral/Roll/PID/D", rollPID.getD());
-
-            SmartDashboard.putNumber("Coral/Pitch/PID/P", pitchPID.getP());
-            SmartDashboard.putNumber("Coral/Pitch/PID/I", pitchPID.getI());
-            SmartDashboard.putNumber("Coral/Pitch/PID/D", pitchPID.getD());
+        private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless);
+        private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless);
+        // sim motors
+        private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR);
+        private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR);
+
+        private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT);
+        private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog();
+        private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog();
+        // sim encoders
+        private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor);
+        private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor);
+        private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor);
+
+        // physics simulations
+        private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim(
+                        Coral.Pivot.PhysicalConstants.MOTOR,
+                        Coral.Pivot.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pivot.PhysicalConstants.MOI,
+                        Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS,
+                        0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE,
+                        0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI);
+        private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim(
+                        Coral.Roll.PhysicalConstants.MOTOR,
+                        Coral.Roll.PhysicalConstants.NET_REDUCTION,
+                        Coral.Roll.PhysicalConstants.MOI,
+                        Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS,
+                        Coral.Roll.MAXIMUM_ANGLE * -1,
+                        Coral.Roll.MAXIMUM_ANGLE,
+                        false,
+                        0);
+        private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim(
+                        Coral.Pitch.PhysicalConstants.MOTOR,
+                        Coral.Pitch.PhysicalConstants.NET_REDUCTION,
+                        Coral.Pitch.PhysicalConstants.MOI,
+                        Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS,
+                        Coral.Pitch.MAXIMUM_ANGLE * -1,
+                        Coral.Pitch.MAXIMUM_ANGLE,
+                        false,
+                        0);
+
+        private final SparkFlexConfig pivotConfig = new SparkFlexConfig();
+        private final SparkMaxConfig rollConfig = new SparkMaxConfig();
+        private final SparkMaxConfig pitchConfig = new SparkMaxConfig();
+
+        private final ProfiledPIDController pivotPID = Coral.Pivot.PID;
+
+        public ProfiledPIDController getPivotPID() {
+                return pivotPID;
         }
 
-        // Warm up the filters
-        for (int i = 0; i < 3; ++i) {
-            readPitchEncoderPosition();
-            readRollEncoderPosition();
+        public ProfiledPIDController getRollPID() {
+                return rollPID;
         }
-    }
-
-    double lastPitchReading = 0.0;
-    double lastRollReading = 0.0;
-
-    @Override
-    public void periodic() {
-        // if entering the frame border
-        // && wrist is at neutral
-        if ((pivotPID.getGoal().position != pivotGoal)
-                && (rollPID.atGoal() && pitchPID.atGoal())) {
-            // allow arm to enter frame border
-            pivotPID.setGoal(pivotGoal);
-        } else if ( // if exiting the frame border && has exited the frame border
-        ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
-                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.FRAME_BORDER_ANGLE)) {
-            // set roll and pitch back to their goals
-            rollPID.setGoal(rollGoal);
-            pitchPID.setGoal(pitchGoal);
+
+        public ProfiledPIDController getPitchPID() {
+                return pitchPID;
+        }
+
+        private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD;
+        private final ProfiledPIDController rollPID = Coral.Roll.PID;
+        private final ProfiledPIDController pitchPID = Coral.Pitch.PID;
+
+        private double pivotGoal = 0.0; // Rads
+        private double rollGoal = 0.0; // Rads
+        private double pitchGoal = 0.0; // Rads
+
+        LinearFilter pitchEncFilter = LinearFilter.movingAverage(3);
+        LinearFilter rollEncFilter = LinearFilter.movingAverage(3);
+
+        public CoralArm() {
+                AnalogSensorConfig wristEncConfig = new AnalogSensorConfig();
+
+                pivotMotor.configure(
+                                pivotConfig.idleMode(IdleMode.kBrake).apply(
+                                                new EncoderConfig()
+                                                                .positionConversionFactor(
+                                                                                (1.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)
+                                                                .velocityConversionFactor(
+                                                                                (60.0 * 2.0 * Math.PI)
+                                                                                                / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder
+                                                                                                                                                           // ->
+                                // Rotations &
+                                // Seconds
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                rollMotor.configure(
+                                rollConfig.idleMode(IdleMode.kBrake)
+                                                .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED))
+                                                .apply(new SparkMaxConfig().inverted(
+                                                                Constants.Coral.Pitch.MOTOR_INVERTED)),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+                pitchMotor.configure(pitchConfig
+                                .idleMode(IdleMode.kBrake).apply(wristEncConfig
+                                                .inverted(Constants.Coral.Pitch.ENCODER_INVERTED))
+                                .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)),
+                                ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+                pivotMotor.getEncoder().setPosition(readPivotEncoderPosition());
+
+                /*
+                 * .apply(new EncoderConfig()
+                 * .positionConversionFactor(
+                 * (1.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)
+                 * .velocityConversionFactor((60.0 * 2.0 * Math.PI)
+                 * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)),
+                 */
+                // Not sure how to get
+
+                // Disabling this for now because we don't want the wrist wire bundle to explode
+                // rollPID.enableContinuousInput(-180, 180);
+
+                // TODO: Are these reasonable? who knows.
+                pivotPID.setTolerance(Units.degreesToRadians(3.0));
+                rollPID.setTolerance(Units.degreesToRadians(3.0));
+                pitchPID.setTolerance(Units.degreesToRadians(3.0));
+
+                if (Constants.Coral.DEBUG_PIDS) {
+                        SmartDashboard.putNumber("Coral/Pivot/PID/P", pivotPID.getP());
+                        SmartDashboard.putNumber("Coral/Pivot/PID/I", pivotPID.getI());
+                        SmartDashboard.putNumber("Coral/Pivot/PID/D", pivotPID.getD());
+
+                        SmartDashboard.putNumber("Coral/Roll/PID/P", rollPID.getP());
+                        SmartDashboard.putNumber("Coral/Roll/PID/I", rollPID.getI());
+                        SmartDashboard.putNumber("Coral/Roll/PID/D", rollPID.getD());
+
+                        SmartDashboard.putNumber("Coral/Pitch/PID/P", pitchPID.getP());
+                        SmartDashboard.putNumber("Coral/Pitch/PID/I", pitchPID.getI());
+                        SmartDashboard.putNumber("Coral/Pitch/PID/D", pitchPID.getD());
+                }
+
+                // Warm up the filters
+                for (int i = 0; i < 3; ++i) {
+                        readPitchEncoderPosition();
+                        readRollEncoderPosition();
+                }
+        }
+
+        double lastPitchReading = 0.0;
+        double lastRollReading = 0.0;
+
+        @Override
+        public void periodic() {
+                // if entering the frame border
+                // && wrist is at neutral
+                if ((pivotPID.getGoal().position != pivotGoal)
+                                && (rollPID.atGoal() && pitchPID.atGoal())) {
+                        // allow arm to enter frame border
+                        pivotPID.setGoal(pivotGoal);
+                } else if ( // if exiting the frame border && has exited the frame border
+                ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position))
+                                && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.ELEVATOR_BORDER_ANGLE)) {
+                        // set roll and pitch back to their goals
+                        rollPID.setGoal(rollGoal);
+                        pitchPID.setGoal(pitchGoal);
+                }
+
+                double rollPosition = readRollEncoderPosition();
+                double pitchPosition = readPitchEncoderPosition();
+                double dRollDt = (rollPosition - lastRollReading) / 0.02;
+                double dPitchDt = (pitchPosition - lastPitchReading) / 0.02;
+                lastPitchReading = pitchPosition;
+                lastRollReading = rollPosition;
+
+                if (Constants.Coral.DEBUG_PIDS) {
+                        pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP()));
+                        pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI()));
+                        pivotPID.setD(SmartDashboard.getNumber("Coral/Pivot/PID/D", pivotPID.getD()));
+
+                        rollPID.setP(SmartDashboard.getNumber("Coral/Roll/PID/P", rollPID.getP()));
+                        rollPID.setI(SmartDashboard.getNumber("Coral/Roll/PID/I", rollPID.getI()));
+                        rollPID.setD(SmartDashboard.getNumber("Coral/Roll/PID/D", rollPID.getD()));
+
+                        pitchPID.setP(SmartDashboard.getNumber("Coral/Pitch/PID/P", pitchPID.getP()));
+                        pitchPID.setI(SmartDashboard.getNumber("Coral/Pitch/PID/I", pitchPID.getI()));
+                        pitchPID.setD(SmartDashboard.getNumber("Coral/Pitch/PID/D", pitchPID.getD()));
+                }
+                /*
+                 * run the motors
+                 */
+
+                // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
+                // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
+                double pivotPosition = readPivotEncoderPosition();
+
+                double pivotFFout = pivotFeedforward.calculate(
+                                Math.PI * 0.5 + pivotPosition,
+                                pivotPID.getSetpoint().velocity);
+                double pivotPIDout = pivotPID.calculate(pivotPosition);
+                SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
+                SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
+                SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
+                SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pivot/velocity_target", pivotPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Pivot/velocity", readPivotEncoderVelocity());
+                SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
+                if (!Constants.Coral.Pivot.DBG_DISABLED)
+                        pivotMotor.setVoltage(
+                                        (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband(
+                                                        pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout);
+                if (Robot.isSimulation())
+                        simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
+
+                double rollPIDout = rollPID.calculate(readRollEncoderPosition());
+                double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
+                SmartDashboard.putNumber("Coral/Roll/velocity", dRollDt);
+                SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Roll/velocity_target", rollPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
+                SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
+                if (!Constants.Coral.Roll.DBG_DISABLED)
+                        rollMotor.setVoltage(rollPIDout + rollFFout);
+                if (Robot.isSimulation())
+                        simRollMotor.setAppliedOutput(rollPIDout);
+
+                double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
+                double pitchFFout = 0.0;// Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
+                SmartDashboard.putNumber("Coral/Pitch/velocity", dPitchDt);
+                SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
+                SmartDashboard.putNumber("Coral/Pitch/velocity_target", pitchPID.getSetpoint().velocity);
+                SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
+                SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
+                SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
+                SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
+                if (!Constants.Coral.Pitch.DBG_DISABLED)
+                        pitchMotor.setVoltage(pitchPIDout + pitchFFout);
+                if (Robot.isSimulation())
+                        simPitchMotor.setAppliedOutput(pitchPIDout);
+        }
+
+        @Override
+        public void simulationPeriodic() {
+                // update physics
+                simPivotPhysics
+                                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                simRollPhysics
+                                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                simPitchPhysics
+                                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
+                                                -12.0, 12.0));
+                SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
+                                * RoboRioSim.getVInVoltage());
+
+                simPivotPhysics.update(0.02);
+                simRollPhysics.update(0.02);
+                simPitchPhysics.update(0.02);
+
+                // update sim objects
+                simPivotMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simRollMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+                simPitchMotor.iterate(
+                                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
+                                RoboRioSim.getVInVoltage(),
+                                0.02);
+
+                simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
+                                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
+                simRollEncoder.iterate(
+                                Units.radiansToRotations(simRollPhysics.getVelocityRadPerSec()) * 3.3,
+                                0.02);
+                simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
+                                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
+                simPitchEncoder.iterate(
+                                Units.radiansToRotations(simPitchPhysics.getVelocityRadPerSec()) * 3.3,
+                                0.02);
+
+                simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
+                simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
+        }
+
+        // this should be relative to straight upwards.
+        // i.e. 0 should be straight vertical,
+        // 20 would be to the right, -20 to the left
+        public void setPivotGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pivotGoal = MathUtil.clamp(goalRads,
+                                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
+                // if passing through the frame border
+                /*
+                 * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
+                 * && (Math.abs(this.getPivotPositionDegrees()) <
+                 * Coral.Pivot.FRAME_BORDER_ANGLE)) {
+                 * // set the pivot to stop at the frame border to allow the wrist to move
+                 * // neutral
+                 * pivotPID.setGoal(
+                 * Coral.Pivot.FRAME_BORDER_ANGLE
+                 * (pivotGoal < 0 ? -1 : 1));
+                 * rollPID.setGoal(0);
+                 * pitchPID.setGoal(0);
+                 * } else {
+                 * pivotPID.setGoal(pivotGoal);
+                 * }
+                 */
+
+                pivotPID.setGoal(pivotGoal);
+        }
+
+        // similarly, this ranges from -180 to 180
+        public void setRollGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
+                /*
+                 * // if outside of frame border
+                 * if (Math.abs(this.getPivotPositionDegrees()) >
+                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
+                 * // start moving to goal
+                 * rollPID.setGoal(rollGoal);
+                 * }
+                 */
+                rollPID.setGoal(rollGoal);
+        }
+
+        public void setPitchGoalDegrees(double goal) {
+                double goalRads = Units.degreesToRadians(goal);
+                pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
+                /*
+                 * // if outside the frame border
+                 * if (Math.abs(this.getPivotPositionDegrees()) >
+                 * Coral.Pivot.FRAME_BORDER_ANGLE) {
+                 * // start moving to goal
+                 * pitchPID.setGoal(pitchGoal);
+                 * }
+                 */
+                pitchPID.setGoal(pitchGoal);
+        }
+
+        public double getPivotGoalDegrees() {
+                return Units.radiansToDegrees(pivotGoal);
+        }
+
+        public double getRollGoalDegrees() {
+                return Units.radiansToDegrees(rollGoal);
+        }
+
+        public double getPitchGoalDegrees() {
+                return Units.radiansToDegrees(pitchGoal);
+        }
+
+        public double getPivotPositionDegrees() {
+                return Units.radiansToDegrees(readPivotEncoderPosition());
+        }
+
+        public double getRollPositionDegrees() {
+                return Units.radiansToDegrees(readRollEncoderPosition());
+        }
+
+        public double getPitchPositionDegrees() {
+                return Units.radiansToDegrees(readPitchEncoderPosition());
+        }
+
+        public boolean isPitchInPosition() {
+                return pitchPID.atGoal();
+        }
+
+        public boolean isRollInPosition() {
+                return rollPID.atGoal();
+        }
+
+        public boolean isPivotInPosition() {
+                return pivotPID.atGoal();
+        }
+
+        public double readPitchEncoderPosition() {
+                return MathUtil
+                                .angleModulus(((pitchEncFilter
+                                                .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
+                                                                : pitchEncoder.getPosition())
+                                                + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
+                                                / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readPitchEncoderVelocity() {
+                return MathUtil
+                                .angleModulus(((pitchEncFilter
+                                                .calculate(Robot.isSimulation() ? simPitchEncoder.getVelocity()
+                                                                : pitchEncoder.getVelocity()))
+                                                / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readRollEncoderPosition() {
+                return MathUtil
+                                .angleModulus(((rollEncFilter
+                                                .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
+                                                                : rollEncoder.getPosition())
+                                                + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readRollEncoderVelocity() {
+                return MathUtil
+                                .angleModulus(((rollEncFilter
+                                                .calculate(Robot.isSimulation() ? simRollEncoder.getVelocity()
+                                                                : rollEncoder.getVelocity()))
+                                                / 3.3)
+                                                * 2 * Math.PI);
+        }
+
+        public double readPivotEncoderPosition() {
+                return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
+                                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
+                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+        }
+
+        public double readPivotEncoderVelocity() {
+                return Robot.isSimulation() ? (simPivotPhysics.getVelocityRadPerSec())
+                                : Units.rotationsToRadians((pivotEncoder.getVelocity().getValueAsDouble()
+                                                * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
+        }
+
+        public boolean isPitchSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
+                // this.pitchPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
+        }
+
+        public boolean isPivotSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
+                // this.pivotPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
         }
 
-        double rollPosition = readRollEncoderPosition();
-        double pitchPosition = readPitchEncoderPosition();
-        double dRollDt = (rollPosition - lastRollReading) / 0.02;
-        double dPitchDt = (pitchPosition - lastPitchReading) / 0.02;
-        lastPitchReading = pitchPosition;
-        lastRollReading = rollPosition;
-
-        if (Constants.Coral.DEBUG_PIDS) {
-            pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP()));
-            pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI()));
-            pivotPID.setD(SmartDashboard.getNumber("Coral/Pivot/PID/D", pivotPID.getD()));
-
-            rollPID.setP(SmartDashboard.getNumber("Coral/Roll/PID/P", rollPID.getP()));
-            rollPID.setI(SmartDashboard.getNumber("Coral/Roll/PID/I", rollPID.getI()));
-            rollPID.setD(SmartDashboard.getNumber("Coral/Roll/PID/D", rollPID.getD()));
-
-            pitchPID.setP(SmartDashboard.getNumber("Coral/Pitch/PID/P", pitchPID.getP()));
-            pitchPID.setI(SmartDashboard.getNumber("Coral/Pitch/PID/I", pitchPID.getI()));
-            pitchPID.setD(SmartDashboard.getNumber("Coral/Pitch/PID/D", pitchPID.getD()));
+        public boolean isRollSupposedToBeInPosition() {
+                // return MathUtil.isNear(this.rollPID.getSetpoint().position,
+                // this.rollPID.getGoal().position, 0.01);
+                return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
         }
-        /*
-         * run the motors
-         */
-
-        // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition()
-        // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition();
-        double pivotPosition = readPivotEncoderPosition();
-
-        double pivotFFout = pivotFeedforward.calculate(
-                Math.PI * 0.5 + pivotPosition,
-                pivotPID.getSetpoint().velocity);
-        double pivotPIDout = pivotPID.calculate(pivotPosition);
-        SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout);
-        SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout);
-        SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout);
-        SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition);
-        SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Pivot/velocity_target", pivotPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Pivot/velocity", readPivotEncoderVelocity());
-        SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position);
-        if (!Constants.Coral.Pivot.DBG_DISABLED)
-            pivotMotor.setVoltage(
-                    (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband(
-                            pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout);
-        if (Robot.isSimulation())
-            simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout);
-
-        double rollPIDout = rollPID.calculate(readRollEncoderPosition());
-        double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition());
-        SmartDashboard.putNumber("Coral/Roll/velocity", dRollDt);
-        SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Roll/velocity_target", rollPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position);
-        SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout);
-        SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout);
-        SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout);
-        if (!Constants.Coral.Roll.DBG_DISABLED)
-            rollMotor.setVoltage(rollPIDout + rollFFout);
-        if (Robot.isSimulation())
-            simRollMotor.setAppliedOutput(rollPIDout);
-
-        double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition());
-        double pitchFFout = 0.0;// Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition());
-        SmartDashboard.putNumber("Coral/Pitch/velocity", dPitchDt);
-        SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position);
-        SmartDashboard.putNumber("Coral/Pitch/velocity_target", pitchPID.getSetpoint().velocity);
-        SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position);
-        SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout);
-        SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout);
-        SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout);
-        if (!Constants.Coral.Pitch.DBG_DISABLED)
-            pitchMotor.setVoltage(pitchPIDout + pitchFFout);
-        if (Robot.isSimulation())
-            simPitchMotor.setAppliedOutput(pitchPIDout);
-    }
-
-    @Override
-    public void simulationPeriodic() {
-        // update physics
-        simPivotPhysics
-                .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        simRollPhysics
-                .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        simPitchPhysics
-                .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(),
-                        -12.0, 12.0));
-        SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads()
-                * RoboRioSim.getVInVoltage());
-
-        simPivotPhysics.update(0.02);
-        simRollPhysics.update(0.02);
-        simPitchPhysics.update(0.02);
-
-        // update sim objects
-        simPivotMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-        simRollMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-        simPitchMotor.iterate(
-                Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()),
-                RoboRioSim.getVInVoltage(),
-                0.02);
-
-        simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3
-                - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS);
-        simRollEncoder.iterate(
-                Units.radiansToRotations(simRollPhysics.getVelocityRadPerSec()) * 3.3,
-                0.02);
-        simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3
-                - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS);
-        simPitchEncoder.iterate(
-                Units.radiansToRotations(simPitchPhysics.getVelocityRadPerSec()) * 3.3,
-                0.02);
-
-        simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5);
-        simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02);
-    }
-
-    // this should be relative to straight upwards.
-    // i.e. 0 should be straight vertical,
-    // 20 would be to the right, -20 to the left
-    public void setPivotGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        pivotGoal = MathUtil.clamp(goalRads,
-                -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE);
-        // if passing through the frame border
-        /*
-         * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0))
-         * && (Math.abs(this.getPivotPositionDegrees()) <
-         * Coral.Pivot.FRAME_BORDER_ANGLE)) {
-         * // set the pivot to stop at the frame border to allow the wrist to move
-         * // neutral
-         * pivotPID.setGoal(
-         * Coral.Pivot.FRAME_BORDER_ANGLE
-         * (pivotGoal < 0 ? -1 : 1));
-         * rollPID.setGoal(0);
-         * pitchPID.setGoal(0);
-         * } else {
-         * pivotPID.setGoal(pivotGoal);
-         * }
-         */
-
-        pivotPID.setGoal(pivotGoal);
-    }
-
-    // similarly, this ranges from -180 to 180
-    public void setRollGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE);
-        /*
-         * // if outside of frame border
-         * if (Math.abs(this.getPivotPositionDegrees()) >
-         * Coral.Pivot.FRAME_BORDER_ANGLE) {
-         * // start moving to goal
-         * rollPID.setGoal(rollGoal);
-         * }
-         */
-        rollPID.setGoal(rollGoal);
-    }
-
-    public void setPitchGoalDegrees(double goal) {
-        double goalRads = Units.degreesToRadians(goal);
-        pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE);
-        /*
-         * // if outside the frame border
-         * if (Math.abs(this.getPivotPositionDegrees()) >
-         * Coral.Pivot.FRAME_BORDER_ANGLE) {
-         * // start moving to goal
-         * pitchPID.setGoal(pitchGoal);
-         * }
-         */
-        pitchPID.setGoal(pitchGoal);
-    }
-
-    public double getPivotGoalDegrees() {
-        return Units.radiansToDegrees(pivotGoal);
-    }
-
-    public double getRollGoalDegrees() {
-        return Units.radiansToDegrees(rollGoal);
-    }
-
-    public double getPitchGoalDegrees() {
-        return Units.radiansToDegrees(pitchGoal);
-    }
-
-    public double getPivotPositionDegrees() {
-        return Units.radiansToDegrees(readPivotEncoderPosition());
-    }
-
-    public double getRollPositionDegrees() {
-        return Units.radiansToDegrees(readRollEncoderPosition());
-    }
-
-    public double getPitchPositionDegrees() {
-        return Units.radiansToDegrees(readPitchEncoderPosition());
-    }
-
-    public boolean isPitchInPosition() {
-        return pitchPID.atGoal();
-    }
-
-    public boolean isRollInPosition() {
-        return rollPID.atGoal();
-    }
-
-    public boolean isPivotInPosition() {
-        return pivotPID.atGoal();
-    }
-
-    public double readPitchEncoderPosition() {
-        return MathUtil
-                .angleModulus(((pitchEncFilter
-                        .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition()
-                                : pitchEncoder.getPosition())
-                        + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS)
-                        / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readPitchEncoderVelocity() {
-        return MathUtil
-                .angleModulus(((pitchEncFilter
-                        .calculate(Robot.isSimulation() ? simPitchEncoder.getVelocity()
-                                : pitchEncoder.getVelocity()))
-                        / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readRollEncoderPosition() {
-        return MathUtil
-                .angleModulus(((rollEncFilter
-                        .calculate(Robot.isSimulation() ? simRollEncoder.getPosition()
-                                : rollEncoder.getPosition())
-                        + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readRollEncoderVelocity() {
-        return MathUtil
-                .angleModulus(((rollEncFilter
-                        .calculate(Robot.isSimulation() ? simRollEncoder.getVelocity()
-                                : rollEncoder.getVelocity()))
-                        / 3.3)
-                        * 2 * Math.PI);
-    }
-
-    public double readPivotEncoderPosition() {
-        return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
-                : Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
-                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-    }
-
-    public double readPivotEncoderVelocity() {
-        return Robot.isSimulation() ? (simPivotPhysics.getVelocityRadPerSec())
-                : Units.rotationsToRadians((pivotEncoder.getVelocity().getValueAsDouble()
-                        * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
-    }
-
-    public boolean isPitchSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.pitchPID.getSetpoint().position,
-        // this.pitchPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01);
-    }
-
-    public boolean isPivotSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.pivotPID.getSetpoint().position,
-        // this.pivotPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01);
-    }
-
-    public boolean isRollSupposedToBeInPosition() {
-        // return MathUtil.isNear(this.rollPID.getSetpoint().position,
-        // this.rollPID.getGoal().position, 0.01);
-        return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01);
-    }
 }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index 98c38be..bc99c2c 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -19,14 +19,19 @@ public class CoralSubsystem extends SubsystemBase {
 
     private final CoralArm arm = new CoralArm();
     private final CoralIntake intake = new CoralIntake();
+
     private final CoralElevator elevator = new CoralElevator();
 
     private final Mechanism2d coralMechanism = new Mechanism2d(2, 3);
-    private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 0.0, 0.0);
+    private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.0, 0.0);
     private final MechanismLigament2d elevatorMechanism = rootMechanism.append(
-            new MechanismLigament2d("Elevator", Constants.Elevator.PhysicalParameters.BOTTOM_TO_FLOOR, 0));
+            new MechanismLigament2d("Elevator", Constants.Elevator.PhysicalParameters.BOTTOM_TO_FLOOR, 90));
     private final MechanismLigament2d pivotMechanism = elevatorMechanism.append(
             new MechanismLigament2d("Coral", Constants.Coral.Pivot.PhysicalConstants.JOINT_LENGTH_METERS, 0));
+    private final MechanismLigament2d pitchMechanism = pivotMechanism.append(
+            new MechanismLigament2d("Pitch", Constants.Coral.Pitch.PhysicalConstants.JOINT_LENGTH_METERS, 0));
+    private final MechanismLigament2d rollMechanism = pivotMechanism.append(
+            new MechanismLigament2d("Roll", Constants.Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS, 90));
 
     private final AnalogPotentiometer leftUltrasonic = new AnalogPotentiometer(Constants.Coral.LEFT_ULTRASONIC_PORT,
             254.0, 0.0);
@@ -100,6 +105,9 @@ public void periodic() {
         elevatorMechanism
                 .setLength(elevator.getPosition() + Constants.Elevator.PhysicalParameters.CORAL_PIVOT_VERTICAL_OFFSET);
         pivotMechanism.setAngle(arm.getPivotPositionDegrees());
+        pitchMechanism.setAngle(arm.getPitchPositionDegrees());
+        rollMechanism.setLength(Math.cos(Units.degreesToRadians(arm.getRollPositionDegrees()))
+                * Constants.Coral.Roll.PhysicalConstants.JOINT_LENGTH_METERS);
 
         SmartDashboard.putData("Coral Mechanism", coralMechanism);
         SmartDashboard.putBoolean("Elevator in position", isElevatorInPosition());
@@ -283,4 +291,16 @@ public boolean isSupposedToBeInPosition() {
         return isPivotSupposedToBeInPosition() && isRollSupposedToBeInPosition() && isElevatorSupposedToBeInPosition()
                 && isPitchSupposedToBeInPosition();
     }
+
+    public CoralArm getCoralArm() {
+        return arm;
+    }
+
+    public CoralIntake getIntake() {
+        return intake;
+    }
+
+    public CoralElevator getElevator() {
+        return elevator;
+    }
 }

From d9cfff1670de584aab03e8f3b2b64cfe930c6c88 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 16 Feb 2025 13:20:43 -0600
Subject: [PATCH 69/71] Climber stops, zeroing in TEST mode.

---
 src/main/java/frc/robot/Constants.java        | 20 +++++-----
 src/main/java/frc/robot/RobotContainer.java   |  6 ++-
 .../robot/subsystems/ClimberSubsystem.java    | 39 ++++++++++++++-----
 3 files changed, 44 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6b25762..f6ecaa6 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -168,8 +168,8 @@ public static class Climber {
     public static boolean DBG_DISABLED = false;
     public static double GEAR_RATIO = 45.0;
 
-    public static double DEPLOY_SOFT_LIMIT = 0.0;
-    public static double CLIMB_SOFT_LIMIT = 0.0;
+    public static double DEPLOY_SOFT_LIMIT = -6.0;
+    public static double CLIMB_SOFT_LIMIT = -2.5;
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
@@ -191,16 +191,16 @@ public static class Pivot {
 
       // TODO: Tune in simulation
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          18.0,
+          12.0,
           0.0,
-          0.1,
+          0.005,
           new TrapezoidProfile.Constraints(7.0, 15.0));
 
       // Updated with THEORETICAL values
       public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(
           0.0,
-          0.4, // V
-          0.35, // 1.0, // V*s/rad
+          0.0, // V
+          0.0, // 1.0, // V*s/rad
           0.00// V*s^2/rad
       );
 
@@ -222,10 +222,10 @@ public static class Roll {
       public static boolean DBG_DISABLED = false;
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          3.5,
+          6,
           0.0,
-          0.0,
-          new TrapezoidProfile.Constraints(12.5, 20.0));
+          0.02,
+          new TrapezoidProfile.Constraints(15, 25.0));
       // public static final SimpleMotorFeedforward FEEDFORWARD = new
       // SimpleMotorFeedforward(0.0, 0.1);
 
@@ -251,7 +251,7 @@ public static class Pitch {
       public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
 
       public static final ProfiledPIDController PID = new ProfiledPIDController(
-          5.0,
+          7.0,
           0.0,
           0.0,
           new TrapezoidProfile.Constraints(10.0, 30.0)); // Radians
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 84721a5..ad6c5e5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -270,8 +270,9 @@ private void configureBindings() {
          * }
          * })
          */
-        operatorXbox.rightBumper().whileTrue(new ScoreCoralCommand(coralSubsystem));
+        driverXbox.rightBumper().whileTrue(new ScoreCoralCommand(coralSubsystem));
         operatorXbox.rightBumper().whileFalse(getStowCommand());
+        driverXbox.rightBumper().whileFalse(getStowCommand());
 
         // Intake coral
         operatorXbox.rightTrigger().and(new BooleanSupplier() {
@@ -287,6 +288,9 @@ public boolean getAsBoolean() {
 
         // purge coral
         operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem));
+        operatorXbox.button(8).onTrue(new InstantCommand(() -> {
+            climberSubsystem.resetClimberDeploy();
+        }));
 
         coralAquisition.onChange(new InstantCommand(() -> {
             operatorXbox.setRumble(RumbleType.kBothRumble, 1.0);
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 0fd9ac2..114f06c 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -10,6 +10,9 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -22,12 +25,14 @@ public class ClimberSubsystem extends SubsystemBase {
     private RelativeEncoder climberEncoder;
     XboxController operator;
 
+    boolean isTested = false;
+
     public ClimberSubsystem(XboxController operator) {
         this.operator = operator;
         climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake)
-                // .apply(new
-                // SoftLimitConfig().forwardSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT)
-                // .forwardSoftLimitEnabled(true).reverseSoftLimit(Constants.Climber.CLIMB_SOFT_LIMIT))
+                .apply(new SoftLimitConfig().reverseSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT)
+                        .reverseSoftLimitEnabled(true).forwardSoftLimit(0.0)
+                        .forwardSoftLimitEnabled(true))
                 .apply(
                         new EncoderConfig().positionConversionFactor(1.0 / Constants.Climber.GEAR_RATIO)
                                 .velocityConversionFactor(
@@ -44,15 +49,29 @@ public ClimberSubsystem(XboxController operator) {
     @Override
     public void periodic() {
         SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition());
-        // if (climberEncoder.getPosition() > Constants.Climber.CLIMB_SOFT_LIMIT &&
-        // !deployed) {
-        // deployed = true;
-        // climberMotor.configure(climberConfig.apply(climberConfig.softLimit.reverseSoftLimitEnabled(true)),
-        // ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
-        // }
+        if (climberEncoder.getPosition() < Constants.Climber.CLIMB_SOFT_LIMIT &&
+                !deployed) {
+            deployed = true;
+            climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(
+                    Constants.Climber.CLIMB_SOFT_LIMIT)),
+                    ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+        }
+
+        if (DriverStation.isTestEnabled() & !isTested) {
+            climberMotor.configure(
+                    climberConfig.apply(
+                            climberConfig.softLimit.forwardSoftLimitEnabled(false).reverseSoftLimitEnabled(false)),
+                    ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+            isTested = false;
+        }
 
         if (!Constants.Climber.DBG_DISABLED)
-            climberMotor.set(operator.getLeftBumper() ? (operator.getLeftY()) : 0.0);
+            climberMotor.set(operator.getLeftBumper() ? (MathUtil.applyDeadband(operator.getLeftY(), 0.05)) : 0.0);
+    }
+
+    public void resetClimberDeploy() {
+        climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(0.0)),
+                ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
     }
 
     public void setOutput(double output) {

From c19f560069f38ddeec609366cc39554a28aa85ec Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 16 Feb 2025 14:44:38 -0600
Subject: [PATCH 70/71] Better intaking currenting.

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 .../robot/subsystems/coral/CoralIntake.java   |  4 ++++
 .../subsystems/coral/CoralSubsystem.java      | 24 +++++++++++--------
 3 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f6ecaa6..271dcff 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -169,7 +169,7 @@ public static class Climber {
     public static double GEAR_RATIO = 45.0;
 
     public static double DEPLOY_SOFT_LIMIT = -6.0;
-    public static double CLIMB_SOFT_LIMIT = -2.5;
+    public static double CLIMB_SOFT_LIMIT = -3.10;
   }
 
   // TODO: ##################### PLACEHOLDERS #####################
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
index 55d0882..02cd626 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java
@@ -99,6 +99,10 @@ public void setOutputPercentage(double outputPercentage) {
         this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1);
     }
 
+    public void setStatorLimit(double amps) {
+        intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(amps));
+    }
+
     public double getOutputPercentage() {
         return outputPercentage;
     }
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
index bc99c2c..e6ed080 100644
--- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -40,9 +40,9 @@ public class CoralSubsystem extends SubsystemBase {
 
     public enum CoralPresets {
         LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(1.0), Units.radiansToDegrees(1.636)),
-        LEVEL_2(0.247, 19.032, 90, 98.068),
-        LEVEL_3(0.650, 19.032, 90, 98.068),
-        LEVEL_4(1.342, 21.238, 90, 110.062),
+        LEVEL_2(0.247, 18.032, 90, 98.068),
+        LEVEL_3(0.650, 18.032, 90, 98.068),
+        LEVEL_4(1.342, 20.0, 90, 110.062),
         INTAKE(0.05, 18.0, 90, 30.0),
         STOW(0.05, 0.0, 0.0, 0.0),
 
@@ -81,18 +81,20 @@ private MirrorPresets(boolean isMirrored) {
     }
 
     public enum CoralIntakePresets {
-        INTAKE(1),
-        HOLD(0.4),
-        PURGE(-1),
-        SCORE(-1),
-        STOP(0),
+        INTAKE(1, 40.0),
+        HOLD(0.4, 12.5),
+        PURGE(-1, 40.0),
+        SCORE(-1, 30.0),
+        STOP(0, 12.5),
 
-        CUSTOM(Double.NaN);
+        CUSTOM(Double.NaN, 40.0);
 
         double intakePercentage;
+        double intakeCurrent;
 
-        private CoralIntakePresets(double intakePercentage) {
+        private CoralIntakePresets(double intakePercentage, double intakeCurrent) {
             this.intakePercentage = intakePercentage;
+            this.intakeCurrent = intakeCurrent;
         }
     }
 
@@ -258,6 +260,7 @@ public void setCoralIntakePreset(CoralIntakePresets preset) {
         SmartDashboard.putString("Coral Intake Preset", preset.toString());
         if (preset != currentIntakePreset) {
             intake.setOutputPercentage(preset.intakePercentage);
+            intake.setStatorLimit(preset.intakeCurrent);
             currentIntakePreset = preset;
         }
     }
@@ -265,6 +268,7 @@ public void setCoralIntakePreset(CoralIntakePresets preset) {
     public void setCustomIntakePercent(double percentage) {
         currentIntakePreset = CoralIntakePresets.CUSTOM;
         intake.setOutputPercentage(percentage);
+        intake.setStatorLimit(currentIntakePreset.intakeCurrent);
     }
 
     public double getPivotPositionDegrees() {

From 826708220f0a1fba348148ca111e47a45f88dec6 Mon Sep 17 00:00:00 2001
From: NifleySnifley <finnsbart@gmail.com>
Date: Sun, 16 Feb 2025 16:40:08 -0600
Subject: [PATCH 71/71] Disable PID debugging.

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 271dcff..f3ea16b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -177,7 +177,7 @@ public static class Coral {
     public static int LEFT_ULTRASONIC_PORT = 0;
     public static int RIGHT_ULTRASONIC_PORT = 1;
 
-    public static boolean DEBUG_PIDS = true;
+    public static boolean DEBUG_PIDS = false;
 
     public static class Pivot {
       public static final int MOTOR_PORT = 14;