Merge pull request #72 from frc2052/mlms/2Dapriltags

Mlms/2 dapriltags

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

@@ -347,6 +347,10 @@ public static final class AprilTagLocations {
         public static final double TAG16_ROTATION = 240;
     }
 
+    public static final class AprilTags {
+        public static final double APRILTAG_TIMEOUT = 1000;
+    }
+
 
     public static final class Elevator{
             public static final int BELT_MOTOR = 0;

src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.auto.commands.drive;
+
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotState;
+import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.util.AimingCalculator;
+
+public class AimToSpeakerUsingOneAprilTagCommand extends Command {
+    protected final DrivetrainSubsystem drivetrain;
+    private double goalAngleDegrees;
+    private double deltaDegrees;
+    private boolean isFinished = false;
+
+    public AimToSpeakerUsingOneAprilTagCommand(DrivetrainSubsystem drivetrain) {
+        this.drivetrain = drivetrain;
+        isFinished = false;
+
+        addRequirements(drivetrain);
+    }
+
+    private double getRotation() {
+        isFinished = false;
+        goalAngleDegrees = AimingCalculator.calculateRobotAngleOneAprilTag();
+        deltaDegrees = RobotState.getInstance().getRotation2d360().getDegrees() - goalAngleDegrees;
+        Logger.recordOutput("goal angle", goalAngleDegrees);
+        Logger.recordOutput("measured angle", RobotState.getInstance().getRotation2d360().getDegrees());
+
+        // return 0;
+        if (Math.abs(deltaDegrees) > 90) {
+            return Math.copySign(0.5, -deltaDegrees);
+        } else if (Math.abs(deltaDegrees) > 45){
+            return Math.copySign(0.25, -deltaDegrees);
+        } else if (Math.abs(deltaDegrees) > 5){
+            return Math.copySign(0.1, -deltaDegrees);
+        } else {
+            isFinished = true;
+            return 0;
+        }
+    }
+
+    @Override
+    public void execute() {
+        drivetrain.drive(0, 0, getRotation(), true);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drivetrain.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return isFinished;
+    }
+}

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

@@ -3,6 +3,9 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.OptionalDouble;
+
+import javax.swing.text.html.Option;
 
 import org.photonvision.targeting.PhotonTrackedTarget;
 
@@ -11,8 +14,12 @@
 import com.team2052.lib.photonvision.PhotonPoseEstimator;
 import com.team2052.lib.photonvision.PhotonPoseEstimator.PoseStrategy;
 
+import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.RobotState;
@@ -27,6 +34,12 @@ public class AprilTagSubsystem extends SubsystemBase{
     private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
     private PhotonCamera camera0 = new PhotonCamera(Constants.PhotonCamera1.CAMERA_NAME, Constants.PhotonCamera1.ROBOT_TO_CAMERA_METERS);
     private PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera0, camera0.getRobotToCamera());
+    private int speakertag;
+    private double speakerTagYaw;
+    private Timer timer;
+    private double lastUpdatedTime;
+    private boolean isSeeingTag;
+
     //private PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy., camera0, camera0.getRobotToCamera());
     //private PhotonCamera camera1 = new PhotonCamera(Constants.PhotonCamera2.CAMERA_NAME, Constants.PhotonCamera2.ROBOT_TO_CAMERA_METERS);
 
@@ -41,13 +54,23 @@ public static AprilTagSubsystem getInstance(){
     private AprilTagSubsystem() {
         cameras.add(camera0);
         //cameras.add(camera1);
+        if (!robotState.getInstance().isRedAlliance()){
+            speakertag = 8; //blue
+        } else {
+            speakertag = 4;//red
+        }
+        timer.start();
+        isSeeingTag = false;
+        SmartDashboard.putBoolean("Is Seeing Speaker April Tag", isSeeingTag);
     }
 
     @Override
     public void periodic() {
         for(int i = 0; i < cameras.size(); i++){
             PhotonCamera camera = cameras.get(i);
             var result = camera.getLatestResult();
+            var targets = result.getTargets();
+
             boolean hasTargets = result.hasTargets();
             if (hasTargets){
                 // PhotonTrackedTarget target7 = null;
@@ -66,7 +89,25 @@ public void periodic() {
                     estimatedPose = poseUpdate.get();
                     robotState.addAprilTagVisionUpdate(estimatedPose);
                 }
+
+                for (int j = 0; j < targets.size(); j++) {
+                    if (targets.get(j).getFiducialId() == speakertag) {
+                        speakerTagYaw = targets.get(j).getYaw();
+                        double lastUpdatedTime = timer.get();
+                    }
+                }
+
             }
         }
     }
+
+    public Optional<Double> getYaw() {
+        if (timer.get() - lastUpdatedTime <= Constants.AprilTags.APRILTAG_TIMEOUT) {
+            isSeeingTag = true;
+            return Optional.of(speakerTagYaw);
+        } else {
+            isSeeingTag = false;
+            return Optional.empty();
+        }
+    }
 }

src/main/java/frc/robot/util/AimingCalculator.java

@@ -1,14 +1,18 @@
 package frc.robot.util;
 
+import java.util.Optional;
+
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.robot.RobotState;
+import frc.robot.subsystems.AprilTagSubsystem;
 
 public class AimingCalculator {
 
@@ -85,6 +89,16 @@ public static double calculateAngle(Pose2d robotPose){
         return MathUtil.inputModulus(Math.copySign(angleToSpeakerFieldRelativeDegrees, robotPose.getRotation().getDegrees()), 0, 360);
     }
 
+    public static double calculateRobotAngleOneAprilTag() {
+        Optional<Double> yaw = AprilTagSubsystem.getInstance().getYaw();
+
+        if (!yaw.isEmpty()) {
+            return yaw.get();
+        } else {
+            return 0;
+        }
+    }
+
     public static double calculateDistanceToSpeaker(Pose2d robotPose) {
         if(!RobotState.getInstance().isRedAlliance()) { // blue alliance
             Translation2d speakerLocation = Constants.FieldAndRobot.BLUE_SPEAKER_LOCATION;