fixing dt bindings divide by zero flaw

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

@@ -27,6 +27,12 @@
           "data": {
             "pathName": "H Backup"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
         }
       ]
     }

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

@@ -0,0 +1,74 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Pickup Algae L2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG Algae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Algae"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Post-Barge Backup"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.0549180327868852,
+        "x": 1.1268442622950818,
-        "y": 7.285655737704918
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 1.9113820738827756,
+        "x": 2.287270519874242,
-        "y": 6.339037587019986
+        "y": 6.774371912194027
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.7,
+        "x": 5.758260140458621,
-        "y": 4.3
+        "y": 4.193633481772718
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.053790983606557,
+        "x": 6.112051124065178,
-        "y": 4.312704918032787
+        "y": 4.206338399805505
       },
       "isLocked": false,
       "linkedName": "H"
     },
     {
       "anchor": {
-        "x": 6.389446721311475,
+        "x": 6.5,
-        "y": 4.3
+        "y": 4.021985060730393
       },
       "prevControl": {
-        "x": 6.1394487099079695,
+        "x": 6.250001988596495,
-        "y": 4.300997143065429
+        "y": 4.022982203795822
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Behind HG Face"
     }
   ],
   "rotationTargets": [],
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 0.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -45,10 +45,10 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Center",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/HG Algae.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.5,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.224227344787433,
+        "y": 4.009974315068493
+      },
+      "isLocked": false,
+      "linkedName": "Behind HG Face"
+    },
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": {
+        "x": 6.125859753477119,
+        "y": 4.004837296105254
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    }
+  ],
+  "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": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HG to Barge.path

@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.829105562465259,
+        "y": 3.8676166530198106
+      },
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    },
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": {
+        "x": 7.262336691900294,
+        "y": 4.359747737103425
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.17234468937875755,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Processor Position",
+      "waypointRelativePos": 0.188095238095238,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Processor Position"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 300.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0549180327868852,
+        "x": 1.1268442622950818,
-        "y": 7.285655737704918
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.3471269368964744,
+        "x": 2.0019467213114757,
-        "y": 7.060270463732315
+        "y": 6.434528688524591
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 3.5723360655737704,
+        "x": 3.609900518622585,
-        "y": 4.996004098360656
+        "y": 5.005924534374863
       },
       "prevControl": {
-        "x": 3.256796682012127,
+        "x": 3.2943611350609414,
-        "y": 5.521903070963395
+        "y": 5.531823506977602
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0549180327868852,
+        "x": 1.1268442622950818,
-        "y": 7.285655737704918
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.5703893442622952,
+        "x": 1.9266540815180775,
-        "y": 6.734221311475409
+        "y": 6.744320542331013
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
@@ -20,8 +20,8 @@
         "y": 5.211782786885246
       },
       "prevControl": {
-        "x": 3.2246926229508195,
+        "x": 2.975085616438356,
-        "y": 6.230737704918033
+        "y": 6.023416095890411
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,8 +45,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -62,5 +62,5 @@
     "velocity": 0,
     "rotation": -53.97262661489646
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.5723360655737704,
+        "x": 3.609900518622585,
-        "y": 4.996004098360656
+        "y": 5.005924534374863
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.4398363305729753,
+        "x": 3.47740078362179,
-        "y": 5.208003674361928
+        "y": 5.217924110376135
       },
       "isLocked": false,
       "linkedName": "L"
     },
     {
       "anchor": {
-        "x": 1.0549180327868852,
+        "x": 1.1268442622950818,
-        "y": 7.285655737704918
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 1.258709016393443,
+        "x": 1.3306352459016395,
-        "y": 7.009938524590164
+        "y": 6.926024590163935
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.786987704919465,
+        "x": 2.0,
-        "y": 4.348668032784689
+        "y": 7.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 9.786987704919456,
+        "x": 3.0,
-        "y": 4.348668032784689
+        "y": 7.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 11.256454918034219,
+        "x": 3.756421232876712,
-        "y": 4.348668032784689
+        "y": 5.227054794520548
       },
       "prevControl": {
-        "x": 7.543546317006037,
+        "x": 2.756421232876712,
-        "y": 6.492316813064786
+        "y": 5.227054794520548
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -59.18537788806707
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 124.695153531234
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Post-Barge Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.71311475409836,
+        "y": 4.9480532786885245
+      },
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    },
+    {
+      "anchor": {
+        "x": 5.933913934426228,
+        "y": 5.247745901639345
+      },
+      "prevControl": {
+        "x": 6.641188524590164,
+        "y": 5.043954918032786
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 1.25,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.7,
+        "x": 5.758260140458621,
-        "y": 4.3
+        "y": 4.193633481772718
       },
       "prevControl": {
-        "x": 6.347336065573771,
+        "x": 6.405596206032391,
-        "y": 4.2640368852459005
+        "y": 4.157670367018619
       },
       "nextControl": null,
       "isLocked": false,
@@ -31,10 +31,22 @@
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.10238095238095252,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -45,10 +57,10 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Center",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/settings.json

@@ -3,7 +3,8 @@
   "robotLength": 0.8763,
   "holonomicMode": true,
   "pathFolders": [
-    "Left Paths"
+    "Left Paths",
+    "Center"
   ],
   "autoFolders": [],
   "defaultMaxVel": 3.5,

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

@@ -35,7 +35,7 @@ public Robot() {
 
     if (isReal()) {
         Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        //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

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

@@ -111,9 +111,9 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftY(),
-				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftX(),
-				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
@@ -226,8 +226,8 @@ public class RobotContainer {
 		operator.back().onTrue(elevator.homeCommand());
 
 		operator.start().toggleOnTrue(
-			climberPivot.runPivot(() -> -operator.getRightY()*0.5)
+			climberPivot.runPivot(() -> -operator.getRightY())
-			.alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+			.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
 
 		operator.a().onTrue(
 			safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
@@ -339,7 +339,7 @@ public class RobotContainer {
 					), 
 				manipulatorPivot.maintainPosition()
 					.withTimeout(
-						0.01
+						0.1
 					)
 			)
 		);
@@ -351,6 +351,28 @@ public class RobotContainer {
 				ManipulatorPivotConstants.kStartingPosition
 			)
 		);
+
+		NamedCommands.registerCommand(
+			"Shoot Algae",
+			shootAlgae().withTimeout(2)   
+		);
+
+		NamedCommands.registerCommand(
+			"Processor Position", 
+			moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+		);
+
+		NamedCommands.registerCommand(
+			"Pickup Algae L2", 
+			moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+			.andThen(
+				elevator.maintainPosition()
+				.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
+
+			//Dont you need a holdPosition call?
+		);
 	}
 
 	//creates tabs and transforms them on the shuffleboard
@@ -607,10 +629,10 @@ public class RobotContainer {
 	}
 
 	private Command shootAlgae(){
-		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
-		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}
 

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

@@ -16,6 +16,8 @@ public class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 

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 = 5.5;
+    public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration

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

@@ -56,6 +56,8 @@ public class ElevatorConstants {
 
     public static final double kVoltageLimit = 7;
 
+    public static final double kVoltageLimitAlgae = 9;
+
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;
     public static final double kSysIDStepVolts = 3;

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

@@ -23,7 +23,9 @@ public class ManipulatorPivotConstants {
     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(1.5);
+    public static final double kPositionalTolerance = Units.degreesToRadians(3);
+
+    public static final double kAlgaeP = 7;
 
     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

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

@@ -9,4 +9,6 @@ public class OIConstants {
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
     public static final String kApriltagTab = "Apriltag Tab";
+
+    public static final double kJoystickExponential = 3;
 }

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

@@ -180,13 +180,13 @@ public class Drivetrain extends SubsystemBase {
         m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
 
         if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
-            if(vision.getOrangeDist() < 60 && getVelocity() < 2){
+            if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
-            }else if(vision.getOrangeDist() < 100 && getVelocity() < 2){
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
-            }else if(vision.getOrangeDist() < 60 && getVelocity() > 2){
+            }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
-            }else if(vision.getOrangeDist() < 100 && getVelocity() > 2){
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
             }
 
@@ -203,16 +203,18 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
         Logger.recordOutput("orange dist", vision.getOrangeDist());
         Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange FPS", vision.getOrangeFPS());
 
 
         if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
-            if(vision.getBlackDist() < 60 && getVelocity() < 2){
+            if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
-            }else if(vision.getBlackDist() < 100 && getVelocity() < 2){
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
-            }else if(vision.getBlackDist() < 60 && getVelocity() > 2){
+            }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
-            }else if(vision.getBlackDist() < 100 && getVelocity() > 2){
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
                 m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
             }
 
@@ -227,9 +229,12 @@ public class Drivetrain extends SubsystemBase {
         }
         Logger.recordOutput("black pose", new Pose3d(blackPose2d));
         Logger.recordOutput("black dist", vision.getBlackDist());
-        Logger.recordOutput("orange detected", vision.getBlackTagDetected());
+        Logger.recordOutput("black detected", vision.getBlackTagDetected());
+        Logger.recordOutput("black tag", vision.getBlackTagDetected());
+        Logger.recordOutput("black FPS", vision.getBlackFPS());
 
         Logger.recordOutput("drive velocity", getVelocity());
+        Logger.recordOutput("closest tag", getClosestTag());
         Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
         
         if(musicTimer.get()>10){
@@ -318,8 +323,18 @@ public class Drivetrain extends SubsystemBase {
      */
     public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
         // Convert the commanded speeds into the correct units for the drivetrain
-        double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
-        double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double xSpeedDelivered = 0;
+        double ySpeedDelivered = 0;
+
+        if(p != 0){
+            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        }else{
+            xSpeedDelivered = 0;
+            ySpeedDelivered = 0;
+        }
+        
         double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
@@ -357,8 +372,8 @@ public class Drivetrain extends SubsystemBase {
                 );
 
                 double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
-                if (speed > AutoConstants.kMaxSpeedMetersPerSecond) {
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
-                    double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed;
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
                     controlEffort.vxMetersPerSecond *= mul;
                     controlEffort.vyMetersPerSecond *= mul;
                 }

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

@@ -2,6 +2,8 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -94,6 +96,10 @@ public class Elevator extends SubsystemBase {
         if (!getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
+
+        Logger.recordOutput("elevator position", getEncoderPosition());
+        Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
+        Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
     }
 
     /**
@@ -245,50 +251,65 @@ public class Elevator extends SubsystemBase {
                 
                 }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
         }
+    }          
+
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+            
+        if (setpoint.getAsDouble() == 0) {
+            return startRun(() -> {
         
-            
+                pidControllerUp.reset();
-            /*
+                pidControllerDown.reset();
-            elevatorMotor1.setVoltage(
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidController.calculate(
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                    encoder.getPosition(),
+                
-                    clampedSetpoint
+                }, 
-                ) + feedForward.calculate(0)
+                () -> {
-            );
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-*/
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-            /* 
+        
-            if (!pidController.atSetpoint()) {
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                elevatorMotor1.setVoltage(
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                    pidController.calculate(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        encoder.getPosition(),
+                        );
-                        clampedSetpoint
+                    else{
-                    ) + feedForward.calculate(0)
+                        elevatorMotor1.setVoltage(
-                );
+                            MathUtil.clamp(
-            } else {
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                elevatorMotor1.setVoltage(
+                        );
-                    feedForward.calculate(0)
+                    }
-                );
+                
-            }
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-             
+                    .andThen(runManualElevator(() -> -.5)
-        });*/
+                    .until(() -> encoder.getPosition() == 0));
-    }
+
-            
+        } else {
-            /* 
+            return startRun(() -> {
-            if(encoder.getPosition() >= setpoint.getAsDouble()){
+        
-                elevatorMotor1.setVoltage(
+                pidControllerUp.reset();
-                    pidControllerUp.calculate(
+                pidControllerDown.reset();
-                        encoder.getPosition(),
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                        clampedSetpoint
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
+        
-                );
+                },
-            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
+                () -> {
-                elevatorMotor1.setVoltage(
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    pidControllerDown.calculate(
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-                        encoder.getPosition(),
+        
-                        clampedSetpoint
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                );
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-            }
+                        );
-                */
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+        }
+    }   
 
     /**
      * Returns the current encoder position

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

@@ -2,6 +2,8 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -46,6 +48,13 @@ public class Manipulator extends SubsystemBase {
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
     }
 
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("coral beam break", getCoralBeamBreak());
+    }
+
     /**
      * The default command for the manipulator that either stops the manipulator or slowly 
      * runs the manipulator to retain the algae

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

@@ -4,6 +4,8 @@ import com.revrobotics.spark.SparkMax;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.spark.SparkAbsoluteEncoder;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -25,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase {
 
     private PIDController pidController;
 
+    private PIDController algaePIDController;
+
     public ManipulatorPivot() {
         pivotMotor = new SparkMax(
             ManipulatorPivotConstants.kPivotMotorID, 
@@ -44,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase {
             ManipulatorPivotConstants.kPositionalI, 
             ManipulatorPivotConstants.kPositionalD
         );
+        pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+
         pidController.setSetpoint(0);
 
         pidController.enableContinuousInput(0, 280);
         
+        algaePIDController = new PIDController(
+            ManipulatorPivotConstants.kAlgaeP,
+            0,
+            0);
+        algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+        
+        algaePIDController.setSetpoint(0);
+
+        algaePIDController.enableContinuousInput(0, 280);
+
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
             ManipulatorPivotConstants.kFeedForwardG, 
@@ -55,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase {
             );
     }
 
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("manipulator position", getEncoderPosition());
+        Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
+    }
+
     /**
      * Returns whether or not the motion is safe relative to the encoder's current position
      * and the arm safe stow position
@@ -125,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase {
             }).until(() -> pidController.atSetpoint());
     }
 
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            algaePIDController.setSetpoint(setpoint.getAsDouble());
+            algaePIDController.reset();
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    algaePIDController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> algaePIDController.atSetpoint());
+    }
+
     public Command maintainPosition() {
         return startRun(() -> {
             

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

@@ -177,11 +177,11 @@ public class Vision{
     }
 
     public boolean isBlackConnected(){
-        return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0;
+        return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
     }
 
     public boolean isOrangeConnected(){
-        return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0;
+        return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
     }
     
 }