PR for vision branch - working note detection and new drive method.

src/main/java/frc/team2412/robot/Controls.java

@@ -33,6 +33,8 @@ public static class ControlConstants {
 
 	private final CommandXboxController driveController;
 	private final CommandXboxController codriveController;
+	// leaving this code in in case we need to test outside of auto
+	// private final Trigger getWithinDistanceTrigger;
 
 	// Intake
 	private final Trigger driveIntakeInButton;
@@ -55,6 +57,9 @@ public static class ControlConstants {
 	public Controls(Subsystems s) {
 		driveController = new CommandXboxController(CONTROLLER_PORT);
 		codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT);
+		// not sure what drive team wants (or if the trigger is even needed since we are only using the
+		// command in auto)
+		// getWithinDistanceTrigger = driveController.start();
 		this.s = s;
 
 		launcherAmpPresetButton = codriveController.x();
@@ -75,6 +80,12 @@ public Controls(Subsystems s) {
 		if (DRIVEBASE_ENABLED) {
 			bindDrivebaseControls();
 		}
+		// leaving this code in in case we need to test outside of auto
+		/*
+		if (LIMELIGHT_ENABLED) {
+			bindLimelightControls();
+		}
+		*/
 		if (LAUNCHER_ENABLED) {
 			bindLauncherControls();
 		}
@@ -107,8 +118,8 @@ private void bindDrivebaseControls() {
 								driveController::getLeftY,
 								driveController::getLeftX,
 								() -> Rotation2d.fromRotations(driveController.getRightX())));
-		driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
 		driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels));
+		driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
 		// driveController
 		// 		.back()
 		// 		.onTrue(
@@ -118,6 +129,14 @@ private void bindDrivebaseControls() {
 		// 										new Pose2d(new Translation2d(1.3, 5.55), new Rotation2d(180)))));
 	}
 
+	// leaving this code in in case we need to test outside of auto
+	/*
+	public void bindLimelightControls() {
+		getWithinDistanceTrigger.onTrue(
+				new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem));
+	}
+	*/
+
 	// intake controls
 	private void bindIntakeControls() {
 		// CommandScheduler.getInstance()

src/main/java/frc/team2412/robot/Subsystems.java

@@ -6,13 +6,13 @@
 import frc.team2412.robot.subsystems.DrivebaseSubsystem;
 import frc.team2412.robot.subsystems.IntakeSubsystem;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
+import frc.team2412.robot.subsystems.LimelightSubsystem;
 import frc.team2412.robot.util.DrivebaseWrapper;
 
 public class Subsystems {
 	public static class SubsystemConstants {
 
 		private static final boolean IS_COMP = Robot.getInstance().isCompetition();
-
 		public static final boolean APRILTAGS_ENABLED = true;
 		public static final boolean LIMELIGHT_ENABLED = false;
 		public static final boolean CLIMB_ENABLED = false;
@@ -26,6 +26,7 @@ public static class SubsystemConstants {
 	public final DrivebaseWrapper drivebaseWrapper;
 	public final DrivebaseSubsystem drivebaseSubsystem;
 	public final LauncherSubsystem launcherSubsystem;
+	public final LimelightSubsystem limelightSubsystem;
 	public final IntakeSubsystem intakeSubsystem;
 	public final AprilTagsProcessor apriltagsProcessor;
 
@@ -48,6 +49,11 @@ public Subsystems() {
 		} else {
 			launcherSubsystem = null;
 		}
+		if (LIMELIGHT_ENABLED) {
+			limelightSubsystem = new LimelightSubsystem();
+		} else {
+			limelightSubsystem = null;
+		}
 		if (INTAKE_ENABLED) {
 			intakeSubsystem = new IntakeSubsystem();
 		} else {

src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java

@@ -0,0 +1,42 @@
+package frc.team2412.robot.commands.drivebase;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.DrivebaseSubsystem;
+import frc.team2412.robot.subsystems.LimelightSubsystem;
+
+public class DriveToNoteCommand extends Command {
+
+	private final DrivebaseSubsystem drivebaseSubsystem;
+	private final LimelightSubsystem limelightSubsystem;
+
+	// limelight placement might be different so this multiplier is convenient
+	private final double INVERT_DRIVE_DIRECTION = -1.0;
+
+	public DriveToNoteCommand(
+			DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) {
+		this.drivebaseSubsystem = drivebaseSubsystem;
+		this.limelightSubsystem = limelightSubsystem;
+		addRequirements(drivebaseSubsystem);
+	}
+
+	@Override
+	public void execute() {
+		Translation2d move =
+				new Translation2d(
+						INVERT_DRIVE_DIRECTION
+								* Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()),
+						0.0);
+		Rotation2d turn =
+				new Rotation2d()
+						.fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset());
+		drivebaseSubsystem.drive(move, turn, false);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return (limelightSubsystem.isWithinDistance());
+	}
+}

src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java

@@ -169,8 +169,7 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO
 		// if we're requesting the robot to stay still, lock wheels in X formation
 		if (translation.getNorm() == 0 && rotation.getRotations() == 0 && xWheelsEnabled) {
 			swerveDrive.lockPose();
-		}
-		if (rotationSetpoint != null) {
+		} else if (rotationSetpoint != null) {
 			swerveDrive.drive(
 					translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false);
 		} else {

src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java

@@ -0,0 +1,146 @@
+package frc.team2412.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.Map;
+
+public class LimelightSubsystem extends SubsystemBase {
+
+	// POTENTIAL CONSTANTS
+	private GenericEntry GOAL_DISTANCE_FROM_NOTE;
+
+	// MEMBERS
+
+	final NetworkTable networkTable;
+
+	String currentPoseString;
+	String targetPoseString;
+
+	// network tables
+
+	// CONSTRUCTOR !
+	public LimelightSubsystem() {
+
+		// camera stream at http://10.24.12.11:5800
+
+		// logging
+		currentPoseString = "";
+		targetPoseString = "";
+
+		networkTable = NetworkTableInstance.getDefault().getTable("limelight");
+		ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight");
+
+		// helpers
+		setPipeline();
+
+		limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1);
+		limelightTab
+				.addDouble("Horizontal Offset", this::getHorizontalOffset)
+				.withPosition(1, 0)
+				.withSize(1, 1);
+		limelightTab
+				.addDouble("Vertical Offset", this::getVerticalOffset)
+				.withPosition(2, 0)
+				.withSize(1, 1);
+		limelightTab
+				.addDouble("Distance From Note", this::getDistanceFromTargetInches)
+				.withPosition(3, 0)
+				.withSize(1, 1);
+		limelightTab
+				.addString("Current Pose ", this::getCurrentPoseString)
+				.withPosition(0, 1)
+				.withSize(4, 1);
+		limelightTab
+				.addString("Target Pose ", this::getTargetPoseString)
+				.withPosition(0, 2)
+				.withSize(4, 1);
+		GOAL_DISTANCE_FROM_NOTE =
+				limelightTab
+						.addPersistent("Goal distance", 20.0)
+						.withWidget(BuiltInWidgets.kNumberSlider)
+						.withPosition(4, 0)
+						.withSize(2, 1)
+						.withProperties(Map.of("Min", 0.0, "Max", 30.0))
+						.getEntry();
+	}
+
+	private void setPipeline() {
+		networkTable.getEntry("pipeline").setNumber(0);
+	}
+
+	// METHODS
+
+	public boolean hasTargets() {
+		return (networkTable.getEntry("tv").getDouble(0) != 0);
+	}
+
+	public double getHorizontalOffset() {
+		return networkTable.getEntry("tx").getDouble(0);
+	}
+
+	public double getVerticalOffset() {
+		return networkTable.getEntry("ty").getDouble(0);
+	}
+
+	public double getBoxWidth() {
+		return networkTable.getEntry("tlong").getDouble(0);
+	}
+	// TODO re-measure distances to make this right
+	public double getDistanceFromTargetInches() {
+
+		// focal length = (P x D) / W
+		double focal_length = 559.0394;
+
+		// distance = (W x F) / P
+		// returns inches for testing purposes, will divide by 39.3700787 to return meters
+		return (14 * focal_length) / getBoxWidth();
+	}
+
+	// tan(degree) * distance = sideways distance
+
+	// target height / tan(vertical angle)
+
+	public Pose2d getTargetPose(Pose2d currentPose) {
+
+		// math thing to get target pose using current pose
+
+		Rotation2d targetHeading =
+				new Rotation2d(
+						currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset()));
+
+		double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches());
+
+		Translation2d translationOffset = new Translation2d(targetDistance, targetHeading);
+		Pose2d targetPose =
+				new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading);
+
+		currentPoseString = currentPose.toString();
+		targetPoseString = targetPose.toString();
+
+		return targetPose;
+	}
+
+	public String getCurrentPoseString() {
+		return currentPoseString;
+	}
+
+	public String getTargetPoseString() {
+		return targetPoseString;
+	}
+
+	public boolean isWithinDistance() {
+		return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0));
+	}
+
+	@Override
+	public void periodic() {}
+}