code used for today's tag alignment demo

Author: jhhbrown1

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

@@ -91,14 +91,15 @@ public static final class DriveConstants {
 
 		public static final int kEncoderDepth = 4;
 		public static final int kEncoderMeasurementPeriod = 16;
-		public static final int kSteerSmartCurrentLimit = 60;
+		public static final int kDriveSmartCurrentLimit = 60; // TODO: find a good value
+		public static final int kDrivePeakCurrentLimit = kDriveSmartCurrentLimit + 15;
+		public static final int kSteerSmartCurrentLimit = 60; // TODO: find a good value
 		public static final int kSteerPeakCurrentLimit = kSteerSmartCurrentLimit + 15;
 		// The amount of time to go from 0 to full power in seconds
 		public static final double kRampRate = .1;
 
 		// DriveCommand.java Constants
-		public static final double kDriveP = 6; // up to 1.0?
-		// public static final double kDriveP = 0.4; // up to 1.0?
+		public static final double kDriveP = 5; // TODO: find a good value
 		public static final double kDriveI = 0;
 		public static final double kDriveD = 0;
 		// public static final double kDriveMaxAcceleration = 2 * kDriveMaxSpeed; //
@@ -125,7 +126,7 @@ public static final class RobotConstants {
 		 * the pose of the robot.
 		 */
 		public static Transform3d kRobotToCamera1 = new Transform3d(new Translation3d(0.0, 0.0, 0.2),
-				new Rotation3d(0, Units.degreesToRadians(-20), 0));
+				new Rotation3d(0, Units.degreesToRadians(-10), 0));
 
 		/**
 		 * The {@code Transform3d} expressing the pose of the second camera relative to

src/main/java/frc/robot/Robot.java

@@ -90,39 +90,45 @@ public void bindDriveControls() {
 								DriveCommand.moveForward(m_driveSubsystem, 1.8, distanceTolerance, angleTolerance),
 								DriveCommand.moveForward(m_driveSubsystem, -1.8, distanceTolerance, angleTolerance)));
 
-		Transform2d robotToTarget = new Transform2d(1, 0, Rotation2d.fromDegrees(180));
+		Transform2d robotToTarget = new Transform2d(1.8, 0, Rotation2d.fromDegrees(180));
 		double angleOfCoverageInDegrees = 90;
 		double distanceThresholdInMeters = 4;
-		m_driverController.button(Button.kTriangle)
-				.whileTrue(
-						alignTest(1, robotToTarget, distanceTolerance, angleTolerance));
 		// m_driverController.button(Button.kTriangle)
 		// .whileTrue(
-		// AlignCommand.turnToClosestTag(
-		// m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
-		// distanceThresholdInMeters,
-		// distanceTolerance, angleTolerance));
+		// alignTest(1, robotToTarget, distanceTolerance, angleTolerance));
+
+		m_driverController.button(Button.kTriangle)
+				.whileTrue(
+						AlignCommand.turnToClosestTag(
+								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
+								distanceThresholdInMeters,
+								distanceTolerance, angleTolerance));
 
 		m_driverController.button(Button.kLeftBumper)
 				.whileTrue(
 						AlignCommand.moveToClosestTag(
 								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
 								distanceThresholdInMeters, robotToTarget,
 								distanceTolerance, angleTolerance));
-
 		m_driverController.button(Button.kRightBumper)
 				.whileTrue(
 						tourCommand(
 								m_driveSubsystem, m_poseEstimationSubystem, distanceTolerance,
 								angleTolerance,
-								robotToTarget, 12, 15, 14, 16, 17, 18, 19, 20, 21, 22));
+								robotToTarget, 1, 6, 7, 8, 2, 8, 7, 6, 1));
+		// m_driverController.button(Button.kRightBumper)
+		// .whileTrue(
+		// tourCommand(
+		// m_driveSubsystem, m_poseEstimationSubystem, distanceTolerance,
+		// angleTolerance,
+		// robotToTarget, 12, 15, 14, 16, 17, 18, 19, 20, 21, 22));
 	}
 
-	private Command alignTest(int tagIDs, Transform2d robotToTarget, double distanceTolerance, double angleTolerance) {
+	Command alignTest(int tagID, Transform2d robotToTarget, double distanceTolerance, double angleTolerance) {
 		return sequence(
 				AlignCommand.moveTo(
 						m_driveSubsystem, m_poseEstimationSubystem,
-						kFieldLayout.getTagPose(1).get().toPose2d().plus(robotToTarget),
+						kFieldLayout.getTagPose(tagID).get().toPose2d().plus(robotToTarget),
 						distanceTolerance, angleTolerance),
 				AlignCommand.moveTo(
 						m_driveSubsystem, m_poseEstimationSubystem,

src/main/java/frc/robot/SwerveModule.java

@@ -49,8 +49,17 @@ public SwerveModule(int canId, int drivePort, int steerPort) {
 		config.openLoopRampRate(kRampRate).closedLoopRampRate(kRampRate);
 		// Helps with encoder precision (not set in stone)
 		config.encoder.uvwAverageDepth(kEncoderDepth).uvwMeasurementPeriod(kEncoderMeasurementPeriod);
+		config.smartCurrentLimit(kDriveSmartCurrentLimit).secondaryCurrentLimit(kDrivePeakCurrentLimit);
+		m_driveMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+		config = new SparkMaxConfig();
+		config.idleMode(IdleMode.kBrake).voltageCompensation(12);
+		config.openLoopRampRate(kRampRate).closedLoopRampRate(kRampRate);
+		// Helps with encoder precision (not set in stone)
+		config.encoder.uvwAverageDepth(kEncoderDepth).uvwMeasurementPeriod(kEncoderMeasurementPeriod);
 		config.smartCurrentLimit(kSteerSmartCurrentLimit).secondaryCurrentLimit(kSteerPeakCurrentLimit);
 		m_steerMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
 		m_steerController.enableContinuousInput(0, 360);
 		if (RobotBase.isSimulation()) {
 			m_driveMotorModel = new DCMotorSim(

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

@@ -148,6 +148,50 @@ public Pose2d getEstimatedPose() {
 		return m_poseEstimator.getEstimatedPosition();
 	}
 
+	/**
+	 * Finds the {@code Pose2d} of the {@code AprilTag} that is closest (in terms of
+	 * angular displacement) to the specified {@code Pose2d}.
+	 * 
+	 * @param pose a {@code Pose2d}
+	 * @param distanceThresholdInMeters the maximum distance (in meters) within
+	 *        which {@code AprilTag}s are considered
+	 * @return the {@code Pose2d} of the {@code AprilTag} that is closest (in terms
+	 *         of angular displacement) to the specified {@code Pose2d}
+	 */
+	public Pose2d closestTagPose(Pose2d pose, double distanceThresholdInMeters) {
+		var i = closestTagID(pose, distanceThresholdInMeters);
+		return i == null ? null : kFieldLayout.getTagPose(i).get().toPose2d();
+	}
+
+	/**
+	 * Determines the ID of the {@code AprilTag} that is closest (in terms of
+	 * angular displacement) to the specified {@code Pose2d} ({@code null} if no
+	 * such {@code AprilTag}).
+	 * 
+	 * @param pose a {@code Pose2d}
+	 * @param distanceThresholdInMeters the maximum distance (in meters) within
+	 *        which {@code AprilTag}s are considered
+	 * @return the ID of the {@code AprilTag} that is closest (in terms of angular
+	 *         displacement) to the specified {@code Pose2d} ({@code null} if no
+	 *         such {@code AprilTag})
+	 */
+	public static Integer closestTagID(Pose2d pose, double distanceThresholdInMeters) {
+		var s = kFieldLayout.getTags().stream()
+				// consider only the tags facing toward the robot
+				.filter(
+						t -> Math.abs(
+								t.pose.getTranslation().toTranslation2d().minus(pose.getTranslation()).getAngle()
+										.minus(t.pose.toPose2d().getRotation()).getDegrees()) > 90)
+				.filter(t -> Math.abs(translationalDisplacement(pose, t.pose.toPose2d())) < distanceThresholdInMeters)
+				// only tags sufficently close
+				.map(t -> Map.entry(t.ID, Math.abs(angularDisplacement(pose, t.pose.toPose2d()).getDegrees())));
+		Optional<Entry<Integer, Double>> closest = s.reduce((e1, e2) -> e1.getValue() < e2.getValue() ? e1 : e2);
+		if (closest.isPresent()) {
+			return closest.get().getKey();
+		} else
+			return null;
+	}
+
 	/**
 	 * Finds the {@code Pose2d} of the {@code AprilTag} that is closest to the robot
 	 * ({@code null} if no such {@code AprilTag}).