Reef autoalignment (not accurate)

Author: PhilBurt627

src/main/java/frc/robot/Constants.java

@@ -17,8 +17,12 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.RobotConfig;
+
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -139,6 +143,13 @@ public static final class DriveConstants {
     public static final double driveSimKs = 0.184445; //0.0;
     public static final double driveSimKv = 0.093025; //0.0789;
 
+    public static final Matrix<N3, N1> driveStandardDevs = new Matrix<N3, N1>(
+      N3.instance,
+      N1.instance,
+      new double[] {0.2516809574, 0.4502539169, 1.370530188}
+    );
+
+
     // Turn motor configuration
     //  public static final boolean turnInverted = false;
     public static final int turnMotorCurrentLimit = 40;

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

@@ -32,6 +32,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.ManipulatorIntakeCoralCommand;
 import frc.robot.commands.OuttakeCoralCommand;
+import frc.robot.commands.AutoCommands.LimelightAlignWithReef;
 import frc.robot.commands.ElevatorCommands.ElevatorFineTuningCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorGoToPositionPositionCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorHoldCurrentPositionCommand;
@@ -175,6 +176,16 @@ private void competitionButtonBindings(){
         Trigger leftStickDown = operatorController.leftStick();
         leftStickDown.whileTrue(new ElevatorFineTuningCommand(elevator, () -> -operatorController.getRawAxis(1)));
 
+        Trigger leftReefAlignmentTrigger = new JoystickButton(flightStick, 3);
+        leftReefAlignmentTrigger.whileTrue(
+            new LimelightAlignWithReef(drive, false)
+        );
+
+        Trigger rightReefAlignmentTrigger = new JoystickButton(flightStick, 4);
+        rightReefAlignmentTrigger.whileTrue(
+            new LimelightAlignWithReef(drive, true)
+        );
+
     }
 
 

src/main/java/frc/robot/commands/AutoCommands/LimelightAlignWithReef.java

@@ -0,0 +1,92 @@
+package frc.robot.commands.AutoCommands;
+
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.util.FieldCoordinatePose2d;
+
+public class LimelightAlignWithReef extends Command {
+
+    private Drive driveSubsystem;
+
+    private Command littleCommand;
+
+    private final boolean right;
+
+    private final double horizontalOffsetMeters = Units.inchesToMeters(5);
+
+    private boolean cantFindTag = false;
+
+
+    public LimelightAlignWithReef(Drive _driveSubsystem, boolean _right){
+        driveSubsystem = _driveSubsystem;
+        right = _right;
+
+        addRequirements(driveSubsystem);
+    }
+
+    @Override
+    public void initialize(){
+
+        double horizontalOffset = horizontalOffsetMeters;
+        horizontalOffset *= right ? 1.0 : -1.0;
+
+        Pose2d poseRelativeToAprilTag = new Pose2d(
+            Units.inchesToMeters(15),
+            horizontalOffset,
+            new Rotation2d(Math.PI)
+        );
+
+        FieldCoordinatePose2d tagPose = driveSubsystem.getViewedAprilTagPoseFieldSpace();
+
+        if(tagPose == null){
+            cantFindTag = true;
+            return;
+        }
+
+        Pose2d desiredEndPose = tagPose.pose.
+            transformBy(new Transform2d(new Pose2d(), poseRelativeToAprilTag));
+
+        StructPublisher<Pose2d> pub = NetworkTableInstance.getDefault().getStructTopic(
+        "Pathfinding destination", Pose2d.struct
+        ).publish();
+
+        pub.set(desiredEndPose);
+
+        PathConstraints constraints = new PathConstraints(1.5, 2, Math.PI * 2.0, Math.PI * 2.0);
+
+        littleCommand = AutoBuilder.pathfindToPose(desiredEndPose, constraints);
+
+
+        littleCommand.initialize();
+
+    }
+
+
+    @Override
+    public void execute() {
+        littleCommand.execute();
+    }
+
+
+    @Override
+    public void end(boolean isInterrupted) {
+        littleCommand.end(isInterrupted);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return
+        littleCommand.isFinished() ||
+        cantFindTag;
+    }
+}

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -78,6 +78,7 @@ public class Drive extends SubsystemBase {
   private final Alert gyroDisconnectedAlert =
       new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError);
 
+
   private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(Constants.DriveConstants.moduleTranslations);
   private Rotation2d rawGyroRotation = new Rotation2d();  // NOTICE: Try putting PI in these parenthesis to fix the autonomous problem.
   private SwerveModulePosition[] lastModulePositions = // For delta tracking
@@ -272,27 +273,39 @@ public void periodic() {
       poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
 
       Pose2d visionPose = getRobotVisionPose();
-      // if(visionPose != null){
-      //  poseEstimator.addVisionMeasurement(visionPose, sampleTimestamps[i]);
-      // }
+      if(visionPose != null){
+       poseEstimator.addVisionMeasurement(visionPose, sampleTimestamps[i], Constants.DriveConstants.driveStandardDevs);
+    //   System.out.println(visionPose.getX() + "," + visionPose.getY() + "," + visionPose.getRotation().getRadians());
+      }
     }
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM);
   }
 
 
-  private Pose2d getRobotVisionPose(){
-    double [] limelightData = LimelightHelpers.getBotPose_TargetSpace("limelight");
-    //double[] limelightDataOld = LimelightHelpers.getTargetPose_RobotSpace("limelight");
+  public FieldCoordinatePose2d getViewedAprilTagPoseFieldSpace(){
+
     int tagId = (int)LimelightHelpers.getFiducialID("limelight");
 
     Optional<Pose3d> tagPose3d = AprilTagDataLoader.field.getTagPose(tagId);
 
     if(tagPose3d.isEmpty()) return null;
 
     Pose2d actualTagCoordTemp = new Pose2d(tagPose3d.get().getX(), tagPose3d.get().getY(), tagPose3d.get().getRotation().toRotation2d());
-    FieldCoordinatePose2d actualTagCoord = new FieldCoordinatePose2d(actualTagCoordTemp);
+
+    return new FieldCoordinatePose2d(actualTagCoordTemp);
+
+  }
+
+  private Pose2d getRobotVisionPose(){
+    double [] limelightData = LimelightHelpers.getBotPose_TargetSpace("limelight");
+
+    FieldCoordinatePose2d actualTagCoord = getViewedAprilTagPoseFieldSpace();
+
+    if(actualTagCoord == null){
+      return null;
+    }
 
     RelativeCoordinatePose2d aprilTagRelativePose = new RelativeCoordinatePose2d(Help.limelightCoordsToWPICoordsPose2d(limelightData));
     RelativeCoordinatePose2d robotPoseTagSpace = new RelativeCoordinatePose2d(new Pose2d(