Revert "Make boiler plate code - Needs to be programmed with robot"

This reverts commit 0f7861ff02.

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;
 

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
     );

src/main/java/frc4388/robot/commands/Autos/AutoAlign.java

@@ -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);
-    }
-}

src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java

@@ -66,13 +66,7 @@ public class PlaybackChooser {
     public void nextChooser() {
         SendableChooser<Command> 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()) {

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);
+    }
+}

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<TargetCorner> getAprilCorners() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget().getDetectedCorners();
+  }
+
+  public double getAprilSkew() {
+    List<TargetCorner> corners = getAprilCorners();
+    ArrayList<TargetCorner> 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<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
+    if (box == null) return null;
+
+    ArrayList<TargetCorner> 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<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) 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]);
-  }
-}
+  public void periodic() {}
+}

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;
+    }
+}

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;
+    }
+}