diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 57e4057..8564901 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java b/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java new file mode 100644 index 0000000..bb773e8 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java index c6f69aa..946ad88 100644 --- a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java +++ b/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,6 +54,14 @@ 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 @@ -48,6 +69,8 @@ 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 getYaw() { + if (timer.get() - lastUpdatedTime <= Constants.AprilTags.APRILTAG_TIMEOUT) { + isSeeingTag = true; + return Optional.of(speakerTagYaw); + } else { + isSeeingTag = false; + return Optional.empty(); + } + } } diff --git a/src/main/java/frc/robot/util/AimingCalculator.java b/src/main/java/frc/robot/util/AimingCalculator.java index 90e437c..db84f4b 100644 --- a/src/main/java/frc/robot/util/AimingCalculator.java +++ b/src/main/java/frc/robot/util/AimingCalculator.java @@ -1,5 +1,7 @@ package frc.robot.util; +import java.util.Optional; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -7,8 +9,10 @@ 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 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;