Simplied PoseEstimationSubystem and Robot#driveWithAlignmentCommand(...)

Author: jhhbrown1

src/main/java/frc/robot/CommandComposer.java

@@ -157,8 +157,7 @@ public static Command moveOnSquare(double sideLength, double distanceTolerance,
 	 * @return a {@code Command} for moving the robot on a circle
 	 */
 	public static Command moveOnCircle(double radius, double initialAngularVelocity, double finalAngularVelocity,
-			double distanceTolerance,
-			double angleTolerance, double timeout) {
+			double distanceTolerance, double angleTolerance, double timeout) {
 
 		Timer timer = new Timer();
 

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

@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import static edu.wpi.first.wpilibj2.command.Commands.*;
 import static frc.robot.Constants.*;
 import static frc.robot.Constants.RobotConstants.*;
 import static frc.robot.subsystems.PoseEstimationSubsystem.*;
@@ -15,6 +14,7 @@
 import org.littletonrobotics.urcl.URCL;
 import org.photonvision.PhotonCamera;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -31,6 +31,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import frc.robot.Constants.ControllerConstants;
 import frc.robot.Constants.ControllerConstants.Button;
+import frc.robot.commands.drive.DriveCommand2Controllers;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.PhotonCameraSimulator;
 import frc.robot.subsystems.PoseEstimationSubsystem;
@@ -64,6 +65,9 @@ public Robot() {
 		m_autoSelector.addOption("Test DriveSubsystem", m_driveSubsystem.testCommand());
 		SmartDashboard.putData("Auto Selector", m_autoSelector);
 
+		double distanceTolerance = 0.01;
+		double angleToleranceInDegrees = 1.0;
+
 		m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
 		m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
 		m_testSelector.addOption("Check DriveSubsystem (Robot-Oriented F/B/L/R/LR/RR)", m_driveSubsystem.testCommand());
@@ -74,15 +78,16 @@ public Robot() {
 		m_testSelector
 				.addOption(
 						"Check kDriveGearRatio and kWheelDiameter (F/B 6 feet)",
-						CommandComposer.moveForwardBackward2Controllers(6, 0.01, 1));
+						CommandComposer.moveForwardBackward2Controllers(6, distanceTolerance, angleToleranceInDegrees));
 		m_testSelector
 				.addOption(
 						"Check PID Constants for Driving (5'x5' Square)",
-						CommandComposer.moveOnSquare(Units.feetToMeters(5), 0.01, 1, 16));
+						CommandComposer
+								.moveOnSquare(Units.feetToMeters(5), distanceTolerance, angleToleranceInDegrees, 16));
 		m_testSelector
 				.addOption(
 						"Check PID Constants for Driving (Unit Circle)",
-						CommandComposer.moveOnCircle(1, 1, 3, 0.01, 1, 10));
+						CommandComposer.moveOnCircle(1, 1, 3, distanceTolerance, angleToleranceInDegrees, 10));
 		m_testSelector.addOption("Test Rotation", CommandComposer.testRotation());
 		m_testSelector.addOption("Turn toward Tag 1", CommandComposer.turnTowardTag(1));
 
@@ -114,7 +119,7 @@ public void bindDriveControls() {
 								() -> -m_driverController.getLeftY(),
 								() -> -m_driverController.getLeftX(),
 								() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
-								new Transform2d(0.5, 0, Rotation2d.fromDegrees(180)), 2));
+								new Transform2d(0.5, 0, Rotation2d.fromDegrees(180)), 2, 0.03, 3));
 	}
 
 	/**
@@ -130,20 +135,30 @@ public void bindDriveControls() {
 	 * @param robotToTag the {@code Tranform2d} representing the pose of the
 	 *        closest {@code AprilTag} relative to the robot when the robot is
 	 *        aligned
-	 * @param isFieldRelative {@code Supplier} for determining whether or not
-	 *        driving should be field relative.
+	 * @param distanceThresholdInMeters the maximum distance (in meters) within
+	 *        which {@code AprilTag}s are considered
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleToleranceInDegrees the angle error in degrees which is tolerable
 	 * @return a {@code Command} to automatically align the robot to the closest tag
 	 *         while driving the robot with joystick input
 	 */
 	Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
-			DoubleSupplier rotation, Transform2d robotToTag, double distanceThresholdInMeters) {
-		return run(() -> {
-			ChassisSpeeds speeds = DriveSubsystem.chassisSpeeds(forwardSpeed, strafeSpeed, rotation);
-			speeds = speeds.plus(
-					m_poseEstimationSubsystem
-							.chassisSpeedsTowardClosestTag(robotToTag, distanceThresholdInMeters));
-			m_driveSubsystem.drive(speeds, true);
-		});
+			DoubleSupplier rotation, Transform2d robotToTag, double distanceThresholdInMeters, double distanceTolerance,
+			double angleToleranceInDegrees) {
+		return new DriveCommand2Controllers(m_driveSubsystem, () -> {
+			Pose2d closestTagPose = m_poseEstimationSubsystem.closestTagPose(180, distanceThresholdInMeters);
+			if (closestTagPose == null)
+				return m_driveSubsystem.getPose();
+			return m_poseEstimationSubsystem.odometryCentricPose(closestTagPose.plus(robotToTag));
+		}, false, distanceTolerance, angleToleranceInDegrees) {
+
+			@Override
+			public ChassisSpeeds chassisSpeeds() {
+				ChassisSpeeds speeds = DriveSubsystem.chassisSpeeds(forwardSpeed, strafeSpeed, rotation);
+				return speeds.plus(super.chassisSpeeds());
+			}
+
+		};
 	}
 
 	@Override

src/main/java/frc/robot/commands/drive/DriveCommand2Controllers.java

@@ -203,6 +203,7 @@ public void initialize() {
 			m_targetPose = m_targetPoseSupplier.get();
 		} catch (Exception e) {
 			e.printStackTrace();
+			m_targetPose = pose;
 		}
 		m_controllerXY.setTolerance(m_distanceTolerance);
 		m_controllerYaw.setTolerance(m_angleTolerance);
@@ -216,27 +217,24 @@ public void initialize() {
 	 */
 	@Override
 	public void execute() {
+		m_driveSubsystem.drive(chassisSpeeds(), true);
+	}
+
+	/**
+	 * Calculates the {@code ChassisSpeeds} to move the robot toward the target.
+	 * 
+	 * @return the calculated {@code ChassisSpeeds} to move the robot toward the
+	 *         target
+	 */
+	public ChassisSpeeds chassisSpeeds() {
 		var currentPose = m_poseSupplier.get();
 		if (m_continuousTargeting)
 			try {
 				m_targetPose = m_targetPoseSupplier.get();
 			} catch (Exception e) {
 				// e.printStackTrace();
 			}
-		m_driveSubsystem.drive(chassisSpeeds(currentPose, m_targetPose), true);
-	}
-
-	/**
-	 * Calculates the {@code ChassisSpeeds} to move from the current {@code Pose2d}
-	 * toward the target {@code Pose2d}.
-	 * 
-	 * @param currentPose the current {@code Pose2d}
-	 * @param targetPose the target {@code Pose2d}
-	 * @return the calculated {@code ChassisSpeeds} to move from the current
-	 *         {@code Pose2d} toward the target {@code Pose2d}
-	 */
-	public ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose) {
-		Translation2d current2target = targetPose.getTranslation()
+		Translation2d current2target = m_targetPose.getTranslation()
 				.minus(currentPose.getTranslation());
 		double velocityX = 0, velocityY = 0;
 		double distance = current2target.getNorm();
@@ -247,7 +245,7 @@ public ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose) {
 			velocityY = speed * current2target.getAngle().getSin();
 		}
 		double angularVelocityRadiansPerSecond = m_controllerYaw
-				.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
+				.calculate(currentPose.getRotation().getRadians(), m_targetPose.getRotation().getRadians());
 		angularVelocityRadiansPerSecond = applyThreshold(angularVelocityRadiansPerSecond, kTurnMinAngularSpeed);
 		return new ChassisSpeeds(velocityX, velocityY, angularVelocityRadiansPerSecond);
 	}

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

@@ -1,7 +1,6 @@
 package frc.robot.subsystems;
 
 import static frc.robot.Constants.*;
-import static frc.robot.Constants.DriveConstants.*;
 
 import java.util.HashMap;
 import java.util.Map;
@@ -15,14 +14,12 @@
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -78,20 +75,6 @@ public class PoseEstimationSubsystem extends SubsystemBase {
 	 */
 	private final StructPublisher<Pose2d> m_estimatedPosePublisher;
 
-	/**
-	 * The {@code PIDController} for controlling the robot in the x and y
-	 * dimensions in meters (input: error in meters, output: velocity in meters per
-	 * second).
-	 */
-	private PIDController m_controllerXY;
-
-	/**
-	 * The {@code PIDController} for controlling the robot in the yaw
-	 * dimension in radians (input: error in radians, output: velocity in radians
-	 * per second).
-	 */
-	private PIDController m_controllerYaw;
-
 	/**
 	 * Constructs a {@code PoseEstimationSubsystem}.
 	 * 
@@ -113,9 +96,6 @@ public PoseEstimationSubsystem(DriveSubsystem driveSubsystem) {
 		m_estimatedPosePublisher = NetworkTableInstance.getDefault()
 				.getStructTopic("/SmartDashboard/Pose@PoseEstimationSubsystem", Pose2d.struct)
 				.publish();
-		m_controllerXY = new PIDController(kDriveP, kDriveI, kDriveD);
-		m_controllerYaw = new PIDController(kTurnP, kTurnI, kTurnD);
-		m_controllerYaw.enableContinuousInput(0, 2 * Math.PI);
 	}
 
 	/**
@@ -169,54 +149,15 @@ public Pose2d getEstimatedPose() {
 	}
 
 	/**
-	 * Calculates the {@code ChassisSpeeds} to move the robot toward the closest
-	 * {@code AprilTag}.
-	 * 
-	 * @param robotToTag the {@code Tranform2d} representing the pose of the
-	 *        {@code AprilTag} relative to the robot when the robot is aligned
-	 * @param distanceThresholdInMeters the maximum distance (in meters) within
-	 *        which {@code AprilTag}s are considered
-	 * @return the calculated {@code ChassisSpeeds} to move the robot toward the
-	 *         closest {@code AprilTag}
-	 */
-	public ChassisSpeeds chassisSpeedsTowardClosestTag(Transform2d robotToTag, double distanceThresholdInMeters) {
-		var currentRobotPose = getEstimatedPose();
-		var closestTagPose = closestTagPose(180, distanceThresholdInMeters);
-		if (closestTagPose == null)
-			return new ChassisSpeeds();
-		var targetRobotPose = closestTagPose.plus(robotToTag);
-		return chassisSpeeds(currentRobotPose, targetRobotPose, m_controllerXY, m_controllerYaw);
-	}
-
-	/**
-	 * Calculates the {@code ChassisSpeeds} to move from the current {@code Pose2d}
-	 * toward the target {@code Pose2d}.
+	 * Returns the odometry-centric {@code Pose2d} that corresponds to the specified
+	 * field-centric {@code Pose2d}.
 	 * 
-	 * @param currentPose the current {@code Pose2d}
-	 * @param targetPose the target {@code Pose2d}
-	 * @param controllerXY the {@code PIDController} for controlling the robot in
-	 *        the x and y dimensions in meters (input: error in meters, output:
-	 *        velocity in meters per second)
-	 * @param controllerYaw the {@code PIDController} for controlling the robot in
-	 *        the yaw dimension in radians (input: error in radians, output:
-	 *        velocity in radians per second)
-	 * @return the calculated {@code ChassisSpeeds} to move from the current
-	 *         {@code Pose2d} toward the target {@code Pose2d}
+	 * @param fieldCentricPose a field-centric {@code Pose2d}
+	 * @return the odometry-centric {@code Pose2d} that corresponds to the specified
+	 *         field-centric {@code Pose2d}
 	 */
-	public ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose, PIDController controllerXY,
-			PIDController controllerYaw) {
-		Translation2d current2target = targetPose.minus(currentPose).getTranslation()
-				.rotateBy(m_driveSubsystem.getPose().getRotation());
-		double velocityX = 0, velocityY = 0;
-		double distance = current2target.getNorm();
-		if (distance > 0) {
-			double speed = Math.abs(controllerXY.calculate(distance, 0));
-			velocityX = speed * current2target.getAngle().getCos();
-			velocityY = speed * current2target.getAngle().getSin();
-		}
-		double angularVelocityRadiansPerSecond = controllerYaw
-				.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
-		return new ChassisSpeeds(velocityX, velocityY, angularVelocityRadiansPerSecond);
+	public Pose2d odometryCentricPose(Pose2d fieldCentricPose) {
+		return m_driveSubsystem.getPose().plus(fieldCentricPose.minus(getEstimatedPose()));
 	}
 
 	/**