From 2825af3cde2e6f8b13eb754876510db551592d25 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Thu, 15 Feb 2024 15:35:53 -0700 Subject: [PATCH] Revert "Make boiler plate code - Needs to be programmed with robot" This reverts commit 0f7861ff022153ff66ea4014bb567603839582fd. --- build.gradle | 2 +- gradlew | 0 simgui.json | 19 +- src/main/java/frc4388/robot/Constants.java | 7 +- .../java/frc4388/robot/RobotContainer.java | 15 +- .../robot/commands/Autos/AutoAlign.java | 62 ------- .../robot/commands/Autos/PlaybackChooser.java | 8 +- .../frc4388/robot/subsystems/Apriltags.java | 36 ++++ .../frc4388/robot/subsystems/Limelight.java | 166 ++++++++++++++++-- .../java/frc4388/robot/subsystems/Vision.java | 38 ++++ src/main/java/frc4388/utility/AprilTag.java | 13 ++ 11 files changed, 241 insertions(+), 125 deletions(-) mode change 100755 => 100644 gradlew delete mode 100644 src/main/java/frc4388/robot/commands/Autos/AutoAlign.java create mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java create mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/build.gradle b/build.gradle index dc75beb..e7eff7d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.1.1" } java { diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/simgui.json b/simgui.json index a735151..fb3cd1b 100644 --- a/simgui.json +++ b/simgui.json @@ -8,27 +8,10 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo", - "/Shuffleboard/Auto Chooser/Add Sequence": "Command", - "/Shuffleboard/Auto Chooser/Command: 1": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 10": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 2": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 3": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 4": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 5": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 6": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 7": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 8": "String Chooser", - "/Shuffleboard/Auto Chooser/Command: 9": "String Chooser" + "/FMSInfo": "FMSInfo" } }, "NetworkTables Info": { - "Server": { - "Subscribers": { - "open": true - }, - "open": true - }, "visible": true } } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 198b32f..7b5ba85 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -139,12 +139,7 @@ public final class Constants { public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value } - - public static final class AutoAlignConstants { - public static final double MoveSpeed = 0.0; - public static final double RotSpeed = 0.0; - } - + public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 14; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 692471d..3fda500 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,14 +19,12 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; -import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Intake; @@ -62,15 +60,12 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - - /* Autos */ - private Limelight limelight = new Limelight(); - private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt"); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.pidIn()), new InstantCommand(() -> m_robotShooter.spin()) @@ -87,12 +82,6 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) ); - private SequentialCommandGroup autoShoot = new SequentialCommandGroup( - new RunCommand(() -> MoveToSpeaker.execute()), - new RunCommand(() -> autoAlign.execute()) - ); - - private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java deleted file mode 100644 index fda5f60..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ /dev/null @@ -1,62 +0,0 @@ -package frc4388.robot.commands.Autos; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.Constants.AutoAlignConstants; - -import edu.wpi.first.wpilibj2.command.Command; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Pose2d; - -import edu.wpi.first.wpilibj.DriverStation; - -public class AutoAlign extends Command { - private SwerveDrive swerve; - private Limelight limelight; - private Pose2d pose; - - private boolean isRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - - public AutoAlign(SwerveDrive swerve, Limelight limelight) { - this.swerve = swerve; - } - - private Translation2d calcMoveStick(){ - //TODO - DO THE MATH! - pose = limelight.getPose(); - - double curX = pose.getX(); - double curY = pose.getY(); - double curYaw = pose.getRotation().getDegrees(); - - // This is not math - double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - - return new Translation2d(stickX, stickY); - } - - private Translation2d calcRotStick(){ - //TODO - DO THE MATH! - pose = limelight.getPose(); - - double curX = pose.getX(); - double curY = pose.getY(); - double curYaw = pose.getRotation().getDegrees(); - - // This is not math - double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - - return new Translation2d(stickX, stickY); - } - - public void execute() { - Translation2d moveStick = calcMoveStick(); - Translation2d rotStick = calcRotStick(); - - // This would greatly benifit from having feild Relative implemented. - swerve.driveWithInput(moveStick, rotStick, false); - } -} diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index a270d7b..2438a38 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -66,13 +66,7 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - String[] dirs = m_dir.list(); - - if(dirs == null){ // Fix funny error - return; - } - - for (String auto : dirs) { + for (String auto : m_dir.list()) { chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); } for (var cmd_name : m_commandPool.keySet()) { diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java new file mode 100644 index 0000000..c6062e8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -0,0 +1,36 @@ +package frc4388.robot.subsystems; + +//import edu.wpi.first.apriltag.AprilTag; +//import edu.wpi.first.math.geometry.Pose3d; +//import edu.wpi.first.math.geometry.Rotation3d; +//import edu.wpi.first.networktables.NetworkTable; +//import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Apriltags { + public static class Tag { + public boolean visible = true; + public double x, y, z = 0; + public double ry, rp, rr = 0; + } + + public Tag getTagPosRot() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + final Tag tag = new Tag(); + tag.visible = isAprilTag(); + tag.x = tagTable.getEntry("TagPosX").getDouble(0); + tag.y = tagTable.getEntry("TagPosY").getDouble(0); + tag.z = tagTable.getEntry("TagPosZ").getDouble(0); + tag.ry = tagTable.getEntry("TagRotY").getDouble(0); + tag.rp = tagTable.getEntry("TagRotP").getDouble(0); + tag.rr = tagTable.getEntry("TagRotR").getDouble(0); + + return tag; + } + + public boolean isAprilTag() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + return tagTable.getEntry("IsTag").getBoolean(false); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index ce7bfe1..9d1289b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,33 +3,163 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// Look at vvv for networktables stuff -// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data +import frc4388.robot.Constants.VisionConstants; public class Limelight extends SubsystemBase { - private double[] cameraPose; + + private PhotonCamera cam; + private PhotonPoseEstimator photonPoseEstimator; - public Pose2d getPose() { - //TODO - Get actual values! + private boolean lightOn; - double x = 0; - double y = 0; - double yaw = 0; + /** Creates a new Limelight. */ + public Limelight() { + cam = new PhotonCamera(VisionConstants.NAME); + cam.setDriverMode(false); + } - Rotation2d rot = Rotation2d.fromDegrees(yaw); + public void setLEDs(boolean on) { + lightOn = on; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } - return new Pose2d(x, y, rot); + public void toggleLEDs() { + lightOn = !lightOn; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void setDriverMode(boolean driverMode) { + cam.setDriverMode(driverMode); + } + + public void setToLimePipeline() { + cam.setPipelineIndex(1); + setLEDs(true); + } + + public void setToAprilPipeline() { + cam.setPipelineIndex(0); + setLEDs(false); + } + + public PhotonTrackedTarget getAprilPoint() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget(); + } + + private List getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); + } + + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList bottomSide = getAprilBottomSide(corners); + + if (bottomSide == null) return 0; + + TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); + TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); + + return bottomLeft.y - bottomRight.y; + } + + private ArrayList getAprilBottomSide(List box) { + if (box == null) return null; + + ArrayList bottomSide = new ArrayList<>(); + + TargetCorner l1 = new TargetCorner(-1, -1); + TargetCorner l2 = new TargetCorner(-1, -1); + + for (TargetCorner c : box) { + if (c.y > l1.y) l1 = c; + } + + for (TargetCorner c : box) { + if (c.y == l1.y) continue; + if (c.y > l2.y) l2 = c; + } + + bottomSide.add(l1); + bottomSide.add(l2); + + return bottomSide; + } + + public double getDistanceToApril() { + PhotonTrackedTarget aprilPoint = getAprilPoint(); + if (aprilPoint == null) return -1; + + double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + aprilPoint.getPitch(); + + double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); + return distanceToApril; + } + + public PhotonTrackedTarget getLowestTape() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + ArrayList points = (ArrayList) result.getTargets(); + + PhotonTrackedTarget lowest = points.get(0); + for (PhotonTrackedTarget point : points) { + if (point.getPitch() < lowest.getPitch()) { + lowest = point; + } + } + + return lowest; + } + + public double getDistanceToTape() { + PhotonTrackedTarget tapePoint = getLowestTape(); + if (tapePoint == null) return -1; + + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + tapePoint.getPitch(); + + double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + return distanceToTape; } @Override - public void periodic() { - // This method will be called once per scheduler run - cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); - } -} \ No newline at end of file + public void periodic() {} +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..371f621 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,38 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Vision { + private final NetworkTableEntry m_isTags; + private final NetworkTableEntry m_xPoses; + private final NetworkTableEntry m_yPoses; + private final NetworkTableEntry m_zPoses; + + public Vision() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + m_isTags = tagTable.getEntry("IsTag"); + m_xPoses = tagTable.getEntry("TagPosX"); + m_yPoses = tagTable.getEntry("TagPosY"); + m_zPoses = tagTable.getEntry("TagPosZ"); + } + + public AprilTag[] getAprilTags() { + if (!m_isTags.getBoolean(false)) return new AprilTag[0]; + + double xarr[] = m_xPoses.getDoubleArray(new double[] {}); + double yarr[] = m_yPoses.getDoubleArray(new double[] {}); + double zarr[] = m_zPoses.getDoubleArray(new double[] {}); + + AprilTag tags[] = new AprilTag[xarr.length]; + for (int i = 0; i < tags.length; i++) { + tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); + } + + return tags; + } +} diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java new file mode 100644 index 0000000..c2ad269 --- /dev/null +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -0,0 +1,13 @@ +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +}