Something to try to fix field oriented control with

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\infin\\Downloads",
+    "serverTeam": "2648"
+  }
+}

.OutlineViewer/outlineviewer.json

@@ -0,0 +1,13 @@
+{
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)"
+  },
+  "transitory": {
+    "Shuffleboard": {
+      "Sensors Tab": {
+        "open": true
+      },
+      "open": true
+    }
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

build.gradle

@@ -10,6 +10,11 @@ java {
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
 deploy {
@@ -72,6 +77,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {

src/main/deploy/pathplanner/autos/1 L4 Center.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Field Oriented Test Auto.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Field Oriented Test Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Backup"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Two Coral Left.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "30 Right to HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 330 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "fein"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/150 Right to HP.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9732020547945206
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.950171232876712,
+        "y": 1.6208904109589037
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4424657534246574,
+        "y": 0.7944777397260271
+      },
+      "prevControl": {
+        "x": 2.389083904109589,
+        "y": 2.5374571917808213
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 54.162347045721745
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 120.96375653207336
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.257106164383561
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.649657534252619,
+        "y": 6.474186643842743
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3072345890410957,
+        "y": 7.135316780821918
+      },
+      "prevControl": {
+        "x": 2.1636986301369863,
+        "y": 6.188698630136986
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.20476190476190542,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -120.06858282186238
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -37,12 +37,6 @@
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
-    {
-      "name": "Score L4",
-      "waypointRelativePos": 0,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
     {
       "name": "HP Pickup",
       "waypointRelativePos": 0.16666666666666663,
@@ -52,9 +46,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

src/main/deploy/pathplanner/paths/Field Oriented Test Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.495389344262295,
+        "y": 6.788165983606557
+      },
+      "prevControl": {
+        "x": 3.4164959016393444,
+        "y": 6.80015368852459
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/H Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.3
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.053790983606557,
+        "y": 4.312704918032787
+      },
+      "isLocked": false,
+      "linkedName": "H"
+    },
+    {
+      "anchor": {
+        "x": 6.389446721311475,
+        "y": 4.3
+      },
+      "prevControl": {
+        "x": 6.1394487099079695,
+        "y": 4.300997143065429
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to 330 Left.path

@@ -12,16 +12,16 @@
         "y": 6.909931506849315
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 4.026883561643835,
-        "y": 5.257106164383561
+        "x": 3.973766447368421,
+        "y": 5.246957236842105
       },
       "prevControl": {
-        "x": 3.7113441780821916,
-        "y": 5.783005136986301
+        "x": 3.6582270638067773,
+        "y": 5.772856209444845
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,17 +33,17 @@
   "pointTowardsZones": [],
   "eventMarkers": [
     {
-      "name": "Score L4",
-      "waypointRelativePos": 0,
+      "name": "Lift L4",
+      "waypointRelativePos": 0.5,
       "endWaypointRelativePos": null,
       "command": null
     }
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

src/main/deploy/pathplanner/paths/J Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.227054794520548
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.1321837955756875,
+        "y": 5.49468698033176
+      },
+      "isLocked": false,
+      "linkedName": "J"
+    },
+    {
+      "anchor": {
+        "x": 5.442044107776481,
+        "y": 6.005045141603656
+      },
+      "prevControl": {
+        "x": 5.268886874487802,
+        "y": 5.749866060967707
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -118.30075576600632
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -121.60750224624898
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 12.272,
+        "y": 2.975
+      },
+      "prevControl": {
+        "x": 10.940715672291898,
+        "y": 2.975
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 5.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.15,
+      "maxWaypointRelativePos": 0.4,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Start to 150 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.602996575342465,
+        "y": 2.0115582191780823
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.491095890410959,
+        "y": 1.9965325342465756
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9882277397260277
+      },
+      "prevControl": {
+        "x": 6.130479452054795,
+        "y": 2.11673801369863
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.7142857142857124,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 120.25643716352937
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.587970890410959,
+        "y": 6.143621575342466
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.385916095890411,
+        "y": 6.158647260273973
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.227054794520548
+      },
+      "prevControl": {
+        "x": 5.574529109589041,
+        "y": 6.098544520547945
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.4547619047619047,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -121.60750224624898
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to H.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.588217213114754,
+        "y": 3.9890368852459024
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.916905737704918,
+        "y": 4.036987704918033
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.3
+      },
+      "prevControl": {
+        "x": 6.347336065573771,
+        "y": 4.2640368852459005
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "H"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.08214624881291864,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.600204918039607,
+        "y": 6.374590163938041
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.600204918032786,
+        "y": 7.573360655737705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.209631147540984,
+        "y": 6.074897540983606
+      },
+      "prevControl": {
+        "x": 7.120696721311476,
+        "y": 6.074897540983606
+      },
+      "nextControl": {
+        "x": 4.745363537952068,
+        "y": 6.074897540983606
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.8670081967213115,
+        "y": 6.973975409836065
+      },
+      "prevControl": {
+        "x": 5.826024590163935,
+        "y": 6.9979508196721305
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -5,9 +5,9 @@
   "pathFolders": [],
   "autoFolders": [],
   "defaultMaxVel": 4.0,
-  "defaultMaxAccel": 4.0,
+  "defaultMaxAccel": 1.0,
   "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxAngAccel": 400.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 48.35,
   "robotMOI": 6.883,

src/main/java/frc/robot/Robot.java

@@ -4,7 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -14,11 +22,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
+
+
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  @SuppressWarnings("resource")
+public Robot() {
+    Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
+
+    if (isReal()) {
+        Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+    } else {
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+        Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+    }
+
+    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+    }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.

src/main/java/frc/robot/RobotContainer.java

@@ -7,18 +7,24 @@ package frc.robot;
 import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Manipulator;
 
+import java.util.function.IntSupplier;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.path.EventMarker;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -35,6 +41,7 @@ public class RobotContainer {
 	private Drivetrain drivetrain;
 
 	private Elevator elevator;
+	//private ElevatorSysID elevator;
 
 	private Manipulator manipulator;
 
@@ -45,6 +52,10 @@ public class RobotContainer {
 
 	private SendableChooser<Command> autoChooser;
 
+	private Vision vision;
+
+	private IntSupplier closestTag;
+
 	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
@@ -52,116 +63,178 @@ public class RobotContainer {
 
 		drivetrain = new Drivetrain();
 
+		vision = new Vision(drivetrain::getGyroValue);
+
 		elevator = new Elevator();
+		//elevator = new ElevatorSysID();
 
 		manipulator = new Manipulator();
 
 		manipulatorPivot = new ManipulatorPivot();
 
+		configureNamedCommands();
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
 		autoChooser = AutoBuilder.buildAutoChooser();
 
-		configureButtonBindings();
+		closestTag = drivetrain::getClosestTag;
 
-		configureNamedCommands();
+		configureButtonBindings();
+		//elevatorSysIDBindings();
+		//elevatorOnlyBindings();
 
 		configureShuffleboard();
 	}
 
+	/*private void elevatorSysIDBindings() {
+		elevator.setDefaultCommand(elevator.maintainPosition());
+
+		operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
+		operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
+		operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
+		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
+	}*/
+
 	private void configureButtonBindings() {
 		//Default commands
 		climberPivot.setDefaultCommand(
-			climberPivot.runPivot(0)
+			climberPivot.runPivot(() -> 0)
 		);
 
 		climberRollers.setDefaultCommand(
-			climberRollers.runRoller(0)
+			climberRollers.runRoller(() -> 0)
 		);
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				driver::getLeftY,
-				driver::getLeftX,
-				driver::getRightX,
+				() -> Math.pow(driver.getLeftY(), 3),
+				() -> Math.pow(driver.getLeftX(), 3),
+				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
 				() -> true
 			)
 		);
-
-		//elevator.setDefaultCommand(
-			//elevator.runAssistedElevator(operator::getLeftY)
-	//	);
-
-		elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
-
-		manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+		
+		elevator.setDefaultCommand(
+			elevator.maintainPosition()
+		);
+		
+		
+		manipulatorPivot.setDefaultCommand(
+			manipulatorPivot.maintainPosition()
+		);
+		
 
 		manipulator.setDefaultCommand(
-			manipulator.defaultCommand()
+			manipulator.runUntilCollected(
+				() -> 0.0
+			)
 		);
 
-		//manipulatorPivot.setDefaultCommand(
-		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
-		//);
-
 		//Driver inputs
+		
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(() -> 1, true)
+			manipulator.runManipulator(() -> 0.35, true)
+		);
+
+		driver.leftTrigger().whileTrue( 
+			manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
 		);
 
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
 
+		driver.y().whileTrue(drivetrain.zeroHeading());
+		
+		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
+		
+		/* 
+		driver.rightBumper().whileTrue(
+				drivetrain.goToPose(
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
+					() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+				)
+			);
+
+		driver.leftBumper().whileTrue(
+			drivetrain.goToPose(
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
+				() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+			)
+		);
+		*/
+
 		//Operator inputs
 		operator.povUp().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL4Position, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL3Position, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL2Position, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL1Position, 
 				ManipulatorPivotConstants.kL1Position
 			)
 		);
 
+		operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+
 		operator.a().onTrue(
-			coralIntakeRoutine()
+			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
 		);
 
 		operator.x().onTrue(
-			algaeIntakeRoutine(true)
+			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
-			algaeIntakeRoutine(false)
+			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.until(() -> driver.a().getAsBoolean())
 		);
+
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.alongWith(manipulator.runManipulator(() -> 0.5, false))
+			.until(() -> driver.a().getAsBoolean())
+		);
+
 	}
 
 	private void configureNamedCommands() {
+		new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
+		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
 	}
 
 	//creates tabs and transforms them on the shuffleboard
@@ -178,31 +251,72 @@ public class RobotContainer {
 
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(0, 1)
-			.withWidget(BuiltInWidgets.kTextView);
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kGraph);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
-			.withSize(2, 1)
-			.withPosition(2, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(2, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
+		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(2, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+			
+		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
+			.withSize(2, 1)
+			.withPosition(0, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
 		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
+			.withSize(1, 1)
+			.withPosition(4, 0)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
 			.withSize(1, 1)
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
 
-		/* 
-		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
+		sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
 			.withSize(1, 1)
-			.withPosition(4, 0)
-			.withWidget(BuiltInWidgets.kBooleanBox);
-			*/
-  }
+			.withPosition(5, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
+			.withSize(1, 1)
+			.withPosition(5, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
+			.withSize(1, 1)
+			.withPosition(5, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
+			.withSize(1, 1)
+			.withPosition(5, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
+
+		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
+
+		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
+
+		sensorTab.addDouble("velocity", drivetrain::getVelocity);
+
+
+		//sensorTab.add("odometry", drivetrain::getPose);
+
+ 	}
 
 	public Command getAutonomousCommand() {
 		return autoChooser.getSelected();
@@ -212,12 +326,13 @@ public class RobotContainer {
 	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
 	 * @return Moves the elevator and arm, then intakes coral
 	 */
+	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
 			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(1));
+			.andThen(manipulator.runUntilCollected(() -> .5));
 		}
 
 	/**
@@ -226,12 +341,13 @@ public class RobotContainer {
 	 * @param l2 Is the algae on L2? (True = L2, False = L3)
 	 * @return Moves the elevator and arm then intakes algae
 	 */
+	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
 			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
-			.andThen(manipulator.runUntilCollected(1));
+			.andThen(manipulator.runUntilCollected(() -> 1));
 	}
 
 	/**
@@ -249,8 +365,8 @@ public class RobotContainer {
 		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
 		// then the elevator, then the arm again
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
@@ -259,8 +375,8 @@ public class RobotContainer {
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		}
 	}
 
@@ -274,41 +390,43 @@ public class RobotContainer {
 	 * @return Moves the elevator and arm to the setpoints
 	 */
 	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
-		if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
+		
+		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
 				armPosition, 
 				0,
 				ManipulatorPivotConstants.kRotationLimit
 			);
-		}
+		}*/
 		
 		return Commands.either(
 			Commands.either(
-				elevator.goToSetpoint(elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
-				elevator.goToSetpoint(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
 				() -> sequential
 			), 
 			Commands.either(
-				manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(elevatorPosition)), 
-				manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
 		);
 	}
 
+	@SuppressWarnings("unused")
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
-				elevator.goToSetpoint(elevatorPosition),
-				manipulatorPivot.goToSetpoint(armPosition));
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.goToSetpoint(() -> elevatorPosition),
+				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
-				elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
-				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition));
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}
@@ -320,12 +438,25 @@ public class RobotContainer {
 	 * @param armPosition The target rotation of the arm
 	 * @return Moves the elevator and arm to the setpoints
 	 */
-	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(armPosition));
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
 	}
 
+	private Command moveWithAlgae(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
+	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));

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

@@ -12,13 +12,13 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 4;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 1;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 1;
-    public static final double kPYController = 1;
-    public static final double kPThetaController = 1;
+    public static final double kPXController = 6;
+    public static final double kPYController = 6;
+    public static final double kPThetaController = 5.5;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(

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

@@ -3,7 +3,7 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ClimberPivotConstants {
-    public static final int kPivotMotorID = 0;
+    public static final int kPivotMotorID = 10;
 
     public static final int kClimberLimitSwitchID = 0;
 

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

@@ -3,7 +3,7 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ClimberRollersConstants {
-    public static final int kRollerMotorID = 0;
+    public static final int kRollerMotorID = 9;
 
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

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

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.8;
+    public static final double kMaxSpeedMetersPerSecond = 5.5;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration
@@ -22,10 +22,17 @@ public class DrivetrainConstants {
     public static final double kWheelBase = Units.inchesToMeters(24.5);
 
     // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
-    public static final double kFrontRightChassisAngularOffset = 0;
-    public static final double kBackLeftChassisAngularOffset = Math.PI;
-    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
+    /* 
+    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = 0;
+*/
+
+    public static final double kFrontLeftChassisAngularOffset = 0;
+    public static final double kFrontRightChassisAngularOffset = Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = -Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = Math.PI;
 
     // 1, 7, 10 is the default for these three values
     public static final double kSysIDDrivingRampRate = 1;
@@ -38,18 +45,24 @@ public class DrivetrainConstants {
     public static final double kSysIDTurningTimeout = 10;
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 11;
-    public static final int kRearLeftDrivingCanId = 13;
-    public static final int kFrontRightDrivingCanId = 15;
-    public static final int kRearRightDrivingCanId = 17;
+    public static final int kFrontLeftDrivingCanId = 0;
+    public static final int kRearLeftDrivingCanId = 2;
+    public static final int kFrontRightDrivingCanId = 1;
+    public static final int kRearRightDrivingCanId = 3;
 
-    public static final int kFrontLeftTurningCanId = 10;
-    public static final int kRearLeftTurningCanId = 12;
-    public static final int kFrontRightTurningCanId = 14;
-    public static final int kRearRightTurningCanId = 16;
+    public static final int kFrontLeftTurningCanId = 2;
+    public static final int kRearLeftTurningCanId = 4;
+    public static final int kFrontRightTurningCanId = 7;
+    public static final int kRearRightTurningCanId = 5;
 
     public static final boolean kGyroReversed = true;
 
+    public static final double kHeadingP = 0.0;
+
+    public static final double kXTranslationP = 0.0;
+    public static final double kYTranslationP = 0.0;
+
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
 
     // Distance between front and back wheels on robot

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

@@ -11,51 +11,54 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
-    public static final int kElevatorMotor1ID = 0;
-    public static final int kElevatorMotor2ID = 0;
+    public static final int kElevatorMotor1ID = 8;
+    public static final int kElevatorMotor2ID = 6;
 
     public static final int kBottomLimitSwitchID = 0;
 
     // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
-    public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;
+    public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
+    public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0;
-    public static final double kPositionControllerI = 0;
-    public static final double kPositionControllerD = 0;
+    public static final double kUpControllerP = 5.6;//7; //
+    public static final double kUpControllerI = 0;
+    public static final double kUpControllerD = 0.28;//0.28
 
-    public static final double kAllowedError = 0.5;
-    
-    /* 
-    public static final double kVelocityControllerP = 0;
-    public static final double kVelocityControllerI = 0;
-    public static final double kVelocityControllerD = 0;
-    */
+    public static final double kDownControllerP = 5.6;//7; //
+    public static final double kDownControllerI = 0;
+    public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
 
-    public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0.53; // calculated value
-    public static final double kFeedForwardV = 4.78; // calculated value
+    public static final double kMaintainP = 3;
+     
+    public static final double kAllowedError = 1;
 
-    public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 500.0; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8;   /* kG too high - kG too low / 2  0.95, 0.2 */
+    public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
+    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
+
+    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 14.5;
-    public static final double kL3Position = 29.0;
-    public static final double kL4Position = 53.0;
+    public static final double kL2Position = 9;
+    public static final double kL3Position = 23.0;
+    public static final double kL4Position = 50.5;
     public static final double kL4TransitionPosition = 40.0;
-    public static final double kL2AlgaePosition = 22.0;
+    public static final double kL2AlgaePosition = 23.0;
     public static final double kL3AlgaePosition = 39.0;
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
-    public static final double kBracePosition = 5.5;
-    public static final double kMaxHeight = 53.0;
+    public static final double kBracePosition = 0;
+    public static final double kMaxHeight = 51.0; //actual is 51
+
+    public static final double kVoltageLimit = 7;
 
     // 1, 7, 10 are the defaults for these, change as necessary
-    public static final double kSysIDRampRate = 1;
-    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDRampRate = .25;
+    public static final double kSysIDStepVolts = 3;
     public static final double kSysIDTimeout = 10;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -73,18 +76,10 @@ public class ElevatorConstants {
     static {
         motorConfig
             .smartCurrentLimit(kCurrentLimit)
-            .idleMode(kIdleMode);
+            .idleMode(kIdleMode)
+            .inverted(true);
         motorConfig.encoder
-            .positionConversionFactor(kEncoderConversionFactor)
-            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
-        motorConfig.closedLoop
-            .p(kPositionControllerP)
-            .i(kPositionControllerI)
-            .d(kPositionControllerD)
-            .velocityFF(0.0); // keep at zero for position pid
-        motorConfig.closedLoop.maxMotion
-            .maxAcceleration(kMaxAcceleration)
-            .maxVelocity(kMaxVelocity)
-            .allowedClosedLoopError(kAllowedError);
+            .positionConversionFactor(kEncoderPositionConversionFactor)
+            .velocityConversionFactor(kEncoderVelocityConversionFactor);
     }
 }

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

@@ -3,9 +3,8 @@ package frc.robot.constants;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
-    public static final int kManipulatorMotorID = 0;
-    public static final int kCoralBeamBreakID = 0;
-    public static final int kAlgaeBeamBreakID = 0;
-
+    public static final int kManipulatorMotorID = 12;
+    public static final int kCoralBeamBreakID = 2;
+    
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

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

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
@@ -13,47 +12,48 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ManipulatorPivotConstants {
-    public static final int kArmMotorID = 0;
+    public static final int kPivotMotorID = 1;
 
     public static final int kMotorCurrentMax = 40;
 
-    public static final double kPivotConversion = 12/60 * 20/60 * 12/28;
+    public static final double kPivotConversion = 2 * Math.PI;
 
-    public static final double kPivotMaxVelocity = 0;
+    public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 0;
+    public static final double kPositionalP = 4;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
-    public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
+    public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
 
-    public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0.41; // calculated value
-    public static final double kFeedForwardV = 0.68; //calculated value
+    public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
+    public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
+    public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
-    public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
 
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
+    public static final double kEncoderOffset = 0.780;
+
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(25.0);
-    public static final double kL3Position = Units.degreesToRadians(60.0);
+    public static final double kL2Position = Units.degreesToRadians(22.0);
+    public static final double kL3Position = Units.degreesToRadians(22.0);
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kProcesserPosition = Units.degreesToRadians(175.0);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0);
+    public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0);
-    /**The forward rotation limit of the arm */
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
+    /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;
     public static final double kSysIDTimeout = 10;
 
-    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
-
     public static final IdleMode kIdleMode = IdleMode.kBrake;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
@@ -67,20 +67,14 @@ public class ManipulatorPivotConstants {
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 
     static {
-
         motorConfig
             .smartCurrentLimit(kMotorCurrentMax)
-            .idleMode(kIdleMode);
-        motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
-        motorConfig.closedLoop
-            .p(kPositionalP)
-            .i(kPositionalI)
-            .d(kPositionalD)
-            .velocityFF(0.0); // keep at zero for position pid
-        motorConfig.closedLoop.maxMotion
-            .maxAcceleration(kMaxAcceleration)
-            .maxVelocity(kMaxVelocity)
-            .allowedClosedLoopError(kPositionalTolerance);
+            .idleMode(kIdleMode)
+            .inverted(true);
+        motorConfig.absoluteEncoder
+            .positionConversionFactor(kPivotConversion)
+            .inverted(false)
+            .zeroOffset(kEncoderOffset);
     }
 
 

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

@@ -2,6 +2,7 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.AudioConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -22,7 +23,7 @@ public class ModuleConstants {
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
     // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
     // teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
@@ -47,7 +48,7 @@ public class ModuleConstants {
 
     public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
 
-    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_Positive;
     public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM 
@@ -57,6 +58,7 @@ public class ModuleConstants {
     public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
     public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
     public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
+    public static final AudioConfigs kAudioConfig = new AudioConfigs();
     public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
 
     static {
@@ -70,6 +72,8 @@ public class ModuleConstants {
         kDriveMotorConfig.Inverted = kDriveInversionState;
         kDriveMotorConfig.NeutralMode = kDriveIdleMode;
 
+        kAudioConfig.AllowMusicDurDisable = true;
+
         kDriveSlot0Config.kP = kDriveP;
         kDriveSlot0Config.kI = kDriveI;
         kDriveSlot0Config.kD = kDriveD;

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

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class NeoMotorConstants {
-    public static final double kFreeSpeedRpm = 5676;
+    public static final double kFreeSpeedRpm = 6000; //for kraken not neo
 }

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

@@ -0,0 +1,62 @@
+package frc.robot.constants;
+
+public class VisionConstants {
+    
+    // global coordinate map of all tags. index is the tag id.
+    // Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
+    // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
+    public static final double[][] globalTagCoords = {{},
+        {656.98, 24.73, 58.50, 126.0, 0},
+        {656.98, 291.90, 58.50, 234.0, 0},
+        {452.4, 316.21, 51.25, 270, 0},
+        {365.2, 241.44, 73.54, 0, 30},
+        {365.2, 75.19, 73.54, 0, 30},
+        {530.49, 129.97, 12.13, 300, 0},
+        {546.87, 158.3, 12.13, 0, 0},
+        {530.49, 186.63, 12.13, 60, 0},
+        {497.77, 186.63, 12.13, 120, 0},
+        {481.39, 158.3, 12.13, 180, 0},
+        {497.77, 129.97, 12.13, 240, 0},
+        {33.9, 24.73, 58.5, 54, 0},
+        {33.9, 291.9, 58.5, 306, 0},
+        {325.68, 241.44, 73.54, 180, 30},
+        {325.68, 75.19, 73.54, 180, 30},
+        {238.49, 0.42, 51.25, 90, 0},
+        {160.39, 129.97, 12.13, 240, 0},
+        {144.00, 158.3, 12.13, 180, 0},
+        {160.39, 186.63, 12.13, 120, 0},
+        {193.1, 186.63, 12.13, 60, 0},
+        {209.49, 158.3, 12.13, 0, 0},
+        {193.1, 129.97, 12.13, 300, 0},
+    };
+
+    //map of coral placing setpoints based on the tag that is on the same reef face
+    // and the on the left or right branch of that side of the reef
+    // <tag_number, {left_x, left_y, right_x, right_y}>
+    public static final double[][] reefSetpointsMap = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
+        {5.789+12.272, 3.862, 5.789+12.272, 4.194},
+        {5.275+12.272, 5.075, 4.991+12.272, 5.246},
+        {3.986+12.272, 5.24, 3.701+12.272, 5.076},
+        {3.183+12.272, 4.191, 3.183, 3.857},
+        {3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.703, 3.975, 3.982, 2.806},
+        {3.183, 4.191, 3.183, 3.857},
+        {3.986, 5.24, 3.701, 5.076},
+        {5.275, 5.075, 4.991, 5.246},
+        {5.789, 3.862, 5.789, 4.194},
+        {4.993, 2.816, 5.272, 2.996}
+    };
+
+}

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

@@ -1,12 +1,13 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberPivotConstants;
@@ -16,8 +17,6 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
-    private DigitalInput cageLimitSwitch;
-
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
@@ -31,13 +30,11 @@ public class ClimberPivot extends SubsystemBase {
         );
 
         neoEncoder = pivotMotor.getEncoder();
-
-        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
     }
 
-    public Command runPivot(double speed) {
+    public Command runPivot(DoubleSupplier speed) {
         return run(() -> {
-            pivotMotor.set(speed);
+            pivotMotor.set(speed.getAsDouble());
         });
     }
 
@@ -54,15 +51,6 @@ public class ClimberPivot extends SubsystemBase {
         }).until(() -> neoEncoder.getPosition() >= setpoint);
     }
 
-    /**
-     * Returns the limit switch attached to the climber. Detects if the cage is present
-     * 
-     * @return Is the cage in the climber
-     */
-    public boolean getCageLimitSwitch() {
-        return cageLimitSwitch.get();
-    }
-
     public double getEncoderPosition() {
         return neoEncoder.getPosition();
     }

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -32,9 +34,9 @@ public class ClimberRollers extends SubsystemBase {
      * @param speed The speed in which the roller runs
      * @return Runs the rollers at a set speed
      */
-    public Command runRoller(double speed) {
+    public Command runRoller(DoubleSupplier speed) {
         return run(() -> {
-            rollerMotor.set(speed);
+            rollerMotor.set(speed.getAsDouble());
         });
     }
 }

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

@@ -4,15 +4,18 @@
 
 package frc.robot.subsystems;
 
+import java.io.File;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.ctre.phoenix6.Orchestra;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -21,11 +24,14 @@ 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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.VisionConstants;
 
 public class Drivetrain extends SubsystemBase {
     // Create MAXSwerveModules
@@ -35,11 +41,18 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private AHRS ahrs;
+    private AHRS gyro;
 
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
 
+    public Orchestra m_orchestra = new Orchestra();
+    private Timer musicTimer = new Timer();
+
+    private PIDController pidHeading;
+    private PIDController pidTranslationX;
+    private PIDController pidTranslationY;
+
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
         m_frontLeft = new MAXSwerveModule(
@@ -66,11 +79,11 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        ahrs = new AHRS(NavXComType.kMXP_SPI);
+        gyro = new AHRS(NavXComType.kMXP_SPI);
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(ahrs.getAngle()),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -80,6 +93,12 @@ public class Drivetrain extends SubsystemBase {
             new Pose2d()
         );
 
+        pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
+        pidHeading.enableContinuousInput(-180, 180);
+
+        pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
+        pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
+
         AutoBuilder.configure(
             this::getPose, 
             this::resetOdometry, 
@@ -96,6 +115,21 @@ public class Drivetrain extends SubsystemBase {
             }, 
             this
         );
+
+        m_orchestra.loadMusic(Filesystem.getDeployDirectory()
+        .toPath()
+        .resolve("Orchestra" + File.separator + "doomE1M1.chrp")
+        .toString());
+
+        // Add a single device to the orchestra
+        m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
+        m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
+        m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
+        m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
+
+        m_orchestra.play();
+        musicTimer.reset();
+        musicTimer.start();
     }
 
     @Override
@@ -109,6 +143,32 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
+
+
+        // if the detected tags match your alliances reef tags use their pose estimates 
+        /* 
+        if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+            
+        }else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+        }
+
+        if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); 
+        }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
+        }
+        */
+        
+        if(musicTimer.get()>20){
+            if (m_orchestra.isPlaying()) {
+                m_orchestra.stop();
+            }
+            musicTimer.stop();
+            musicTimer.reset();
+        }
+        
     }
 
     public ChassisSpeeds getCurrentChassisSpeeds() {
@@ -143,7 +203,14 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_estimator.resetPose(
+        m_estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()), 
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+            }, 
             pose
         );
     }
@@ -194,6 +261,50 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
+        return run(() -> {
+            drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
+                pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), 
+                pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), 
+                true);
+            });
+    }
+
+    public int getClosestTag(){
+
+		if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
+			int closestTag = 17;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			
+			for(int i = 17; i <= 22; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}else{
+			int closestTag = 6;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			
+			for(int i = 6; i <= 11; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}
+	}
+
     /**
      * Sets the wheels into an X formation to prevent movement.
      */
@@ -226,13 +337,16 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.resetEncoders();
     }
 
-    /** Zeroes the heading of the robot. */
-    public void zeroHeading() {
-        ahrs.reset();;
+    /** Zeroes the heading of the robot. 
+     * @return */
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+        });
     }
 
     public double getGyroValue() {
-        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -250,10 +364,18 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
     public void addVisionMeasurement(Pose2d pose, double timestamp){
         m_estimator.addVisionMeasurement(pose, timestamp);
     }
+
+    public double driveDistance(){
+        return m_frontLeft.getTotalDist();
+    }
+
+    public double getVelocity(){
+        return m_frontLeft.getState().speedMetersPerSecond;
+    }
 }

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

@@ -3,9 +3,6 @@ package frc.robot.subsystems;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -13,6 +10,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -22,12 +20,15 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
-    private SparkClosedLoopController elevatorClosedLoop;
-
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
 
+    private PIDController pidControllerUp;
+    private PIDController pidControllerDown;
+
+    private PIDController maintainPID;
+
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
@@ -41,8 +42,6 @@ public class Elevator extends SubsystemBase {
             MotorType.kBrushless
         );
 
-        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
-
         elevatorMotor1.configure(
             ElevatorConstants.motorConfig, 
             ResetMode.kResetSafeParameters, 
@@ -61,13 +60,27 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        /* 
-        velocityController = new PIDController(
-            ElevatorConstants.kVelocityControllerP, 
-            ElevatorConstants.kVelocityControllerI, 
-            ElevatorConstants.kVelocityControllerD
+        pidControllerDown = new PIDController(
+            ElevatorConstants.kDownControllerP, 
+            ElevatorConstants.kDownControllerI,
+            ElevatorConstants.kDownControllerD
         );
-        */
+        pidControllerDown.setSetpoint(0);
+
+        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
+
+        pidControllerUp = new PIDController(
+            ElevatorConstants.kUpControllerP, 
+            ElevatorConstants.kUpControllerI,
+            ElevatorConstants.kUpControllerD
+        );
+        pidControllerUp.setSetpoint(0);
+
+        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
+
+        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
+
+        maintainPID.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -78,7 +91,7 @@ public class Elevator extends SubsystemBase {
 
     @Override
     public void periodic() {
-        if (getBottomLimitSwitch()) {
+        if (!getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
     }
@@ -110,97 +123,164 @@ public class Elevator extends SubsystemBase {
      * @param speed The speed at which the elevator translates
      * @return Sets motor voltage to translate the elevator and maintain position
      */
-    public Command runManualElevator(double speed) {
+    public Command runManualElevator(DoubleSupplier speed) {
         return run(() -> {
-            elevatorMotor1.set(speed);
+            double desired = speed.getAsDouble();
+
+            if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
+                elevatorMotor1.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
+            }
+            
         });
     }
 
     /**
-     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * A command that will use the feed forward to hold up the elevator.
+     * Used for feed forward tuning.
      * 
-     * @param speed How fast the elevator moves
-     * @return Sets motor voltage to move the elevator relative to the speed parameter
-     *
-    public Command runAssistedElevator(DoubleSupplier speed) {
-        return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
+     * @return Sets motor voltage based on feed forward calculation.
+     */
+    public Command maintainPosition() {
 
-            double voltsOut = velocityController.calculate(
-                encoder.getVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(realSpeedTarget);
+        return startRun(() -> {
+            maintainPID.reset();
+            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
+        }, 
+        () -> {
 
-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(
-            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
-    }
-            */
-    
-    public Command manualControl(DoubleSupplier speed){
-        return run(() -> {
+            double maintainOutput = maintainPID.calculate(getEncoderPosition());
+
+            if(!maintainPID.atSetpoint()) 
+                elevatorMotor1.setVoltage( MathUtil.clamp(
+                    maintainOutput + feedForward.calculate(0), -2, 2)
+                );
+            else{
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            } 
             
-            elevatorMotor1.setVoltage(feedForward.calculate(0) + (speed.getAsDouble()/12));
+/* 
+            elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+                */
 
         });
+        
     }
 
     /**
      * Moves the elevator to a target destination (setpoint). 
-     * If the setpoint is 0, the elevator will creep down to hit the limit switch
      * 
      * @param setpoint Target destination of the subsystem
      * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
-    public Command goToSetpoint(double setpoint) {
-        double clampedSetpoint = MathUtil.clamp(
-            setpoint, 
-            0, 
-            ElevatorConstants.kMaxHeight
-        );
+    public Command goToSetpoint(DoubleSupplier setpoint) {
+            
+        if (setpoint.getAsDouble() == 0) {
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+                
+                }, 
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
+                    .andThen(runManualElevator(() -> -.1)
+                    .until(() -> encoder.getPosition() == 0));
 
-        return run(() -> {
-
-            elevatorClosedLoop.setReference(clampedSetpoint,
-                SparkBase.ControlType.kMAXMotionPositionControl,
-                ClosedLoopSlot.kSlot0,
-                feedForward.calculate(0)
-                );
-
-        });
-/* 
-        if (clampedSetpoint == 0) {
-            return run(() -> {
-                double voltsOut = positionController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout)
-                .andThen(Commands.either(
-                    runAssistedElevator(() -> 0),
-                    runAssistedElevator(() -> -.2), 
-                    bottomLimitSwitch::get
-                )).withTimeout(timeout);
         } else {
-            return run(() -> {
-                double voltsOut = positionController.calculate(
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        
+                },
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+        }
+        
+            
+            /*
+            elevatorMotor1.setVoltage(
+                pidController.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout);
-        }
-            */
-
+                ) + feedForward.calculate(0)
+            );
+*/
+            /* 
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }
+             
+        });*/
     }
+            
+            /* 
+            if(encoder.getPosition() >= setpoint.getAsDouble()){
+                elevatorMotor1.setVoltage(
+                    pidControllerUp.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
+                );
+            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
+                elevatorMotor1.setVoltage(
+                    pidControllerDown.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
+                );
+            }
+                */
 
     /**
      * Returns the current encoder position
@@ -211,6 +291,7 @@ public class Elevator extends SubsystemBase {
         return encoder.getPosition();
     }
 
+
     /**
      * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
      * 
@@ -219,4 +300,38 @@ public class Elevator extends SubsystemBase {
     public boolean getBottomLimitSwitch() {
         return bottomLimitSwitch.get();
     }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor1() {
+        return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
+    }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor2() {
+        return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
+    }
+
+    public double getPIDUpSetpoint() {
+        return pidControllerUp.getSetpoint();
+    }
+
+    public double getPIDUpError() {
+        return pidControllerUp.getError();
+    }
+
+    public double getPIDDownSetpoint() {
+        return pidControllerDown.getSetpoint();
+    }
+
+    public double getPIDDownError() {
+        return pidControllerDown.getError();
+    }
 }

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

@@ -56,6 +56,7 @@ public class MAXSwerveModule {
         m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
         m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
 
         m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
@@ -74,7 +75,7 @@ public class MAXSwerveModule {
     public SwerveModuleState getState() {
         // Apply chassis angular offset to the encoder position to get the position
         // relative to the chassis.
-        return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
+        return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
             new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
     }
 
@@ -86,7 +87,7 @@ public class MAXSwerveModule {
     public SwerveModulePosition getPosition() {
         // Apply chassis angular offset to the encoder position to get the position
         // relative to the chassis.
-        return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
+        return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
             new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
     }
 
@@ -107,9 +108,9 @@ public class MAXSwerveModule {
         // Command driving and turning SPARKS towards their respective setpoints.
         m_drive.setControl(
         driveVelocityRequest.withVelocity(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         ).withFeedForward(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         )
         );
 
@@ -134,8 +135,16 @@ public class MAXSwerveModule {
         return m_turningSpark.get() * RobotController.getBatteryVoltage();
     }
 
+    public TalonFX getDrivingMotor(){
+        return m_drive;
+    }
+
     /** Zeroes all the SwerveModule encoders. */
     public void resetEncoders() {
         m_drive.setPosition(0);
     }
+
+    public double getTotalDist(){
+        return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
+    }
 }

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

@@ -16,7 +16,6 @@ public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
     private DigitalInput coralBeamBreak;
-  //  private DigitalInput algaeBeamBreak; 
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -31,7 +30,6 @@ public class Manipulator extends SubsystemBase {
         );
 
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
-      //  algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
     /**
@@ -42,7 +40,7 @@ public class Manipulator extends SubsystemBase {
      */
     public Command defaultCommand() {
         return run(() -> {
-            runUntilCollected(0.1);
+            runUntilCollected(() -> 0.1);
             });
     }
 
@@ -68,12 +66,32 @@ public class Manipulator extends SubsystemBase {
      * @param coral Is the object a coral? (True = Coral, False = Algae)
      * @return Returns a command that sets the speed of the motor
      */
-    public Command runUntilCollected(double speed) {
+    public Command runUntilCollected(DoubleSupplier speed) {
         return run(() -> {
             manipulatorMotor.set(
-                speed * -1
+                speed.getAsDouble()
             );
-        }).until(() -> coralBeamBreak.get());
+        }).until(() -> !coralBeamBreak.get());
+    }
+
+    public Command retractCommand(DoubleSupplier retractSpeed){
+        return run(() -> {
+            manipulatorMotor.set(-retractSpeed.getAsDouble());
+        }
+        ).until(() -> coralBeamBreak.get());
+    }
+
+    /**
+     * Runs the manipulator in a way that will bring the coral to a reliable holding position
+     * 
+     * @return Returns a command that will position the coral to a known location
+     */
+    public Command indexCoral() {
+        return run(() -> {
+            runUntilCollected(() -> 0.5)
+                .andThen(runManipulator(() -> .1, false))
+                .until(() -> getCoralBeamBreak());
+        });
     }
 
     public boolean getCoralBeamBreak() {

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

@@ -1,42 +1,53 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.ClosedLoopSlot;
+
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.SparkAbsoluteEncoder;
-import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ManipulatorPivotConstants;
 
 public class ManipulatorPivot extends SubsystemBase {
-    protected SparkMax armMotor;
+    protected SparkMax pivotMotor;
+
+    private SparkAbsoluteEncoder encoder;
 
     private ArmFeedforward feedForward;
 
-    private SparkClosedLoopController pivotClosedLoopController;
-    private SparkAbsoluteEncoder absoluteEncoder;
+    private PIDController pidController;
 
     public ManipulatorPivot() {
-        armMotor = new SparkMax(
-            ManipulatorPivotConstants.kArmMotorID, 
+        pivotMotor = new SparkMax(
+            ManipulatorPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
 
-        armMotor.configure(
+        pivotMotor.configure(
             ManipulatorPivotConstants.motorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
+        
+        encoder = pivotMotor.getAbsoluteEncoder();
 
-        pivotClosedLoopController = armMotor.getClosedLoopController();
-        absoluteEncoder = armMotor.getAbsoluteEncoder();
+        pidController = new PIDController(
+            ManipulatorPivotConstants.kPositionalP, 
+            ManipulatorPivotConstants.kPositionalI, 
+            ManipulatorPivotConstants.kPositionalD
+        );
+        pidController.setSetpoint(0);
+
+        pidController.enableContinuousInput(0, 180);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
@@ -63,38 +74,21 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Is the motion safe
      */
     public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
+        return motionTarget > ManipulatorPivotConstants.kPivotSafeStowPosition;
     }
 
     /**
-     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
+     * Manual ManipulatorPivot command that sets the motor based on speed
      * 
-     * @param speed The velocity at which the arm rotates
-     * @return Sets motor voltage to achieve the target velocity
+     * @param speed The speed to set the motor
+     * @return A command that sets the motor speed
      */
-    /* 
-    public Command runAssistedPivot(DoubleSupplier speed) {
-        double clampedSpeed = MathUtil.clamp(
-            speed.getAsDouble(), 
-            -1, 
-            1
-        );
-
+    public Command runManualPivot(DoubleSupplier speed) {
         return run(() -> {
-            double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
-
-            double voltsOut = velocityController.calculate(
-                getEncoderVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(
-                getEncoderPosition(), 
-                getEncoderVelocity()
-            );
-
-            armMotor.setVoltage(voltsOut);
+            pivotMotor.set(speed.getAsDouble());
         });
     }
-*/
+    
     /**
      * Moves the arm to a target destination (setpoint)
      * 
@@ -102,35 +96,88 @@ public class ManipulatorPivot extends SubsystemBase {
      * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
-    public Command goToSetpoint(double setpoint) {
-        double clampedSetpoint = MathUtil.clamp(
-            setpoint, 
-            0, 
-            ManipulatorPivotConstants.kRotationLimit
-        );
+    public Command goToSetpoint(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> pidController.atSetpoint());
+    }
 
-        return run(() -> {
-            pivotClosedLoopController.setReference(clampedSetpoint,
-                SparkBase.ControlType.kMAXMotionPositionControl,
-                ClosedLoopSlot.kSlot0,
-                feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
-        });
+    public Command maintainPosition() {
+        return startRun(() -> {
+            
+        
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        pidController.getSetpoint()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            });
     }
 
     /**
-     * Returns the CANCoder's position in radians
+     * Returns the encoder's position in radians
      * 
-     * @return CANCoder's position in radians
+     * @return Encoder's position in radians
      */
     public double getEncoderPosition() {
-        return absoluteEncoder.getPosition();
+        return Units.radiansToDegrees( encoder.getPosition());
     }
     /**
-     * Returns the CANCoder's velocity in radians per second
+     * Returns the encoder's velocity in radians per second
      * 
-     * @return CANCoder's velocity in radians per second
+     * @return Encoder's velocity in radians per second
      */
     public double getEncoderVelocity() {
-        return absoluteEncoder.getVelocity();
+        return Units.radiansToDegrees(encoder.getVelocity());
+    }
+
+    public double getCGPosition(){
+        return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
+    }
+
+    public double getPivotOutput(){
+        return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
     }
 }

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

@@ -1,53 +1,101 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
-    private DoubleSubscriber blackRobotRelativePose;
+    private DoubleSubscriber blackRobotRelativeX;
+    private DoubleSubscriber blackRobotRelativeY;
+    private DoubleSubscriber blackRobotRelativeZ;
+
     private DoubleSubscriber blackClosestTag;
     private BooleanSubscriber blackTagDetected;
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orangeRobotRelativePose;
+    private DoubleSubscriber orangeRobotRelativeX;
+    private DoubleSubscriber orangeRobotRelativeY;
+    private DoubleSubscriber orangeRobotRelativeZ;
+
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
 
     private DoubleSubscriber orangeFramerate;
 
-    public Vision(){
+    private DoubleSupplier gyroAngle;
+
+    public Vision(DoubleSupplier gyroAngle){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
         NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0);
+        blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0);
+        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
+        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
+        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
+    
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
+        Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
 
-    public double getBlackGlobalPose(){
-        return blackRobotRelativePose.get();
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
+
+        Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
+
+        Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
+
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
     }
 
-    public double getBlackClosestTag(){
-        return blackClosestTag.get();
+    public Pose2d getBlackGlobalPose(){
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
+    }
+
+    public double getBlackRelativeX(){
+        return blackRobotRelativeX.get();
+    }
+
+    public double getBlackRelativeY(){
+        return blackRobotRelativeY.get();
+    }
+
+    public double getBlackRelativeZ(){
+        return blackRobotRelativeZ.get();
+    }
+
+    public int getBlackClosestTag(){
+        return (int) blackClosestTag.get();
     }
 
     public double getBlackTimeStamp(){
-        return blackRobotRelativePose.getLastChange();
+        return blackRobotRelativeX.getLastChange();
     }
 
     public boolean getBlackTagDetected(){
@@ -57,17 +105,30 @@ public class Vision{
     public double getBlackFPS(){
         return blackFramerate.get();
     }
+
+    public Pose2d getOrangeGlobalPose(){
+        return relativeToGlobalPose2d(getOrangeClosestTag(), 
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
+    }
     
-    public double getOrangeGlobalPose(){
-        return orangeRobotRelativePose.get();
+    public double getOrangeRelativeX(){
+        return orangeRobotRelativeX.get();
     }
 
-    public double getOrangeClosestTag(){
-        return orangeClosestTag.get();
+    public double getOrangeRelativeY(){
+        return orangeRobotRelativeY.get();
+    }
+
+    public double getOrangeRelativeZ(){
+        return orangeRobotRelativeZ.get();
+    }
+
+    public int getOrangeClosestTag(){
+        return (int) orangeClosestTag.get();
     }
 
     public double getOrangeTimeStamp(){
-        return orangeRobotRelativePose.getLastChange();
+        return orangeRobotRelativeX.getLastChange();
     }
 
     public boolean getOrangeTagDetected(){

src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java

@@ -1,8 +1,8 @@
 package frc.robot.subsystems.sysid;
 
-import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
 import edu.wpi.first.units.measure.MutLinearVelocity;
@@ -27,9 +27,9 @@ public class ElevatorSysID extends Elevator {
 
         appliedVoltage = Volts.mutable(0);
 
-        elevatorPosition = Meters.mutable(0);
+        elevatorPosition = Inches.mutable(0);
 
-        elevatorVelocity = MetersPerSecond.mutable(0);
+        elevatorVelocity = InchesPerSecond.mutable(0);
 
         routine = new SysIdRoutine(
             ElevatorConstants.kSysIDConfig,
@@ -41,10 +41,10 @@ public class ElevatorSysID extends Elevator {
                             elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
-                            encoder.getPosition(), Meters
+                            encoder.getPosition(), Inches
                         ))
                         .linearVelocity(elevatorVelocity.mut_replace(
-                            encoder.getVelocity(), MetersPerSecond
+                            encoder.getVelocity(), InchesPerSecond
                         ));
                 }, 
                 this
@@ -52,6 +52,11 @@ public class ElevatorSysID extends Elevator {
         );
     }
 
+    @Override
+    public void periodic() {
+
+    }
+
     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
         return routine.quasistatic(direction);
     }

src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java

@@ -34,11 +34,11 @@ public class ManipulatorPivotSysID extends ManipulatorPivot {
         routine = new SysIdRoutine(
             ManipulatorPivotConstants.kSysIDConfig,
             new SysIdRoutine.Mechanism(
-                armMotor::setVoltage, 
+                pivotMotor::setVoltage, 
                 (log) -> {
                     log.motor("armMotor")
                         .voltage(appliedVoltage.mut_replace(
-                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                            pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .angularPosition(pivotPosition.mut_replace(
                             getEncoderPosition(), Radians

vendordeps/AdvantageKit.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.1",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+    ],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.1",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}