tweaks to L1 and auto align good when PIs are on

.OutlineViewer/outlineviewer.json

@@ -1,13 +1,58 @@
 {
+  "Clients": {
+    "open": true
+  },
+  "Connections": {
+    "open": true
+  },
   "NetworkTables Settings": {
     "mode": "Client (NT4)"
   },
+  "client@1": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "client@2": {
+    "open": true
+  },
+  "client@3": {
+    "open": true
+  },
+  "client@4": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "client@5": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@2": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@3": {
+    "open": true
+  },
+  "shuffleboard@1": {
+    "open": true
+  },
   "transitory": {
     "Shuffleboard": {
       "Sensors Tab": {
         "open": true
       },
       "open": true
+    },
+    "orange_Fiducial": {
+      "open": true
     }
   }
 }

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

@@ -4,10 +4,16 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "Start to 30 Right"
+            "pathName": "Start to H"
           }
         },
         {
@@ -19,25 +25,7 @@
         {
           "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"
+            "pathName": "H Backup"
           }
         }
       ]

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

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

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

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

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

@@ -10,11 +10,35 @@
             "pathName": "Start to 30 Right"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Approach"
+          }
+        },
         {
           "type": "named",
           "data": {
             "name": "Shoot Coral L4"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
         }
       ]
     }

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.3072345890410957,
-        "y": 7.135316780821918
+        "x": 1.1987704918032787,
+        "y": 7.189754098360656
       },
       "prevControl": {
-        "x": 2.1636986301369863,
-        "y": 6.188698630136986
+        "x": 2.055234532899169,
+        "y": 6.243135947675724
       },
       "nextControl": null,
       "isLocked": false,
@@ -31,19 +31,12 @@
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "HP Pickup",
-      "waypointRelativePos": 0.20476190476190542,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -52,10 +45,10 @@
     "rotation": -53.97262661489646
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -120.06858282186238
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -45,10 +45,10 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -57,10 +57,10 @@
     "rotation": -53.98486432191523
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -59.99999999999999
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/E to HP.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.958476027397261,
+        "y": 2.837970890410959
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.619606164383562,
+        "y": 1.455607876712329
+      },
+      "isLocked": false,
+      "linkedName": "E"
+    },
+    {
+      "anchor": {
+        "x": 1.1569777397260272,
+        "y": 1.0198630136986298
+      },
+      "prevControl": {
+        "x": 2.7647260273972605,
+        "y": 1.485659246575342
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right HP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.22857142857142831,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "HP Pickup"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "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.25643716352937
+  },
+  "useDefaultConstraints": true
+}

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.289041095890411,
-        "y": 2.9732020547945206
+        "x": 5.7,
+        "y": 4.3
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.950171232876712,
-        "y": 1.6208904109589037
+        "x": 6.053790983606557,
+        "y": 4.312704918032787
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "H"
     },
     {
       "anchor": {
-        "x": 1.4424657534246574,
-        "y": 0.7944777397260271
+        "x": 6.389446721311475,
+        "y": 4.3
       },
       "prevControl": {
-        "x": 2.389083904109589,
-        "y": 2.5374571917808213
+        "x": 6.1394487099079695,
+        "y": 4.300997143065429
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 54.162347045721745
+    "rotation": 180.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 120.96375653207336
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.3072345890410957,
-        "y": 7.135316780821918
+        "x": 1.1987704918032787,
+        "y": 7.189754098360656
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.5994434931506847,
-        "y": 6.909931506849315
+        "x": 2.490979395912868,
+        "y": 6.964368824388053
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 3.973766447368421,
-        "y": 5.246957236842105
+        "x": 3.6202868852459016,
+        "y": 5.031967213114754
       },
       "prevControl": {
-        "x": 3.6582270638067773,
-        "y": 5.772856209444845
+        "x": 3.304747501684258,
+        "y": 5.557866185717494
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "L"
     }
   ],
   "rotationTargets": [],
@@ -34,16 +34,21 @@
   "eventMarkers": [
     {
       "name": "Lift L4",
-      "waypointRelativePos": 0.5,
+      "waypointRelativePos": 0.0,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -52,7 +57,7 @@
     "rotation": -59.69923999693802
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -53.97262661489646

src/main/deploy/pathplanner/paths/HP to D.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.187029109589041,
+        "y": 0.9747859589041092
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.268878424657535,
+        "y": 1.0949914383561645
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.011857876712329,
+        "y": 2.837970890410959
+      },
+      "prevControl": {
+        "x": 3.3657534246575342,
+        "y": 1.9364297945205475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.27142857142857096,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 58.57043438516136
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 54.24611274556325
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to K.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1987704918032787,
+        "y": 7.189754098360656
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7142418032786886,
+        "y": 6.638319672131147
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.9679303278688525,
+        "y": 5.211782786885246
+      },
+      "prevControl": {
+        "x": 3.2246926229508195,
+        "y": 6.230737704918033
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.49491285058726
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.286577868852459,
+        "y": 5.955020491803278
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.166700819672132,
+        "y": 5.679303278688525
+      },
+      "isLocked": false,
+      "linkedName": "Before J"
+    },
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
+      },
+      "prevControl": {
+        "x": 5.058811475409836,
+        "y": 5.523463114754099
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.11855393929902,
+        "y": 5.503390382532524
+      },
+      "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": 3.5,
+    "maxAcceleration": 1.75,
+    "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": -119.71497744813712
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/K Approach.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.35655737704918,
+        "y": 5.78719262295082
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.402629510813402,
+        "y": 5.541474576217554
+      },
+      "isLocked": false,
+      "linkedName": "Before K"
+    },
+    {
+      "anchor": {
+        "x": 3.6322745901639335,
+        "y": 5.019979508196721
+      },
+      "prevControl": {
+        "x": 3.596311475409835,
+        "y": 5.271721311475409
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.94539590092286
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6202868852459016,
+        "y": 5.031967213114754
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.4877871502451065,
+        "y": 5.243966789116026
+      },
+      "isLocked": false,
+      "linkedName": "L"
+    },
+    {
+      "anchor": {
+        "x": 1.1987704918032787,
+        "y": 7.189754098360656
+      },
+      "prevControl": {
+        "x": 1.4025614754098363,
+        "y": 6.914036885245903
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": false
+}

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.602996575342465,
-        "y": 2.0115582191780823
+        "x": 7.572945205479453,
+        "y": 0.4939640410958907
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.491095890410959,
-        "y": 1.9965325342465756
+        "x": 6.461044520547946,
+        "y": 0.4789383561643841
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.289041095890411,
-        "y": 2.9882277397260277
+        "x": 4.958476027397261,
+        "y": 2.837970890410959
       },
       "prevControl": {
-        "x": 6.130479452054795,
-        "y": 2.11673801369863
+        "x": 5.799914383561644,
+        "y": 1.9664811643835614
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "E"
     }
   ],
   "rotationTargets": [],
@@ -34,16 +34,28 @@
   "eventMarkers": [
     {
       "name": "Lift L4",
-      "waypointRelativePos": 0.7142857142857124,
+      "waypointRelativePos": 0.4261904761904757,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.587970890410959,
-        "y": 6.143621575342466
+        "x": 7.150967037968244,
+        "y": 7.5521014571037055
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.385916095890411,
-        "y": 6.158647260273973
+        "x": 5.916233431410867,
+        "y": 6.533146539070919
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.988527397260274,
-        "y": 5.227054794520548
+        "x": 4.974897540983607,
+        "y": 5.235758196721312
       },
       "prevControl": {
-        "x": 5.574529109589041,
-        "y": 6.098544520547945
+        "x": 5.560899253312374,
+        "y": 6.107247922748709
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "J"
     }
   ],
   "rotationTargets": [],
@@ -34,25 +34,30 @@
   "eventMarkers": [
     {
       "name": "Lift L4",
-      "waypointRelativePos": 0.4547619047619047,
+      "waypointRelativePos": 0,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -121.60750224624898
+    "rotation": -119.71497744813712
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -90.0

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

@@ -0,0 +1,54 @@
+{
+  "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": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "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/settings.json

@@ -2,12 +2,14 @@
   "robotWidth": 0.8763,
   "robotLength": 0.8763,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Left Paths"
+  ],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
-  "defaultMaxAccel": 4.0,
+  "defaultMaxVel": 3.5,
+  "defaultMaxAccel": 1.75,
   "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

@@ -8,18 +8,21 @@ import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.constants.ElevatorConstants.ElevatorPositions;
+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.commands.PathPlannerAuto;
 
-import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -27,9 +30,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
 	private ClimberPivot climberPivot;
@@ -41,6 +42,8 @@ public class RobotContainer {
 	private Elevator elevator;
 	//private ElevatorSysID elevator;
 
+	//private Indexer indexer;
+
 	private Manipulator manipulator;
 
 	private ManipulatorPivot manipulatorPivot;
@@ -49,32 +52,40 @@ public class RobotContainer {
 	private CommandXboxController operator;
 
 	private SendableChooser<Command> autoChooser;
-	private Vision vision;
+
+	private IntSupplier closestTag;
 
 	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
 
-		manipulator = new Manipulator();
-
-		//vision = new Vision(drivetrain::getGyroValue);
 		drivetrain = new Drivetrain();
 
-		elevator = new Elevator(manipulator::getAlgaePhotoswitch);
+		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
 
+		//indexer = new Indexer();
+
+		manipulator = new Manipulator();
+
 		manipulatorPivot = new ManipulatorPivot();
 
+		configureNamedCommands();
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
 		autoChooser = AutoBuilder.buildAutoChooser();
+		autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
+		autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
+		autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
+
+		closestTag = drivetrain::getClosestTag;
 
 		configureButtonBindings();
 		//elevatorSysIDBindings();
-
-		configureNamedCommands();
+		//elevatorOnlyBindings();
 
 		configureShuffleboard();
 	}
@@ -91,31 +102,38 @@ public class RobotContainer {
 	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,
+				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
 				() -> true
 			)
 		);
 		
+		elevator.setDefaultCommand(
+			elevator.maintainPosition()
+		);
+
+		//indexer.setDefaultCommand(
+		//	indexer.runIndexer(() -> 0)
+		//);
+		
+		
 		manipulatorPivot.setDefaultCommand(
 			manipulatorPivot.maintainPosition()
 		);
 		
 
 		manipulator.setDefaultCommand(
-			manipulator.runUntilCollected(
-				() -> 0
-			)
+			manipulator.runManipulator(() -> 0.0, false)
 		);
 
 		//Driver inputs
@@ -125,83 +143,212 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(() -> 1, true)
+			manipulator.runManipulator(() -> 0.35, true)
 		);
 
 		driver.leftTrigger().whileTrue( 
 			manipulator.runUntilCollected(() -> 0.75)
+			//.alongWith(indexer.runIndexer(() -> .75))
+			.until(() -> manipulator.getCoralBeamBreak() == false)
+			.andThen(manipulator.retractCommand(() -> .1))
 		);
 
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
-		
-		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
-		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
 
-		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
-		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
+		driver.y().whileTrue(drivetrain.zeroHeading());
 		
+		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
+
+		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
+
+		driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
+		
+		driver.start().whileTrue(drivetrain.resetToVision());
+	
+		driver.rightBumper().whileTrue(
+				drivetrain.resetToVision().andThen(
+				drivetrain.goToPose(
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
+					() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+				))
+			);
+
+		driver.leftBumper().whileTrue(
+			drivetrain.resetToVision().andThen(
+			drivetrain.goToPose(
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+			))
+		);
+
 		//Operator inputs
 		operator.povUp().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL4, 
+				ElevatorConstants.kL4Position, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL3, 
+				ElevatorConstants.kL3Position, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL2, 
+				ElevatorConstants.kL2Position, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL1, 
+				ElevatorConstants.kL1Position, 
 				ManipulatorPivotConstants.kL1Position
 			)
 		);
 
+		operator.back().onTrue(elevator.homeCommand());
+
+		operator.start().toggleOnTrue(
+			climberPivot.runPivot(() -> -operator.getRightY()*0.5)
+			.alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+
 		operator.a().onTrue(
-			coralIntakeRoutine()
+			safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
 		);
 
 		operator.x().onTrue(
-			algaeIntakeRoutine(true)
+			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
+				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
-			algaeIntakeRoutine(false)
+			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
+				.until(() -> driver.a().getAsBoolean())
 		);
 
-		operator.y().onTrue(
-			elevator.setTargetPosition(ElevatorPositions.kL2)
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
+			.until(() -> driver.a().getAsBoolean())
 		);
 
-		new Trigger(() -> Math.abs(MathUtil.applyDeadband(operator.getLeftY(), .05)) > .05).onTrue(
-			elevator.runManualElevator(operator::getLeftY)
-		);
-	
+		operator.rightTrigger().onTrue(shootAlgae());
 
 	}
-
 	private void configureNamedCommands() {
-		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+		//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", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 0.4, 
+					true
+				).withTimeout(
+					0.5
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0.0, 
+						false
+					).withTimeout(
+						0.01
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Coral L4 Fast", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 1,
+					true
+				).andThen(
+					manipulator.runManipulator(
+						() -> 1, 
+						true
+					).withTimeout(
+						0.125
+					)
+				).withTimeout(
+					3
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0, 
+						true
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Collect Coral", 
+			manipulator.runUntilCollected(
+				() -> 0.30
+			).andThen(
+				manipulator.runManipulator(
+					() -> 0, 
+					false
+				).withTimeout(
+					0.01
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Lift L4", 
+			safeMoveManipulator(
+				ElevatorConstants.kL4Position, 
+				ManipulatorPivotConstants.kL4Position
+			).andThen(
+				elevator.maintainPosition()
+					.withTimeout(
+						0.1
+					), 
+				manipulatorPivot.maintainPosition()
+					.withTimeout(
+						0.01
+					)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"HP Pickup", 
+			safeMoveManipulator(
+				ElevatorConstants.kCoralIntakePosition, 
+				ManipulatorPivotConstants.kStartingPosition
+			)
+		);
 	}
 
 	//creates tabs and transforms them on the shuffleboard
     private void configureShuffleboard() {
 		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+		ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
 
 		Shuffleboard.selectTab(OIConstants.kAutoTab);
 
@@ -213,7 +360,7 @@ public class RobotContainer {
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
-			.withWidget(BuiltInWidgets.kGraph);
+			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
@@ -246,19 +393,68 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
+		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
 			.withSize(1, 1)
 			.withPosition(5, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
-		sensorTab.addDouble("Elevator error", elevator::getPIDError)
+		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("velocity", drivetrain::getVelocity);
+
+		sensorTab.addDouble("heading", drivetrain::getHeading);
+
+
+		//sensorTab.add("odometry", drivetrain::getPose);
+
+		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
+			.withSize(1,1).withPosition(1,1);
+		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
+			.withSize(1,1).withPosition(2,1);
+		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
+			.withSize(1,1).withPosition(3,1);
+		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
+			.withSize(1,1).withPosition(4,1);
+		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
+			.withSize(1,1).withPosition(5,1);
+		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
+			.withSize(1,1).withPosition(6,1);
+
+		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
+			.withSize(1,1).withPosition(1,2);
+		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
+			.withSize(1,1).withPosition(2,2);
+		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
+			.withSize(1,1).withPosition(3,2);
+		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
+			.withSize(1,1).withPosition(4,2);
+		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
+			.withSize(1,1).withPosition(5,2);
+		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
+			.withSize(1,1).withPosition(6,2);
+
+		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
+			.withSize(2,1).withPosition(4,4);
+
+		apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
+			.withSize(2, 1).withPosition(4, 2);
+		apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
+			.withSize(2, 1).withPosition(6, 2);
 
 
  	}
@@ -274,7 +470,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
-			ElevatorPositions.kCoralIntake, 
+			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> .5));
@@ -289,7 +485,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
-			l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae, 
+			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> 1));
@@ -302,10 +498,10 @@ public class RobotContainer {
 	 * @param armPosition The target rotation of the arm
 	 * @return Moves the elevator and arm to the setpoints using the most efficient path
 	 */
-	private Command moveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
+	private Command moveManipulator(double elevatorPosition, double armPosition) {
 		// If the elevator current and target positions are above the brace, or the arm current and target position is in
 		// front of the brace, move together
-		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
 		// 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
@@ -334,7 +530,7 @@ public class RobotContainer {
 	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
 	 * @return Moves the elevator and arm to the setpoints
 	 */
-	private Command moveManipulatorUtil(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
 		
 		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
@@ -346,16 +542,13 @@ public class RobotContainer {
 		
 		return Commands.either(
 			Commands.either(
-				elevator.setTargetPosition(elevatorPosition).andThen(
-					new WaitUntilCommand(elevator::atSetpoint),
-					manipulatorPivot.goToSetpoint(() -> armPosition)
-				),
-				elevator.setTargetPosition(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.setTargetPosition(elevatorPosition)), 
-				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
@@ -363,24 +556,18 @@ public class RobotContainer {
 	}
 
 	@SuppressWarnings("unused")
-	private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
+	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.setTargetPosition(elevatorPosition),
-				new WaitUntilCommand(elevator::atSetpoint),
+				elevator.goToSetpoint(() -> elevatorPosition),
 				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.setTargetPosition(elevatorPosition),
-				new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
-				Commands.parallel(
-					manipulatorPivot.goToSetpoint(() -> armPosition), 
-					elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
-				)
-			);
+				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}
@@ -392,15 +579,35 @@ 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(ElevatorPositions elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+	private Command safeMoveManipulator(double elevatorPosition, double 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()));
+	}
+
+	private Command shootAlgae(){
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
+		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition()));
 	}
 
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
-		return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
+		return moveManipulatorUtil(0, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
 	}
 }

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

@@ -11,25 +11,30 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 4;
+    public static final double kMaxSpeedMetersPerSecond = 5;
     public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     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 kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
 
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
     // TODO This is a constant being managed like a static rewriteable variable
     public static RobotConfig kRobotConfig;
 
     public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
-        new PIDConstants(kPXController, 0, 0),
-        new PIDConstants(kPYController, 0, 0)
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
     );
 
     static {

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,6 +22,13 @@ 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;
+    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 = Math.PI;
     public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
     public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
@@ -50,6 +57,12 @@ public class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
+    public static final double kHeadingP = 0.1;
+
+    public static final double kXTranslationP = 0.5;
+    public static final double kYTranslationP = 0.5;
+
+
     // 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,34 +11,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
-    public enum ElevatorControlMode {
-        kPID,
-        kManualMaintain
-    }
-
-    public enum ElevatorPositions {
-        kStartingPosition(0),
-        kCoralIntake(0),
-        kL1(0),
-        kL2(14.5),
-        kL2Algae(22),
-        kL3(29),
-        kL3Algae(39),
-        kL4(53),
-        kL4Transition(40),
-        kProcessor(4);
-
-        private double position;
-
-        private ElevatorPositions(double position) {
-            this.position = position;
-        }
-
-        public double getPosition() {
-            return position;
-        }
-    }
-
     public static final int kElevatorMotor1ID = 8;
     public static final int kElevatorMotor2ID = 6;
 
@@ -57,6 +29,8 @@ public class ElevatorConstants {
     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 kMaintainP = 3;
      
     public static final double kAllowedError = 1;
 
@@ -64,16 +38,23 @@ public class ElevatorConstants {
     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 kFeedForwardAlgaeHeldS = 0;
-    public static final double kFeedForwardAlgaeHeldG = 0;
-    public static final double kFeedForwardAlgaeHeldV = 0;
-
     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 = 17;
+    public static final double kL2Position = 11;
+    public static final double kL3Position = 27;
+    public static final double kL4Position = 50.5;
+    public static final double kL4TransitionPosition = 40.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 = 0;
-    public static final double kMaxHeight = 47.5; //actual is 53
+    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 = .25;

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

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IndexerConstants {
+    public static final int kIndexerMotorID = 16;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(30)
+        .idleMode(IdleMode.kBrake);
+    };
+}

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

@@ -1,12 +1,16 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
     public static final int kManipulatorMotorID = 12;
-
     public static final int kCoralBeamBreakID = 2;
-    public static final int kAlgaePhotoswitchID = 3;
     
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(40)
+        .idleMode(IdleMode.kBrake);
+    };
 }

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

@@ -20,35 +20,38 @@ public class ManipulatorPivotConstants {
 
     public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 4;
+    public static final double kPositionalP = 4.5;
     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.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+90);
 
     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.7815;
+    public static final double kEncoderOffset = 0.04500000178813934;
 
-    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(60.0);
-    public static final double kL3Position = Units.degreesToRadians(60.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 kNetPosition = Units.degreesToRadians(175.0);
+    public static final double kStartingPosition = Units.degreesToRadians(90);
+    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL1Position = Units.degreesToRadians(246);
+    public static final double kL2Position = Units.degreesToRadians(22.0+90);
+    public static final double kL3Position = Units.degreesToRadians(22.0+90);
+    public static final double kL4Position = Units.degreesToRadians(45.0+90);
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
+    public static final double kNetPosition = Units.degreesToRadians(175.0+90);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
+
+    public static final double kBargeShotPosition = Units.degreesToRadians(222);
     /**The forward rotation limit of the pivot */
-    public static final double kRotationLimit = Units.degreesToRadians(175.0);
+    public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
 
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;

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;
 
@@ -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/OIConstants.java

@@ -4,8 +4,9 @@ public class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kOperatorControllerPort = 1;
     
-    public static final double kDriveDeadband = 0.05;
+    public static final double kDriveDeadband = Math.pow(0.05, 3);
 
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
+    public static final String kApriltagTab = "Apriltag Tab";
 }

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

@@ -1,6 +1,7 @@
 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.
@@ -28,4 +29,36 @@ public class VisionConstants {
         {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 = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {13.570, 2.816, 13.858, 2.970},//6
+        {14.373, 3.862, 14.385, 4.194},
+        {13.858, 5.032, 13.558, 5.227},
+        {12.575, 5.227, 12.287, 5.056},
+        {11.772, 4.169, 11.772, 3.845},
+        {12.287, 2.982, 12.587, 2.826},//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.891, 5.284},//4.991, 5.246},
+        {5.789, 3.862, 5.789, 4.194},
+        {4.993, 2.816, 5.272, 2.996}
+    };
+
+    public static final double latencyFudge = 0.0;
+
 }

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -30,9 +32,9 @@ public class ClimberPivot extends SubsystemBase {
         neoEncoder = pivotMotor.getEncoder();
     }
 
-    public Command runPivot(double speed) {
+    public Command runPivot(DoubleSupplier speed) {
         return run(() -> {
-            pivotMotor.set(speed);
+            pivotMotor.set(speed.getAsDouble());
         });
     }
 

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,29 +4,45 @@
 
 package frc.robot.subsystems;
 
+import java.io.File;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
+import org.littletonrobotics.junction.Logger;
+
+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.VecBuilder;
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+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;
+import edu.wpi.first.math.geometry.Pose3d;
 
 public class Drivetrain extends SubsystemBase {
     // Create MAXSwerveModules
@@ -40,8 +56,22 @@ public class Drivetrain extends SubsystemBase {
 
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
+    
+    private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
 
-    private Vision vision;
+    public Orchestra m_orchestra = new Orchestra();
+    private Timer musicTimer = new Timer();
+
+    private ProfiledPIDController pidHeading;
+    private PIDController pidTranslationX;
+    private PIDController pidTranslationY;
+
+    private HolonomicDriveController driveController;
+
+    public Vision vision;
+
+    public Pose2d orangePose2d;
+    public Pose2d blackPose2d;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -73,16 +103,29 @@ public class Drivetrain extends SubsystemBase {
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(gyro.getAngle()),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
             },
-            new Pose2d()
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
+        pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
+        pidHeading.setTolerance(Units.degreesToRadians(3));
+        pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
+
+        pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
+        pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
+        pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
+        pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
+
+        driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
+
         AutoBuilder.configure(
             this::getPose, 
             this::resetOdometry, 
@@ -99,6 +142,25 @@ 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();
+
+        vision = new Vision();
+        orangePose2d = new Pose2d();
+        blackPose2d = new Pose2d();
     }
 
     @Override
@@ -113,23 +175,53 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
         });
 
+        gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
 
-        // 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());
-        }
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
 
-        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(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
+            if(vision.getOrangeDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
+            }
+
+            // if the detected tags match your alliances reef tags use their pose estimates  
+            if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
+                orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
+
+            }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
+                orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
+            } 
         }
-        */
+        Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
+
+
+        if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
+            if(vision.getBlackDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
+            }
+
+            if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
+
+            }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
+            }
+        }
+        Logger.recordOutput("black pose", new Pose3d(blackPose2d));
         
+        Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
+        
+        if(musicTimer.get()>10){
+            if (m_orchestra.isPlaying()) {
+                m_orchestra.stop();
+            }
+            musicTimer.stop();
+            musicTimer.reset();
+        }
         
     }
 
@@ -165,7 +257,15 @@ 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
         );
     }
@@ -173,12 +273,20 @@ public class Drivetrain extends SubsystemBase {
     public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
             BooleanSupplier fieldRelative) {
         return run(() -> {
-            drive(
-                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
-                fieldRelative.getAsBoolean()
-            );
+            if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
+                drive(
+                    -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
+                    fieldRelative.getAsBoolean()
+                );
+            }else{
+                drive(
+                    MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                    -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
+                    fieldRelative.getAsBoolean()
+            );}
         });
     }
 
@@ -200,7 +308,7 @@ public class Drivetrain extends SubsystemBase {
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(getGyroValue()))
+                        new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -216,6 +324,71 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
+        return startRun(() -> {
+                pidTranslationX.reset();
+                pidTranslationY.reset();
+                pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
+            },
+            () -> {
+                    ChassisSpeeds controlEffort = driveController.calculate(
+                    m_estimator.getEstimatedPosition(),
+                    new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
+                    headingSetpoint.get()),
+                    0,
+                    headingSetpoint.get()
+                );
+
+                double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecond) {
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed;
+                    controlEffort.vxMetersPerSecond *= mul;
+                    controlEffort.vyMetersPerSecond *= mul;
+                }
+
+                driveWithChassisSpeeds(controlEffort);
+
+                Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
+                    new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
+                    headingSetpoint.get())));
+            });
+    }
+
+    public int getClosestTag(){
+
+		if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
+			int closestTag = 17;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
+			+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
+			
+			for(int i = 17; i <= 22; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
+				+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}else{
+			int closestTag = 6;
+			double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
+			+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
+			
+			for(int i = 6; i <= 11; ++i){
+				double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
+				+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}
+	}
+
     /**
      * Sets the wheels into an X formation to prevent movement.
      */
@@ -248,9 +421,13 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.resetEncoders();
     }
 
-    /** Zeroes the heading of the robot. */
-    public void zeroHeading() {
-        gyro.reset();
+    /** Zeroes the heading of the robot. 
+     * @return */
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+            m_estimator.resetRotation(new Rotation2d(0));
+        });
     }
 
     public double getGyroValue() {
@@ -263,7 +440,11 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
+        return m_estimator.getEstimatedPosition().getRotation().getDegrees();
+    }
+
+    public TimeInterpolatableBuffer<Double> getGyroBuffer(){
+        return gyroBuffer;
     }
 
     /**
@@ -275,7 +456,17 @@ public class Drivetrain extends SubsystemBase {
         return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
-    public void addVisionMeasurement(Pose2d pose, double timestamp){
-        m_estimator.addVisionMeasurement(pose, timestamp);
+    public double getVelocity(){
+        return m_frontLeft.getState().speedMetersPerSecond;
+    }
+
+    public Command resetToVision(){
+        return runOnce(() -> {
+            if(vision.getOrangeTagDetected()){
+                m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }else if(vision.getBlackTagDetected()){
+                m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }
+        });
     }
 }

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

@@ -1,6 +1,5 @@
 package frc.robot.subsystems;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
@@ -16,8 +15,6 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.ElevatorConstants.ElevatorControlMode;
-import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 
 public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
@@ -25,20 +22,16 @@ public class Elevator extends SubsystemBase {
 
     protected RelativeEncoder encoder;
 
-    private BooleanSupplier hasAlgae;
-
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
+    private PIDController pidControllerUp;
+    private PIDController pidControllerDown;
+
+    private PIDController maintainPID;
 
     private ElevatorFeedforward feedForward;
-    private ElevatorFeedforward algaeHeldFeedForward;
-
-    private ElevatorControlMode mode;
-
-    private ElevatorPositions currentTargetPosition;
     
-    public Elevator(BooleanSupplier hasAlgae) {
+    public Elevator() {
         elevatorMotor1 = new SparkMax(
             ElevatorConstants.kElevatorMotor1ID, 
             MotorType.kBrushless
@@ -63,36 +56,37 @@ public class Elevator extends SubsystemBase {
 
         encoder = elevatorMotor1.getEncoder();
 
-        this.hasAlgae = hasAlgae;
-
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new PIDController(
+        pidControllerDown = new PIDController(
             ElevatorConstants.kDownControllerP, 
             ElevatorConstants.kDownControllerI,
             ElevatorConstants.kDownControllerD
         );
-        pidController.setSetpoint(0);
+        pidControllerDown.setSetpoint(0);
 
-        pidController.setTolerance(ElevatorConstants.kAllowedError);
+        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, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
-
-        algaeHeldFeedForward = new ElevatorFeedforward(
-            ElevatorConstants.kFeedForwardAlgaeHeldS, 
-            ElevatorConstants.kFeedForwardAlgaeHeldG, 
-            ElevatorConstants.kFeedForwardAlgaeHeldV
-        );
-
-        mode = ElevatorControlMode.kPID;
-
-        currentTargetPosition = ElevatorPositions.kCoralIntake;
     }
 
     @Override
@@ -100,27 +94,6 @@ public class Elevator extends SubsystemBase {
         if (!getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
-
-        if(mode == ElevatorControlMode.kPID) {
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        currentTargetPosition.getPosition()
-                    ) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0))
-                );
-            } else {
-                if (hasAlgae.getAsBoolean()) {
-                    elevatorMotor1.setVoltage(
-                        algaeHeldFeedForward.calculate(0)
-                    );
-                } else {
-                    elevatorMotor1.setVoltage(
-                        feedForward.calculate(0)
-                    );
-                }
-            }
-        }
     }
 
     /**
@@ -151,10 +124,7 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage to translate the elevator and maintain position
      */
     public Command runManualElevator(DoubleSupplier speed) {
-        return startRun(() -> {
-            mode = ElevatorControlMode.kManualMaintain;
-        },
-        () -> {
+        return run(() -> {
             double desired = speed.getAsDouble();
 
             if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
@@ -162,34 +132,164 @@ public class Elevator extends SubsystemBase {
                     speed.getAsDouble()
                 );
             } else {
-                if (hasAlgae.getAsBoolean()) {
-                    elevatorMotor1.setVoltage(
-                        algaeHeldFeedForward.calculate(0)
-                    );
-                } else {
-                    elevatorMotor1.setVoltage(
-                        feedForward.calculate(0)
-                    );
-                }
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
             }
+            
         });
     }
 
-    public Command setTargetPosition(ElevatorPositions position) {
-        return runOnce(() -> {
-            if (mode == ElevatorControlMode.kManualMaintain) {
-                pidController.reset();
-                mode = ElevatorControlMode.kPID;
-            }
+    /**
+     * A command that will use the feed forward to hold up the elevator.
+     * Used for feed forward tuning.
+     * 
+     * @return Sets motor voltage based on feed forward calculation.
+     */
+    public Command maintainPosition() {
+
+        return startRun(() -> {
+            maintainPID.reset();
+            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
+        }, 
+        () -> {
+
+            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)
+                );
+                */
 
-            currentTargetPosition = position;
         });
+        
     }
 
-    public boolean atSetpoint() {
-        return pidController.atSetpoint();
+    public Command homeCommand(){
+        return run(() -> {
+            elevatorMotor1.setVoltage(0.5);
+        })
+        .until(() -> elevatorMotor1.getOutputCurrent() > 5)
+        .andThen(run(() -> encoder.setPosition(0)));
     }
 
+    /**
+     * Moves the elevator to a target destination (setpoint). 
+     * 
+     * @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(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(() -> -.5)
+                    .until(() -> encoder.getPosition() == 0));
+
+        } else {
+            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)
+            );
+*/
+            /* 
+            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
      * 
@@ -227,11 +327,19 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDSetpoint() {
-        return pidController.getSetpoint();
+    public double getPIDUpSetpoint() {
+        return pidControllerUp.getSetpoint();
     }
 
-    public double getPIDError() {
-        return pidController.getError();
+    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,13 @@ 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);
     }
+
 }

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

@@ -10,13 +10,15 @@ 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.IndexerConstants;
 import frc.robot.constants.ManipulatorConstants;
 
 public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
+    private SparkMax indexerMotor;
+
     private DigitalInput coralBeamBreak;
-    private DigitalInput algaePhotoswitch;
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -30,8 +32,18 @@ public class Manipulator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
+        indexerMotor = new SparkMax(
+            IndexerConstants.kIndexerMotorID,
+            MotorType.kBrushless
+        );
+
+        indexerMotor.configure(
+            IndexerConstants.motorConfig,
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
-        algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID);
     }
 
     /**
@@ -58,6 +70,8 @@ public class Manipulator extends SubsystemBase {
             manipulatorMotor.set(
                 coral ? speed.getAsDouble() : speed.getAsDouble() * -1
             );
+
+            indexerMotor.set(0);
         });
     }
 
@@ -73,7 +87,32 @@ public class Manipulator extends SubsystemBase {
             manipulatorMotor.set(
                 speed.getAsDouble()
             );
-        }).until(() -> !coralBeamBreak.get());
+
+            indexerMotor.set(1);
+
+        }).unless(() -> !coralBeamBreak.get())
+        .until(() -> !coralBeamBreak.get());
+        /*
+        return run(() -> {
+            if(getCoralBeamBreak()) {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            }
+        */
+    }
+
+    public Command retractCommand(DoubleSupplier retractSpeed){
+        return run(() -> {
+            manipulatorMotor.set(-retractSpeed.getAsDouble());
+
+            indexerMotor.set(0);
+        }
+        ).until(() -> coralBeamBreak.get());
     }
 
     /**
@@ -92,8 +131,4 @@ public class Manipulator extends SubsystemBase {
     public boolean getCoralBeamBreak() {
         return coralBeamBreak.get();
     }
-
-    public boolean getAlgaePhotoswitch() {
-        return algaePhotoswitch.get();
-    }
 }

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

@@ -9,7 +9,6 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 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;
@@ -47,7 +46,7 @@ public class ManipulatorPivot extends SubsystemBase {
         );
         pidController.setSetpoint(0);
 
-        pidController.disableContinuousInput();
+        pidController.enableContinuousInput(0, 280);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,

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

@@ -1,94 +1,125 @@
 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.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.util.Units;
 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 edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
-    private DoubleSubscriber blackRobotRelativeX;
-    private DoubleSubscriber blackRobotRelativeY;
-    private DoubleSubscriber blackRobotRelativeZ;
+    private NetworkTable blackVisionTable;
+
+    private DoubleSubscriber black_tx;
+    private DoubleSubscriber black_ty;
+    private DoubleSubscriber black_dist;
 
     private DoubleSubscriber blackClosestTag;
     private BooleanSubscriber blackTagDetected;
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orangeRobotRelativeX;
-    private DoubleSubscriber orangeRobotRelativeY;
-    private DoubleSubscriber orangeRobotRelativeZ;
+    private NetworkTable orangeVisionTable;
+
+    private DoubleSubscriber orange_tx;
+    private DoubleSubscriber orange_ty;
+    private DoubleSubscriber orange_dist;
 
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
 
     private DoubleSubscriber orangeFramerate;
 
-    private DoubleSupplier gyroAngle;
+    private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 14.0-1.05, 7.308+2.75};
+    private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
 
-    public Vision(DoubleSupplier gyroAngle){
+    public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
-        NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
-        NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
+        blackVisionTable = inst.getTable("black_Fiducial");
+        orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
-        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
-        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+        black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
 
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
-        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
-        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+        orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        orange_dist = orangeVisionTable.getDoubleTopic("totalDist").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, Rotation2d gyroAngle){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
         Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
             new Rotation2d());
 
-        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
 
         Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
 
         Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
 
-        return new Pose2d(globalPose.getTranslation(), gyroAngle);
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
     }
 
-    public Pose2d getBlackGlobalPose(){
-        return relativeToGlobalPose2d(getBlackClosestTag(), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+    public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
+
+       // System.out.println(gyroBuffer.getSample(timestamp));
+
+        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
+
+        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
+
+        Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
+        Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
+            new Rotation2d());
+
+        Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
+            .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
+            .getTranslation();
+        Pose2d robotPose = new Pose2d(
+            fieldToCameraTranslation,
+            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
+
+        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
+
+        return robotPose;
     }
 
-    public double getBlackRelativeX(){
-        return blackRobotRelativeX.get();
+    public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
+            getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
     }
 
-    public double getBlackRelativeY(){
-        return blackRobotRelativeY.get();
+    public double getBlackTX(){
+        return black_tx.get();
     }
 
-    public double getBlackRelativeZ(){
-        return blackRobotRelativeZ.get();
+    public double getBlackTY(){
+        return black_ty.get();
+    }
+
+    public double getBlackDist(){
+        return black_dist.get();
     }
 
     public int getBlackClosestTag(){
@@ -96,7 +127,7 @@ public class Vision{
     }
 
     public double getBlackTimeStamp(){
-        return blackRobotRelativeX.getLastChange();
+        return black_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getBlackTagDetected(){
@@ -107,22 +138,26 @@ public class Vision{
         return blackFramerate.get();
     }
 
-    public Pose2d getOrangeGlobalPose(){
-        return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+    public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
+            return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+                orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
+                );
+        }else{
+            return new Pose2d();
+        }
     }
     
-    public double getOrangeRelativeX(){
-        return orangeRobotRelativeX.get();
+    public double getOrangeTX(){
+        return orange_tx.get();
     }
 
-    public double getOrangeRelativeY(){
-        return orangeRobotRelativeY.get();
+    public double getOrangeTY(){
+        return orange_ty.get();
     }
 
-    public double getOrangeRelativeZ(){
-        return orangeRobotRelativeZ.get();
+    public double getOrangeDist(){
+        return orange_dist.get();
     }
 
     public int getOrangeClosestTag(){
@@ -130,7 +165,7 @@ public class Vision{
     }
 
     public double getOrangeTimeStamp(){
-        return orangeRobotRelativeX.getLastChange();
+        return orange_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getOrangeTagDetected(){
@@ -140,5 +175,13 @@ public class Vision{
     public double getOrangeFPS(){
         return orangeFramerate.get();
     }
+
+    public boolean isBlackConnected(){
+        return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0;
+    }
+
+    public boolean isOrangeConnected(){
+        return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0;
+    }
     
 }

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

@@ -2,9 +2,6 @@ package frc.robot.subsystems.sysid;
 
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
-
-import java.util.function.BooleanSupplier;
-
 import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
@@ -25,8 +22,8 @@ public class ElevatorSysID extends Elevator {
 
     private SysIdRoutine routine;
 
-    public ElevatorSysID(BooleanSupplier hasAlgae) {
-        super(hasAlgae);
+    public ElevatorSysID() {
+        super();
 
         appliedVoltage = Volts.mutable(0);
 
@@ -41,7 +38,7 @@ public class ElevatorSysID extends Elevator {
                 (log) -> {
                     log.motor("elevatorMotor1")
                         .voltage(appliedVoltage.mut_replace(
-                            getMotor1(), Volts
+                            elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
                             encoder.getPosition(), Inches

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": []
+}