algae setpoints

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.786987704919465,
+        "y": 4.348668032784689
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 9.786987704919456,
+        "y": 4.348668032784689
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 11.256454918034219,
+        "y": 4.348668032784689
+      },
+      "prevControl": {
+        "x": 7.543546317006037,
+        "y": 6.492316813064786
+      },
+      "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": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 124.695153531234
+  },
+  "useDefaultConstraints": true
+}

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

@@ -167,6 +167,15 @@ public class RobotContainer {
 		
 		driver.start().whileTrue(drivetrain.resetToVision());
 	
+		driver.povUp().whileTrue(
+			drivetrain.resetToVision().andThen(
+			drivetrain.goToPose(
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1], 
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+			))
+		);
+
 		driver.rightBumper().whileTrue(
 				drivetrain.resetToVision().andThen(
 				drivetrain.goToPose(
@@ -600,7 +609,7 @@ public class RobotContainer {
 	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),
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
 		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}

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

@@ -1,9 +1,18 @@
 package frc.robot.constants;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 public class ClimberRollersConstants {
     public static final int kRollerMotorID = 9;
 
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        motorConfig
+            .smartCurrentLimit(40)
+            .idleMode(IdleMode.kBrake)
+            .inverted(true);
+        
+    }
 }

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

@@ -59,6 +59,32 @@ public class VisionConstants {
         {4.993, 2.816, 5.272, 2.996}
     };
 
+    public static final double[][] algaeSetpointsMap = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {13.906, 2.658},//6
+        {14.661, 4.013},
+        {13.834, 5.428},
+        {12.263, 5.452},
+        {11.412, 4.025},
+        {12.191, 2.574},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.649, 2.558},//17
+        {2.776, 4.005},
+        {3.644, 5.514},
+        {5.296, 5.522},//4.991, 5.246},
+        {6.225, 4.008},
+        {5.322, 2.511}//22
+    };
+
     public static final double latencyFudge = 0.0;
 
 }