Merge branch 'robototes:main' into main

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

@@ -36,6 +36,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;
@@ -59,6 +61,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();
@@ -84,6 +89,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();
 		}
@@ -116,9 +127,9 @@ 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		x
+		driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro));
+		// driveController
 		// 		.back()
 		// 		.onTrue(
 		// 				new InstantCommand(
@@ -127,6 +138,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.whileTrue(
+				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,44 @@
+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() {
+		if (limelightSubsystem.hasTargets()) {
+			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() || !limelightSubsystem.hasTargets());
+	}
+}

src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java

@@ -46,7 +46,7 @@ public class AprilTagsProcessor {
 					Units.inchesToMeters(27.0 / 2.0 - 0.94996),
 					0,
 					Units.inchesToMeters(8.12331),
-					new Rotation3d(0, Units.degreesToRadians(-30), 0));
+					new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0));
 
 	// TODO Measure these
 	private static final Vector<N3> STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(30));
@@ -61,14 +61,15 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) {
 
 	private static final double MAX_POSE_AMBIGUITY = 0.1;
 
-	// Both of these are in radians
-	// Negate pitch to go from robot/target to cam pitch to cam to robot/target pitch
-	private static final double EXPECTED_CAM_TO_TARGET_PITCH = -ROBOT_TO_CAM.getRotation().getY();
-	private static final double CAM_TO_TARGET_PITCH_TOLERANCE = 0.1;
+	// Radians
+	private static final double ROBOT_TO_TARGET_ROLL_TOLERANCE = 0.1;
+	private static final double ROBOT_TO_TARGET_PITCH_TOLERANCE = 0.1;
 
-	private static boolean hasCorrectPitch(Transform3d camToTarget) {
-		return Math.abs(camToTarget.getRotation().getY() - EXPECTED_CAM_TO_TARGET_PITCH)
-				< CAM_TO_TARGET_PITCH_TOLERANCE;
+	private static boolean hasValidRotation(Transform3d camToTarget) {
+		// Intrinsic robot to cam + cam to target = extrinsic cam to target + robot to cam
+		var robotToTargetRotation = camToTarget.getRotation().plus(ROBOT_TO_CAM.getRotation());
+		return (Math.abs(robotToTargetRotation.getX()) < ROBOT_TO_TARGET_ROLL_TOLERANCE)
+				&& (Math.abs(robotToTargetRotation.getY()) < ROBOT_TO_TARGET_PITCH_TOLERANCE);
 	}
 
 	private static PhotonTrackedTarget swapBestAndAltTransforms(PhotonTrackedTarget target) {
@@ -93,8 +94,8 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult
 				copy.targets.remove(i);
 				continue;
 			}
-			if (!hasCorrectPitch(target.getBestCameraToTarget())) {
-				if (hasCorrectPitch(target.getAlternateCameraToTarget())) {
+			if (!hasValidRotation(target.getBestCameraToTarget())) {
+				if (hasValidRotation(target.getAlternateCameraToTarget())) {
 					target = swapBestAndAltTransforms(target);
 					copy.targets.set(i, target);
 				} else {
@@ -112,12 +113,13 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult
 	private final FieldObject2d rawVisionFieldObject;
 
 	// These are always set with every pipeline result
+	private double lastRawTimestampSeconds = 0;
 	private PhotonPipelineResult latestResult = null;
 	private PhotonPipelineResult latestFilteredResult = null;
 	private Optional<EstimatedRobotPose> latestPose = Optional.empty();
 
 	// These are only set when there's a valid pose
-	private double lastTimestampSeconds = 0;
+	private double lastValidTimestampSeconds = 0;
 	private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d());
 
 	private static final AprilTagFieldLayout fieldLayout =
@@ -150,44 +152,70 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) {
 				event -> update());
 
 		ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags");
-		shuffleboardTab.addBoolean("Has targets", this::hasTargets).withPosition(0, 0).withSize(1, 1);
 		shuffleboardTab
-				.addInteger("Num targets", this::getNumTargets)
+				.addDouble("Last raw timestamp", this::getLastRawTimestampSeconds)
+				.withPosition(0, 0)
+				.withSize(1, 1);
+		shuffleboardTab
+				.addInteger("Raw num targets", this::getRawNumTargets)
 				.withPosition(0, 1)
 				.withSize(1, 1);
 		shuffleboardTab
-				.addDouble("Last timestamp", this::getLastTimestampSeconds)
+				.addInteger("Filtered num targets", this::getFilteredNumTargets)
+				.withPosition(1, 1)
+				.withSize(1, 1);
+		shuffleboardTab
+				.addBoolean("Has valid targets", this::hasTargets)
+				.withPosition(1, 2)
+				.withSize(1, 1);
+		shuffleboardTab
+				.addDouble("Last timestamp", this::getLastValidTimestampSeconds)
 				.withPosition(1, 0)
 				.withSize(1, 1);
 		shuffleboardTab
 				.add("3d pose on field", new SendablePose3d(this::getRobotPose))
 				.withPosition(2, 0)
-				.withSize(1, 6);
+				.withSize(2, 2);
 	}
 
 	public void update() {
 		latestResult = photonCamera.getLatestResult();
 		latestFilteredResult = filteredPipelineResult(latestResult);
+		lastRawTimestampSeconds = latestResult.getTimestampSeconds();
 		latestPose = photonPoseEstimator.update(latestFilteredResult);
 		if (latestPose.isPresent()) {
-			lastTimestampSeconds = latestPose.get().timestampSeconds;
+			lastValidTimestampSeconds = latestPose.get().timestampSeconds;
 			lastFieldPose = latestPose.get().estimatedPose.toPose2d();
 			rawVisionFieldObject.setPose(lastFieldPose);
-			aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastTimestampSeconds, STANDARD_DEVS);
+			aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastValidTimestampSeconds, STANDARD_DEVS);
 			var estimatedPose = aprilTagsHelper.getEstimatedPosition();
 			aprilTagsHelper.getField().setRobotPose(estimatedPose);
 			photonPoseEstimator.setLastPose(estimatedPose);
 		}
 	}
 
-	public boolean hasTargets() {
-		return latestPose.isPresent();
+	/**
+	 * Returns the timestamp of the last result we got (regardless of whether it has any valid
+	 * targets)
+	 *
+	 * @return The timestamp of the last result we got in seconds since FPGA startup.
+	 */
+	public double getLastRawTimestampSeconds() {
+		return lastRawTimestampSeconds;
 	}
 
-	public int getNumTargets() {
+	public int getRawNumTargets() {
 		return latestResult == null ? -1 : latestResult.getTargets().size();
 	}
 
+	public int getFilteredNumTargets() {
+		return latestFilteredResult == null ? -1 : latestFilteredResult.getTargets().size();
+	}
+
+	public boolean hasTargets() {
+		return latestPose.isPresent();
+	}
+
 	/**
 	 * Calculates the robot pose using the best target. Returns null if there is no known robot pose.
 	 *
@@ -205,7 +233,7 @@ public Pose3d getRobotPose() {
 	 *
 	 * @return The time we last saw an AprilTag in seconds since FPGA startup.
 	 */
-	public double getLastTimestampSeconds() {
-		return lastTimestampSeconds;
+	public double getLastValidTimestampSeconds() {
+		return lastValidTimestampSeconds;
 	}
 }

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

@@ -173,8 +173,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/IntakeSubsystem.java

@@ -101,14 +101,22 @@ public IntakeSubsystem() {
 		initShuffleboard();
 	}
 
-	private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) {
+	private void configureMotor(
+			CANSparkBase motor, int currentLimit, boolean invert, boolean enableLimitSwitch) {
 		motor.restoreFactoryDefaults();
 		motor.setIdleMode(IdleMode.kBrake);
 		motor.setSmartCurrentLimit(currentLimit);
 		motor.setInverted(invert);
+		motor
+				.getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen)
+				.enableLimitSwitch(enableLimitSwitch);
 		motor.burnFlash();
 	}
 
+	private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) {
+		configureMotor(motor, currentLimit, invert, false);
+	}
+
 	private void configureMotor(CANSparkBase motor, boolean invert) {
 		configureMotor(motor, 20, invert);
 	}
@@ -122,10 +130,8 @@ private void resetMotors() {
 		configureMotor(ingestMotor, false);
 		configureMotor(indexMotorUpper, 40, false);
 
-		configureMotor(feederMotor, 40, false);
-		indexMotorUpper
-				.getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen)
-				.enableLimitSwitch(false);
+		configureMotor(feederMotor, 40, true);
+
 		indexMotorUpper.burnFlash();
 	}