diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60d5b818..2a9f5de5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d9bf115..8d20513b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); /* diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java new file mode 100644 index 00000000..2735a64f --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 00000000..56b3ef4f --- /dev/null +++ b/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. + * + *

Units are meters; 0,0 is at the center of the field. + * + *

Will only work if the Limelight can see an AprilTag and read it correctly. + * + *

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. + * + *

Units are meters; 0,0 is at the center of the field. + * + *

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