diff --git a/build.gradle b/build.gradle index e7eff7d..dc75beb 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui.json b/simgui.json index fb3cd1b..a735151 100644 --- a/simgui.json +++ b/simgui.json @@ -8,10 +8,27 @@ }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/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" } }, "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 7b5ba85..198b32f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -139,7 +139,12 @@ 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 3fda500..692471d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,12 +19,14 @@ 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; @@ -60,12 +62,15 @@ 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()) @@ -82,6 +87,12 @@ 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 new file mode 100644 index 0000000..fda5f60 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -0,0 +1,62 @@ +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 2438a38..a270d7b 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -66,7 +66,13 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { + String[] dirs = m_dir.list(); + + if(dirs == null){ // Fix funny error + return; + } + + for (String auto : dirs) { 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 deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -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 9d1289b..ce7bfe1 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,163 +3,33 @@ // 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.wpilibj.DriverStation; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; + +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; + private double[] cameraPose; - private boolean lightOn; + public Pose2d getPose() { + //TODO - Get actual values! - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } + double x = 0; + double y = 0; + double yaw = 0; - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + Rotation2d rot = Rotation2d.fromDegrees(yaw); - 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; + return new Pose2d(x, y, rot); } @Override - public void periodic() {} -} + 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 diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -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 deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -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; - } -}