Merge pull request #43 from frc937/limelight

Implement Limelight

src/main/java/frc/robot/Constants.java

@@ -84,4 +84,47 @@ public static class Intake {
     /** Speed we want to run the Intake at. */
     public static final double INTAKE_MOTOR_SPEED = 0.5;
   }
+
+  /** Holds contstants for the Limelights. */
+  public static class Limelight {
+    /** Constants for aiming Limelight. */
+    public static class AimingLimelight {
+      public static final String LIMELIGHT_NAME = "limelight";
+
+      /** The number of degrees the Limelight is mounted back from perfectly vertical */
+      public static final double MOUNT_ANGLE = 0;
+
+      /** The number of inches from the center of the Limelight lens to the floor */
+      public static final double MOUNT_HEIGHT = 0;
+
+      /** The height to the Amp Apriltag off the ground. */
+      public static final double AMP_APRILTAG_HEIGHT = 0;
+
+      /** How far in inches we want to be from the target when we shoot */
+      public static final double DISTANCE_FROM_TARGET = 0;
+
+      /**
+       * How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor
+       */
+      public static final double STEER_STRENGTH = 0;
+
+      /** How hard to drive toward the target. Same notation as above. */
+      public static final double DRIVE_STRENGTH = 0;
+
+      /** VERY BASIC speed limit to make sure we don't drive too fast towards the target. */
+      public static final double SPEED_LIMIT = 0;
+
+      /**
+       * When we're at or below this number of degrees from where we want to be, we'll consider the
+       * Limelight's aim routine "done"
+       */
+      public static final double TURN_DONE_THRESHOLD = 0;
+
+      /**
+       * When we're at or below this number of inches from the target distance, we'll consider the
+       * Limelight's drive routine "done"
+       */
+      public static final double DISTANCE_DONE_THRESHOLD = 0;
+    }
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.AimWithLimelight;
 import frc.robot.commands.DeployUrMom;
 import frc.robot.commands.EnterXMode;
 import frc.robot.commands.RunIntake;
@@ -27,6 +28,7 @@
 import frc.robot.commands.mailbox.RunBelts;
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.UrMom;
 import frc.robot.subsystems.mailbox.Mailbox;
 import frc.robot.subsystems.mailbox.MailboxBelts;
@@ -60,6 +62,10 @@ public class RobotContainer {
   /** Singleton instance of {@link Intake} for the whole robot. */
   public static Intake intake = new Intake();
 
+  /** Singleton instance of {@link Limelight} for aiming. */
+  public static Limelight limelight =
+      new Limelight(Constants.Limelight.AimingLimelight.LIMELIGHT_NAME);
+
   /** Singleton instance of {@link UrMom} for the whole robot. */
   public static UrMom urMom = new UrMom();
 
@@ -78,6 +84,18 @@ public class RobotContainer {
   private DeindexNote deindexNote = new DeindexNote();
   private FireNoteRoutine fireNote = new FireNoteRoutine();
   private RunIntake runIntake = new RunIntake();
+  private AimWithLimelight aimToAmp =
+      new AimWithLimelight(
+          limelight,
+          Constants.Limelight.AimingLimelight.STEER_STRENGTH,
+          Constants.Limelight.AimingLimelight.DISTANCE_FROM_TARGET,
+          Constants.Limelight.AimingLimelight.MOUNT_HEIGHT,
+          Constants.Limelight.AimingLimelight.MOUNT_ANGLE,
+          Constants.Limelight.AimingLimelight.DRIVE_STRENGTH,
+          Constants.Limelight.AimingLimelight.SPEED_LIMIT,
+          Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
+          Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
+          Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT);
   private DeployUrMom deployUrMom = new DeployUrMom();
 
   /*

src/main/java/frc/robot/commands/AimWithLimelight.java

@@ -0,0 +1,124 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Limelight;
+
+public class AimWithLimelight extends Command {
+  private Drive drive;
+  private Limelight limelight;
+
+  private boolean finished;
+  private int counter;
+
+  private double steerStrength,
+      distanceFromTarget,
+      mountHeight,
+      mountAngle,
+      driveStrength,
+      speedLimit,
+      turnDoneThreshold,
+      distanceDoneThreshold,
+      targetHeight;
+
+  /**
+   * Creates a new command to aim with the Limelight.
+   *
+   * @param limelight The Limelight subsystem to aim with.
+   * @param steerStrength How hard to turn towards the target; between 0 and 1.
+   * @param distanceFromTarget How far from in inches we want to be from the target.
+   * @param mountHeight The height of the center of the Limelight lens off the floor.
+   * @param mountAngle The number of degrees the Limelight is mounted back from perfectly vertical.
+   *     Positive means rotated such that the lens is facing up, and not down.
+   * @param driveStrength How hard to drive towards the target; between 0 and 1.
+   * @param speedLimit Basic speed limit to make sure we don't drive too fast. Percentage of max
+   *     speed the robot can go. 0 to 1.
+   * @param turnDoneThreshold The threshold in degrees when we consider the aiming done.
+   * @param distanceDoneThreshold The threshold in inches when we consider the aiming done.
+   * @param targetHeight The height of the target off the floor.
+   */
+  public AimWithLimelight(
+      Limelight limelight,
+      double steerStrength,
+      double distanceFromTarget,
+      double mountHeight,
+      double mountAngle,
+      double driveStrength,
+      double speedLimit,
+      double turnDoneThreshold,
+      double distanceDoneThreshold,
+      double targetHeight) {
+    this.steerStrength = steerStrength;
+    this.distanceFromTarget = distanceFromTarget;
+    this.mountHeight = mountHeight;
+    this.mountAngle = mountAngle;
+    this.driveStrength = driveStrength;
+    this.speedLimit = speedLimit;
+    this.turnDoneThreshold = turnDoneThreshold;
+    this.distanceDoneThreshold = distanceDoneThreshold;
+    this.targetHeight = targetHeight;
+    this.drive = RobotContainer.drive;
+    this.limelight = limelight;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(drive, limelight);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.finished = false;
+    this.counter = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (limelight.hasValidTarget()) {
+      double z = limelight.getTX() * steerStrength;
+      double yComponent =
+          distanceFromTarget
+              - ((targetHeight - mountHeight) / Math.tan((mountAngle + limelight.getTY())));
+      double y = yComponent * (Math.PI / 180.0) * driveStrength;
+      if (z > speedLimit) {
+        z = speedLimit;
+      }
+      drive.driveRobotOriented(new Translation2d(y * -1.0, 0.0), z);
+      boolean isAngled = Math.abs(limelight.getTX()) < turnDoneThreshold;
+      boolean isDistanced =
+          Math.abs(
+                  (distanceFromTarget)
+                      - ((targetHeight - mountHeight)
+                          / Math.tan((mountAngle + limelight.getTY()) * (Math.PI / 180.0))))
+              <= distanceDoneThreshold;
+      if (isAngled && isDistanced) {
+        counter++;
+        if (counter > 5) {
+          this.finished = true;
+        }
+      }
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return finished;
+  }
+}

src/main/java/frc/robot/subsystems/Limelight.java

@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.DoubleArraySubscriber;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.DoubleTopic;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** Subsystem for the Limelight 2+ that we use for vision. */
+public class Limelight extends SubsystemBase {
+
+  /* variable chaingun, I promise we use all of these */
+  private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber;
+  /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
+   * to understand what the heck the different indices in this array mean
+   */
+  private DoubleArraySubscriber botposSubscriber;
+  private DoubleSubscriber pipelineSubcriber;
+  private DoublePublisher pipelinePublisher;
+  private String name;
+
+  private String fmtPath(String end) {
+    return "/" + name + "/" + end;
+  }
+
+  /**
+   * Creates a new Limelight.
+   *
+   * @param name The hostname of the Limelight
+   */
+  public Limelight(String name) {
+    this.name = name;
+    tvSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tv")).subscribe(0.0);
+    txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0);
+    tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0);
+    taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0);
+    DoubleTopic pipelineTopic =
+        NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("pipeline"));
+    this.pipelineSubcriber = pipelineTopic.subscribe(0.0);
+    this.pipelinePublisher = pipelineTopic.publish();
+    double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    botposSubscriber =
+        NetworkTableInstance.getDefault()
+            .getDoubleArrayTopic(fmtPath("botpose"))
+            .subscribe(defaultBotpos);
+  }
+
+  /* now its time for getter method chaingun, which I have to write manually because VS Code */
+
+  /**
+   * Returns whether the Limelight has a valid target.
+   *
+   * @return Whether or not the Limelight has a valid target. True if it does, false if it doesn't.
+   */
+  public boolean hasValidTarget() {
+    return tvSubscriber.get() == 1.0;
+  }
+
+  /**
+   * Gets the horizontal offset from the crosshair to the currently active target.
+   *
+   * @return tx; the horizontal offset from the crosshair to the target.
+   */
+  public double getTX() {
+    return txSubscriber.get();
+  }
+
+  /**
+   * Gets the vertical offset from the crosshair to the currently active target.
+   *
+   * @return ty; the vertical offset from the crosshair to the target.
+   */
+  public double getTY() {
+    return tySubscriber.get();
+  }
+
+  /**
+   * Gets the area of the target, 0% to 100% of the image.
+   *
+   * @return ta; the area of the target.
+   */
+  public double getTA() {
+    return taSubscriber.get();
+  }
+
+  /**
+   * Returns the robot's current pose relative to the field.
+   *
+   * <p>Units are meters; 0,0 is at the center of the field.
+   *
+   * <p>Will only work if the Limelight can see an AprilTag and read it correctly.
+   *
+   * <p>Notably, the Z, roll, and pitch values will always be zero, since we snap the bot to the
+   * floor on the Limelight.
+   *
+   * @return A {@link Pose3d}; the robot's current position relative to the field.
+   */
+  public Pose3d getBotpose() {
+    double[] botpos = botposSubscriber.get();
+    return new Pose3d(
+        botpos[0], botpos[1], botpos[2], new Rotation3d(botpos[3], botpos[4], botpos[5]));
+  }
+
+  /**
+   * Gets the robot's current pose relative to the field.
+   *
+   * <p>Units are meters; 0,0 is at the center of the field.
+   *
+   * <p>Will only work if the Limelight can see an AprilTag and read it correctly.
+   *
+   * @return A {@link Pose2d}; the robot's current position relative to the field in two dimensions.
+   */
+  public Pose2d getBotpose2d() {
+    double[] botpos = botposSubscriber.get();
+    return new Pose2d(botpos[0], botpos[1], Rotation2d.fromDegrees(botpos[5]));
+  }
+
+  /**
+   * Returns the current Limelight pipeline number.
+   *
+   * @return the current Limelight pipeline number.
+   */
+  public double getLimelightPipeline() {
+    return pipelineSubcriber.get();
+  }
+
+  /**
+   * Sets the Limelight pipeline number.
+   *
+   * @param pipeline the pipeline number.
+   */
+  public void setLimelightPipeline(double pipeline) {
+    pipelinePublisher.set(pipeline);
+  }
+
+  /** Subsystem periodic; runs every scheduler run. */
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+
+  }
+}