From a391cc79103c9ad02c6fe95f872d3dc17e62e632 Mon Sep 17 00:00:00 2001
From: Team 2648 <team2648@coldlightalchemist.com>
Date: Thu, 3 Apr 2025 08:20:37 -0400
Subject: [PATCH] algae setpoints

---
 .../deploy/pathplanner/paths/New Path.path    | 54 +++++++++++++++++++
 src/main/java/frc/robot/RobotContainer.java   | 11 +++-
 .../constants/ClimberRollersConstants.java    |  9 ++++
 .../frc/robot/constants/VisionConstants.java  | 26 +++++++++
 4 files changed, 99 insertions(+), 1 deletion(-)
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..61690b6
--- /dev/null
+++ b/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
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 04f133d..3a892d5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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()));
 	}
diff --git a/src/main/java/frc/robot/constants/ClimberRollersConstants.java b/src/main/java/frc/robot/constants/ClimberRollersConstants.java
index 71b7af9..c65ca34 100644
--- a/src/main/java/frc/robot/constants/ClimberRollersConstants.java
+++ b/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);
+        
+    }
 }
diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index f20eaa2..b3db789 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/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;
 
 }