auto align works kinda

.OutlineViewer/outlineviewer.json

@@ -11,6 +11,9 @@
   "client@2": {
     "open": true
   },
+  "client@3": {
+    "open": true
+  },
   "client@4": {
     "Publishers": {
       "open": true

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.891,
+        "y": 5.284
+      },
+      "prevControl": {
+        "x": 3.891,
+        "y": 5.284
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.events.EventTrigger;
 import com.pathplanner.lib.path.EventMarker;
 
+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;
@@ -159,7 +160,7 @@ public class RobotContainer {
 				drivetrain.goToPose(
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
-					() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
+					() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
 				)
 			);
 
@@ -167,7 +168,7 @@ public class RobotContainer {
 			drivetrain.goToPose(
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
-				() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
 			)
 		);
 
@@ -231,9 +232,10 @@ public class RobotContainer {
 		new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
-		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
+		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2).andThen(manipulator.runUntilCollected(() -> 0)));
-		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5));
-		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
+		.andThen(Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
 		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
 	}
 
@@ -314,6 +316,8 @@ public class RobotContainer {
 
 		sensorTab.addDouble("velocity", drivetrain::getVelocity);
 
+		sensorTab.addDouble("heading", drivetrain::getHeading);
+
 
 		//sensorTab.add("odometry", drivetrain::getPose);
 
@@ -324,6 +328,7 @@ public class RobotContainer {
 		apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
 		apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
 		apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
+		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS());
 
 //		sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
 

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

@@ -57,7 +57,7 @@ public class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
-    public static final double kHeadingP = 0.01;
+    public static final double kHeadingP = 0.1;
 
     public static final double kXTranslationP = 0.5;
     public static final double kYTranslationP = 0.5;

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

@@ -54,7 +54,7 @@ public class VisionConstants {
         {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.991, 5.246},
+        {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}
     };

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

@@ -8,17 +8,20 @@ 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.pathplanner.lib.path.RotationTarget;
 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.PIDController;
+import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -112,7 +115,8 @@ public class Drivetrain extends SubsystemBase {
         );
 
         pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
-        pidHeading.enableContinuousInput(-180, 180);
+        pidHeading.setTolerance(Units.degreesToRadians(3));
+        pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
 
         pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
         pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
@@ -171,11 +175,11 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(360)));
 
         if(vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
             }
 
             // if the detected tags match your alliances reef tags use their pose estimates  
@@ -187,8 +191,9 @@ public class Drivetrain extends SubsystemBase {
                 orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
                 m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
             }
-            Logger.recordOutput("orange pose", new Pose3d());
         }
+        Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
+
 
         if(vision.getBlackTagDetected()){
             if(vision.getBlackDist() < 60){
@@ -310,16 +315,22 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
-    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
-        return run(() -> {
+        return startRun(() -> {
-            drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
+            pidHeading.reset();
-                MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1), 
+            pidTranslationX.reset();
-                pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), 
+            pidTranslationY.reset();
+            },
+            () -> {
+            drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05),
+                MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05), 
+                pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()), 
                 true);
+                Logger.recordOutput("reeeee", pidHeading.getError());
 
                 Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
                     new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
-                    new Rotation2d(headingSetpoint.getAsDouble()))));
+                    headingSetpoint.get())));
             });
     }
 
@@ -409,7 +420,7 @@ 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(){