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

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

@@ -5,6 +5,7 @@
 import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
 import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED;
 import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED;
+import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -29,6 +30,7 @@ public static class ControlConstants {
 
 	private final CommandXboxController driveController;
 	private final CommandXboxController codriveController;
+	private final Trigger getWithinDistanceTrigger;
 
 	// Intake
 	private final Trigger driveIntakeInButton;
@@ -49,6 +51,7 @@ public static class ControlConstants {
 	public Controls(Subsystems s) {
 		driveController = new CommandXboxController(CONTROLLER_PORT);
 		codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT);
+		getWithinDistanceTrigger = driveController.start();
 		this.s = s;
 
 		// launcherAmpPresetButton = codriveController.povDown();
@@ -66,6 +69,13 @@ public Controls(Subsystems s) {
 		if (DRIVEBASE_ENABLED) {
 			bindDrivebaseControls();
 		}
+		if (LIMELIGHT_ENABLED) {
+			bindLimelightControls();
+		}
+		// if (LAUNCHER_ENABLED) {
+		//	bindLauncherControls();
+		// }
+
 		if (LAUNCHER_ENABLED) {
 			bindLauncherControls();
 		}
@@ -95,10 +105,12 @@ 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));
 	}
 
+	public void bindLimelightControls() {
+		getWithinDistanceTrigger.onTrue(s.limelightSubsystem.getWithinDistance(s.drivebaseSubsystem));
+	}
 	// intake controls
 	private void bindIntakeControls() {
 		// CommandScheduler.getInstance()
@@ -131,7 +143,6 @@ private void bindLauncherControls() {
 		//				s.launcherSubsystem,
 		//				LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM,
 		//				LauncherSubsystem.SUBWOOFER_AIM_ANGLE));
-
 	}
 
 	public void vibrateDriveController(double vibration) {

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

@@ -6,6 +6,7 @@
 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 {
@@ -14,7 +15,7 @@ public static class SubsystemConstants {
 		private static final boolean IS_COMP = Robot.getInstance().isCompetition();
 
 		public static final boolean APRILTAGS_ENABLED = false;
-		public static final boolean LIMELIGHT_ENABLED = false;
+		public static final boolean LIMELIGHT_ENABLED = true;
 		public static final boolean CLIMB_ENABLED = false;
 		public static final boolean LAUNCHER_ENABLED = false;
 		public static final boolean INTAKE_ENABLED = false;
@@ -24,6 +25,7 @@ public static class SubsystemConstants {
 	public final DrivebaseWrapper drivebaseWrapper;
 	public final DrivebaseSubsystem drivebaseSubsystem;
 	public final LauncherSubsystem launcherSubsystem;
+	public LimelightSubsystem limelightSubsystem;
 	public final IntakeSubsystem intakeSubsystem;
 	public final AprilTagsProcessor apriltagsProcessor;
 
@@ -46,6 +48,9 @@ public Subsystems() {
 		} else {
 			launcherSubsystem = null;
 		}
+		if (LIMELIGHT_ENABLED) {
+			limelightSubsystem = new LimelightSubsystem();
+		}
 		if (INTAKE_ENABLED) {
 			intakeSubsystem = new IntakeSubsystem();
 		} else {

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

@@ -0,0 +1,97 @@
+package frc.team2412.robot.commands.drivebase;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.DrivebaseSubsystem;
+import java.util.Map;
+import java.util.function.DoubleSupplier;
+
+public class DriveCommand extends Command {
+	private static final double TURBO_ROTATION_DEFAULT = 1.5;
+
+	private final DrivebaseSubsystem drivebaseSubsystem;
+	private final DoubleSupplier forward;
+	private final DoubleSupplier strafe;
+	private final DoubleSupplier rotation;
+	private final DoubleSupplier turboRotation;
+	// temporary because code is kinda messy
+	Rotation2d MAX_ROTATIONS_PER_SEC = Rotation2d.fromRotations(0.8574);
+
+	// shuffleboard
+	private static GenericEntry driveSpeedEntry =
+			Shuffleboard.getTab("Drivebase")
+					.addPersistent("Drive Speed 1", 1)
+					.withSize(2, 1)
+					.withWidget(BuiltInWidgets.kNumberSlider)
+					.withProperties(Map.of("Min", 0, "Max", 1))
+					.getEntry();
+	private static GenericEntry rotationSpeedEntry =
+			Shuffleboard.getTab("Drivebase")
+					.addPersistent("Rotation Speed 1", 1)
+					.withSize(2, 1)
+					.withWidget(BuiltInWidgets.kNumberSlider)
+					.withProperties(Map.of("Min", 0, "Max", 1))
+					.getEntry();
+	private static GenericEntry fieldOrientedEntry =
+			Shuffleboard.getTab("Drivebase")
+					.addPersistent("Field Oriented 1", true)
+					.withWidget(BuiltInWidgets.kToggleSwitch)
+					.getEntry();
+	private static GenericEntry turboRotationEntry =
+			Shuffleboard.getTab("Drivebase")
+					.addPersistent("Turbo Rotation Modifier 1", TURBO_ROTATION_DEFAULT)
+					.withWidget(BuiltInWidgets.kNumberSlider)
+					.withProperties(Map.of("Min", 0.5, "Max", 2.5))
+					.getEntry();
+
+	public DriveCommand(
+			DrivebaseSubsystem drivebaseSubsystem,
+			DoubleSupplier forward,
+			DoubleSupplier strafe,
+			DoubleSupplier rotation,
+			DoubleSupplier turboRotation) {
+		this.drivebaseSubsystem = drivebaseSubsystem;
+		this.forward = forward;
+		this.strafe = strafe;
+		this.rotation = rotation;
+		// this variable give the right trigger input
+		this.turboRotation = turboRotation;
+
+		addRequirements(drivebaseSubsystem);
+	}
+
+	@Override
+	public void execute() {
+		double rotationSpeedModifier =
+				rotationSpeedEntry.getDouble(1.0)
+						* (1
+								- (turboRotation.getAsDouble()
+										* (1 - turboRotationEntry.getDouble(TURBO_ROTATION_DEFAULT))));
+
+		double x = deadbandCorrection(-forward.getAsDouble());
+		double y = deadbandCorrection(strafe.getAsDouble());
+		double rot = deadbandCorrection(-rotation.getAsDouble());
+
+		// math for normalizing and cubing inputs
+		double magnitude = Math.pow(Math.min(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)), 1), 3);
+		double angle = Math.atan2(y, x);
+		double cubed_x = magnitude * Math.cos(angle);
+		double cubed_y = magnitude * Math.sin(angle);
+
+		drivebaseSubsystem.simpleDrive(
+				cubed_x * driveSpeedEntry.getDouble(1.0) * 4.4196, // convert from percent to m/s
+				cubed_y * driveSpeedEntry.getDouble(1.0) * 4.4196,
+				Rotation2d.fromRotations(
+						rot
+								* rotationSpeedModifier
+								* MAX_ROTATIONS_PER_SEC
+										.getRotations())); // convert from percent to rotations per second);
+	}
+
+	public double deadbandCorrection(double input) {
+		return Math.abs(input) < 0.05 ? 0 : (input - Math.signum(input) * 0.05) / 0.95;
+	}
+}

src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java

@@ -0,0 +1,43 @@
+/*package frc.team2412.robot.commands.limelight;
+
+
+public class GetWithinDistanceCommand extends Command {
+
+	private LimelightSubsystem limelightSubsystem;
+	private DrivebaseSubsystem drivebaseSubsystem;
+	private Pose2d currentPose;
+
+	private Command getToPositionCommand;
+
+	public GetWithinDistanceCommand(
+			LimelightSubsystem limelightSubsystem, DrivebaseSubsystem drivebaseSubsystem) {
+		this.limelightSubsystem = limelightSubsystem;
+		this.drivebaseSubsystem = drivebaseSubsystem;
+
+		// find target -> get current pose -> use current pose and target to calculate target pose ->
+		// move towards target pose
+
+	}
+
+	@Override
+	public void initialize() {
+		currentPose = drivebaseSubsystem.getPose();
+		getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem);
+		// limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem);
+		getToPositionCommand.initialize();
+	}
+
+	@Override
+	public void execute() {
+		getToPositionCommand.execute();
+		limelightSubsystem.getTargetPose(currentPose);
+	}
+
+	@Override
+	public void end(boolean interrupted) {}
+
+	@Override
+	public boolean isFinished() {
+		System.out.println("within distance");
+		return limelightSubsystem.isWithinDistance();
+	}*/

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

@@ -164,6 +164,20 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO
 		}
 	}
 
+	public void driveWithDoubleSupplierTurn(
+			Translation2d translation, Supplier<Double> rotation, boolean fieldOriented) {
+		// if we're requesting the robot to stay still, lock wheels in X formation
+		if (translation.getNorm() == 0 && rotation.get() == 0.0 && xWheelsEnabled) {
+			swerveDrive.lockPose();
+		}
+		if (rotationSetpoint != null) {
+			swerveDrive.drive(
+					translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false);
+		} else {
+			swerveDrive.drive(translation.unaryMinus(), rotation.get(), fieldOriented, false);
+		}
+	}
+
 	/**
 	 * Drives the robot using joystick inputs
 	 *
@@ -193,6 +207,14 @@ public Command driveJoystick(
 				});
 	}
 
+	public void simpleDrive(double forward, double strafe, Rotation2d rotation) {
+
+		ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0, 0, 0);
+		chassisSpeeds = new ChassisSpeeds(forward, -strafe, rotation.getRadians());
+
+		drive(chassisSpeeds);
+	}
+
 	// this might need to be put in its own file due to complexity
 	public Command rotateToAngle(Supplier<Rotation2d> angle, boolean endWhenAligned) {
 		Command alignCommand =