auto sucky

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.1987704918032787,
+        "x": 1.0549180327868852,
-        "y": 7.189754098360656
+        "y": 7.285655737704918
       },
       "prevControl": {
-        "x": 2.055234532899169,
+        "x": 1.9113820738827756,
-        "y": 6.243135947675724
+        "y": 6.339037587019986
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1987704918032787,
+        "x": 1.0549180327868852,
-        "y": 7.189754098360656
+        "y": 7.285655737704918
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.490979395912868,
+        "x": 2.3471269368964744,
-        "y": 6.964368824388053
+        "y": 7.060270463732315
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 3.6202868852459016,
+        "x": 3.65625,
-        "y": 5.031967213114754
+        "y": 5.05594262295082
       },
       "prevControl": {
-        "x": 3.304747501684258,
+        "x": 3.3407106164383564,
-        "y": 5.557866185717494
+        "y": 5.581841595553559
       },
       "nextControl": null,
       "isLocked": false,
@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 1.25,
     "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/HP to K.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1987704918032787,
+        "x": 1.0549180327868852,
-        "y": 7.189754098360656
+        "y": 7.285655737704918
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.7142418032786886,
+        "x": 1.5703893442622952,
-        "y": 6.638319672131147
+        "y": 6.734221311475409
       },
       "isLocked": false,
       "linkedName": "HP Left Position"

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6202868852459016,
+        "x": 3.65625,
-        "y": 5.031967213114754
+        "y": 5.05594262295082
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.4877871502451065,
+        "x": 3.523750264999205,
-        "y": 5.243966789116026
+        "y": 5.267942198952092
       },
       "isLocked": false,
       "linkedName": "L"
     },
     {
       "anchor": {
-        "x": 1.1987704918032787,
+        "x": 1.0549180327868852,
-        "y": 7.189754098360656
+        "y": 7.285655737704918
       },
       "prevControl": {
-        "x": 1.4025614754098363,
+        "x": 1.258709016393443,
-        "y": 6.914036885245903
+        "y": 7.009938524590164
       },
       "nextControl": null,
       "isLocked": false,

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/deploy/pathplanner/paths/Start to 30 Right.path

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -62,5 +62,5 @@
     "velocity": 0,
     "rotation": -90.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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;
 
 }

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

@@ -112,7 +112,7 @@ public class Drivetrain extends SubsystemBase {
             },
             new Pose2d(),
             VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
-            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
+            VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))
         );
 
         pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
@@ -177,11 +177,11 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(360)));
 
         if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
             }
 
             // if the detected tags match your alliances reef tags use their pose estimates  
@@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase {
 
         if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
             if(vision.getBlackDist() < 60){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
             }
 
             if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){

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

@@ -84,8 +84,8 @@ public class Manipulator extends SubsystemBase {
      */
     public Command runUntilCollected(DoubleSupplier speed) {
         return run(() -> {
-            manipulatorMotor.set(
+            manipulatorMotor.setVoltage(
-                speed.getAsDouble()
+                speed.getAsDouble()*12
             );
 
             indexerMotor.set(1);