Update

- Remove unneeded limelight stuff
- Make base code for autoAlign

- Need to test with robot

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;
 

src/main/java/frc4388/robot/Robot.java

@@ -51,6 +51,11 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotPeriodic() {
+    // This has to be run after the m_robotContainer is initiated
+    // It will instantly return after it is run
+    m_robotContainer.autoAlign.updateAlliance();
+    
+    
     m_robotTime.updateTimes();
     //mled.updateLED();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled

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();
+    public 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
     );

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

@@ -0,0 +1,78 @@
+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 java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class AutoAlign extends Command {
+    private SwerveDrive swerve;
+    private Limelight limelight;
+    private Pose2d pose;
+
+    private Optional<Alliance> alliance;
+
+    public AutoAlign(SwerveDrive swerve, Limelight limelight) {
+        this.swerve = swerve;
+        this.limelight = limelight;
+    }
+
+    public void updateAlliance() {
+        if(alliance == null){
+            alliance = DriverStation.getAlliance();
+        }
+    }
+
+    private Translation2d calcMoveStick(){
+        //TODO - DO THE MATH!
+
+        final double curX = pose.getX();
+        final double curY = pose.getY();
+        final double curYaw = pose.getRotation().getDegrees();
+
+        final boolean isred = alliance.get() == DriverStation.Alliance.Red;
+
+        // 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!
+
+        final double curX = pose.getX();
+        final double curY = pose.getY();
+        final double curYaw = pose.getRotation().getDegrees();
+
+        final boolean isred = alliance.get() == DriverStation.Alliance.Red;
+
+        // 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() {
+        // Update limelight pos
+        pose = limelight.getPose();
+
+        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,7 +66,13 @@ public class PlaybackChooser {
     public void nextChooser() {
         SendableChooser<Command> 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()) {

src/main/java/frc4388/robot/subsystems/Apriltags.java

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

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