Make boiler plate code - Needs to be programmed with robot

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 {

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

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

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

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 double[] cameraPose;
-  private PhotonCamera cam;
-  private PhotonPoseEstimator photonPoseEstimator;
 
-  private boolean lightOn;
+  public Pose2d getPose() {
+    //TODO - Get actual values!
 
-  /** Creates a new Limelight. */
+    double x = 0;
-  public Limelight() {
+    double y = 0;
-    cam = new PhotonCamera(VisionConstants.NAME);
+    double yaw = 0;
-    cam.setDriverMode(false);
-  }
 
-  public void setLEDs(boolean on) {
+    Rotation2d rot = Rotation2d.fromDegrees(yaw);
-    lightOn = on;
-    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
-  }
 
-  public void toggleLEDs() {
+    return new Pose2d(x, y, rot);
-    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() {}
+  public void periodic() {
-}
+    // This method will be called once per scheduler run
+    cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]);
+  }
+}

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

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

src/main/java/frc4388/utility/AprilTag.java

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