added driveWithAlignment

Author: jhhbrown1

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

@@ -72,7 +72,8 @@ public static final class DriveConstants {
 		// public static final double kDriveMaxSpeed = 3.0; // 3 meters per second
 		public static final double kDriveMaxSpeed = 5.0; // 5 meters per second
 		public static final double kDriveMinSpeed = .3; // .3 meters per second // TODO: find a good value
-		public static final double kTurnMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+		public static final double kTurnMaxAngularSpeed = Math.toRadians(180); // 1/2 rotation per second
+		public static final double kTurnMinAngularSpeed = Math.toRadians(5); // 5 degrees per second
 		public static final double kDriveMaxVoltage = 12;
 
 		public static final double kDriveGearRatio = 6.12;

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

@@ -6,24 +6,22 @@
 
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 import static frc.robot.Constants.*;
-import static frc.robot.Constants.DriveConstants.*;
 import static frc.robot.Constants.RobotConstants.*;
 import static frc.robot.subsystems.PoseEstimationSubsystem.*;
 
 import java.util.Arrays;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.Map;
+import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.urcl.URCL;
 import org.photonvision.PhotonCamera;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 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.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -113,7 +111,11 @@ public void bindDriveControls() {
 
 		m_driverController.button(Button.kLeftBumper)
 				.whileTrue(
-						driveToClosestTag(new Transform2d(0.5, 0, Rotation2d.fromDegrees(180)), 2));
+						driveWithAlignmentCommand(
+								() -> -m_driverController.getLeftY(),
+								() -> -m_driverController.getLeftX(),
+								() -> m_driverController.getR2Axis() - m_driverController.getL2Axis(),
+								new Transform2d(0.5, 0, Rotation2d.fromDegrees(180)), 2));
 
 		// m_driverController.button(Button.kLeftBumper)
 		// .whileTrue(
@@ -144,45 +146,14 @@ public void bindDriveControls() {
 		// robotToTarget, 12, 15, 14, 16, 17, 18, 19, 20, 21, 22));
 	}
 
-	Command driveToClosestTag(Transform2d robotToTarget, double distanceThresholdInMeters) {
-		@SuppressWarnings("resource")
-		PIDController controllerXY = new PIDController(kDriveP, kDriveI, kDriveD);
-		controllerXY.setSetpoint(0);
-
-		@SuppressWarnings("resource")
-		PIDController controllerYaw = new PIDController(kTurnP, kTurnI, kTurnD);
-		controllerYaw.enableContinuousInput(-180, 180);
-		controllerYaw.setSetpoint(0);
+	Command driveWithAlignmentCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
+			DoubleSupplier rotation, Transform2d robotToTarget, double distanceThresholdInMeters) {
 
 		return run(() -> {
-
-			double fwdSpeed = MathUtil
-					.applyDeadband(-m_driverController.getLeftY(), ControllerConstants.kDeadzone);
-			fwdSpeed = Math.signum(fwdSpeed) * Math.pow(fwdSpeed, 2) * kDriveMaxSpeed;
-
-			double strSpeed = MathUtil
-					.applyDeadband(-m_driverController.getLeftX(), ControllerConstants.kDeadzone);
-			strSpeed = Math.signum(strSpeed) * Math.pow(strSpeed, 2) * kDriveMaxSpeed;
-
-			double rotSpeed = MathUtil.applyDeadband(
-					m_driverController.getR2Axis() - m_driverController.getL2Axis(), ControllerConstants.kDeadzone);
-			rotSpeed = Math.signum(rotSpeed) * Math.pow(rotSpeed, 2) * kTurnMaxAngularSpeed;
-
-			var pose = m_poseEstimationSubystem.getEstimatedPose();
-
-			var targetPose = m_poseEstimationSubystem
-					.closestTagPose(180, distanceThresholdInMeters);
-			if (targetPose != null) {
-				targetPose = targetPose.plus(robotToTarget);
-				Translation2d t = targetPose.getTranslation().minus(pose.getTranslation());
-				double speed = controllerXY.calculate(t.getNorm());
-				t = t.getNorm() > 0 ? t.times(-speed / t.getNorm()) : t; // translation toward target
-				fwdSpeed += t.getX();
-				strSpeed += t.getY();
-				rotSpeed += -controllerYaw
-						.calculate(targetPose.getRotation().minus(pose.getRotation()).getDegrees());
-			}
-			m_driveSubsystem.drive(fwdSpeed, strSpeed, rotSpeed, true);
+			ChassisSpeeds speeds = DriveSubsystem.chassisSpeeds(forwardSpeed, strafeSpeed, rotation);
+			speeds = speeds
+					.plus(m_poseEstimationSubystem.chassisSpeedsToClosestTag(robotToTarget, distanceThresholdInMeters));
+			m_driveSubsystem.drive(speeds.plus(speeds), true);
 		});
 	}
 

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

@@ -44,19 +44,22 @@ public class DriveCommand extends Command {
 
 	/**
 	 * The {@code ProfiledPIDController} for controlling the robot in the x
-	 * dimension in meters.
+	 * dimension in meters (input: error in meters, output: velocity in meters per
+	 * second).
 	 */
 	private ProfiledPIDController m_controllerX;
 
 	/**
 	 * The {@code ProfiledPIDController} for controlling the robot in the y
-	 * dimension in meters.
+	 * dimension in meters (input: error in meters, output: velocity in meters per
+	 * second).
 	 */
 	private ProfiledPIDController m_controllerY;
 
 	/**
 	 * The {@code ProfiledPIDController} for controlling the robot in the yaw
-	 * dimension in angles.
+	 * dimension in degrees (input: error in degrees, output: velocity in radians
+	 * per second).
 	 */
 	private ProfiledPIDController m_controllerYaw;
 

src/main/java/frc/robot/commands/DriveCommand2.java

@@ -45,13 +45,15 @@ public class DriveCommand2 extends Command {
 
 	/**
 	 * The {@code ProfiledPIDController} for controlling the robot in the x and y
-	 * dimensions in meters.
+	 * dimensions in meters (input: error in meters, output: velocity in meters per
+	 * second).
 	 */
 	protected ProfiledPIDController m_controllerXY;
 
 	/**
 	 * The {@code ProfiledPIDController} for controlling the robot in the yaw
-	 * dimension in angles.
+	 * dimension in degrees (input: error in degrees, output: velocity in radians
+	 * per second).
 	 */
 	protected ProfiledPIDController m_controllerYaw;
 
@@ -195,6 +197,7 @@ public void execute() {
 		double speedYaw = m_controllerYaw.calculate(pose.getRotation().getDegrees());
 		speedX = applyThreshold(speedX, kDriveMinSpeed);
 		speedY = applyThreshold(speedY, kDriveMinSpeed);
+		speedYaw = applyThreshold(speedYaw, kTurnMinAngularSpeed);
 		m_driveSubsystem.drive(speedX, speedY, speedYaw, true);
 	}
 

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

@@ -174,16 +174,61 @@ private void setModuleStates(SwerveModuleState[] states) {
 		m_backRight.setModuleState(states[3]);
 	}
 
+	/**
+	 * Creates a {@code chassisSpeeds} from the joystick input.
+	 *
+	 * @param forwardSpeed Forward speed supplier. Positive values make the robot
+	 *        go forward (+X direction).
+	 * @param strafeSpeed Strafe speed supplier. Positive values make the robot
+	 *        go to the left (+Y direction).
+	 * @param rotation Rotation speed supplier. Positive values make the
+	 *        robot rotate CCW.
+	 * @param isFieldRelative Supplier for determining if driving should be field
+	 *        relative.
+	 * @return {@code chassisSpeeds} from the joystick input
+	 */
+	public static ChassisSpeeds chassisSpeeds(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
+			DoubleSupplier rotation) {
+		// Get the forward, strafe, and rotation speed, using a deadband on the joystick
+		// input so slight movements don't move the robot
+		double vxMetersPerSecond = MathUtil.applyDeadband(forwardSpeed.getAsDouble(), ControllerConstants.kDeadzone);
+		vxMetersPerSecond = Math.signum(vxMetersPerSecond) * Math.pow(vxMetersPerSecond, 2) * kDriveMaxSpeed;
+
+		double vyMetersPerSecond = MathUtil.applyDeadband(strafeSpeed.getAsDouble(), ControllerConstants.kDeadzone);
+		vyMetersPerSecond = Math.signum(vyMetersPerSecond) * Math.pow(vyMetersPerSecond, 2) * kDriveMaxSpeed;
+
+		double omegaRadiansPerSecond = MathUtil.applyDeadband(rotation.getAsDouble(), ControllerConstants.kDeadzone);
+		omegaRadiansPerSecond = Math.signum(omegaRadiansPerSecond) * Math.pow(omegaRadiansPerSecond, 2)
+				* kTurnMaxAngularSpeed;
+
+		return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+	}
+
+	/**
+	 * Drives the robot.
+	 * 
+	 * @param vxMetersPerSecond forward velocity in meters per second
+	 * @param vyMetersPerSecond sideways velocity in meters per second
+	 * @param omegaRadiansPerSecond angular velocityin radians per second
+	 * @param isFieldRelative a boolean value indicating whether or not the
+	 *        veloicities are relative to the field
+	 */
+	public void drive(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond,
+			boolean isFieldRelative) {
+		setModuleStates(
+				calculateModuleStates(
+						new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond),
+						isFieldRelative));
+	}
+
 	/**
 	 * Drives the robot.
 	 * 
-	 * @param speedFwd The forward speed in meters per second
-	 * @param speedSide The sideways speed in meters per second
-	 * @param speedRot The rotation speed in radians per second
+	 * @param chassisSpeeds the {@code ChassisSpeeds} for the robot
 	 * @param isFieldRelative Whether or not the speeds are relative to the field
 	 */
-	public void drive(double speedFwd, double speedSide, double speedRot, boolean isFieldRelative) {
-		setModuleStates(calculateModuleStates(new ChassisSpeeds(speedFwd, speedSide, speedRot), isFieldRelative));
+	public void drive(ChassisSpeeds chassisSpeeds, boolean isFieldRelative) {
+		setModuleStates(calculateModuleStates(chassisSpeeds, isFieldRelative));
 	}
 
 	/**
@@ -218,18 +263,7 @@ public void periodic() {
 	public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
 			DoubleSupplier rotation, BooleanSupplier isFieldRelative) {
 		return run(() -> {
-			// Get the forward, strafe, and rotation speed, using a deadband on the joystick
-			// input so slight movements don't move the robot
-			double fwdSpeed = MathUtil.applyDeadband(forwardSpeed.getAsDouble(), ControllerConstants.kDeadzone);
-			fwdSpeed = Math.signum(fwdSpeed) * Math.pow(fwdSpeed, 2) * kDriveMaxSpeed;
-
-			double strSpeed = MathUtil.applyDeadband(strafeSpeed.getAsDouble(), ControllerConstants.kDeadzone);
-			strSpeed = Math.signum(strSpeed) * Math.pow(strSpeed, 2) * kDriveMaxSpeed;
-
-			double rotSpeed = MathUtil.applyDeadband(rotation.getAsDouble(), ControllerConstants.kDeadzone);
-			rotSpeed = Math.signum(rotSpeed) * Math.pow(rotSpeed, 2) * kTurnMaxAngularSpeed;
-
-			drive(fwdSpeed, strSpeed, rotSpeed, isFieldRelative.getAsBoolean());
+			drive(chassisSpeeds(forwardSpeed, strafeSpeed, rotation), isFieldRelative.getAsBoolean());
 		}).withName("DefaultDriveCommand");
 	}
 

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

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import static frc.robot.Constants.*;
+import static frc.robot.Constants.DriveConstants.*;
 
 import java.util.HashMap;
 import java.util.Map;
@@ -14,12 +15,14 @@
 
 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;
@@ -75,6 +78,20 @@ public class PoseEstimationSubsystem extends SubsystemBase {
 	 */
 	private final StructPublisher<Pose2d> m_estimatedPosePublisher;
 
+	/**
+	 * The {@code ProfiledPIDController} 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 ProfiledPIDController} for controlling the robot in the yaw
+	 * dimension in degrees (input: error in degrees, output: velocity in radians
+	 * per second).
+	 */
+	private PIDController m_controllerYaw;
+
 	/**
 	 * Constructs a {@code PoseEstimationSubsystem}.
 	 * 
@@ -96,6 +113,12 @@ public PoseEstimationSubsystem(DriveSubsystem driveSubsystem) {
 		m_estimatedPosePublisher = NetworkTableInstance.getDefault()
 				.getStructTopic("/SmartDashboard/Pose@PoseEstimationSubsystem", Pose2d.struct)
 				.publish();
+		m_controllerXY = new PIDController(kDriveP, kDriveI, kDriveD);
+		m_controllerXY.setSetpoint(0);
+
+		m_controllerYaw = new PIDController(kTurnP, kTurnI, kTurnD);
+		m_controllerYaw.enableContinuousInput(-180, 180);
+		m_controllerYaw.setSetpoint(0);
 	}
 
 	/**
@@ -148,6 +171,31 @@ public Pose2d getEstimatedPose() {
 		return m_poseEstimator.getEstimatedPosition();
 	}
 
+	/**
+	 * Calculates the {@code ChassisSpeeds} for moving to the closest
+	 * {@code AprilTag}.
+	 * 
+	 * @param robotToTarget the {@code Tranform2d} representing the pose of the
+	 *        target relative to the robot
+	 * @param distanceThresholdInMeters the maximum distance (in meters) within
+	 *        which {@code AprilTag}s are considered
+	 * @return the calculated {@code ChassisSpeeds} for moving to the closest
+	 *         {@code AprilTag}
+	 */
+	public ChassisSpeeds chassisSpeedsToClosestTag(Transform2d robotToTarget, double distanceThresholdInMeters) {
+		var pose = getEstimatedPose();
+		var targetPose = closestTagPose(180, distanceThresholdInMeters);
+		if (targetPose != null) {
+			targetPose = targetPose.plus(robotToTarget);
+			Translation2d t = targetPose.getTranslation().minus(pose.getTranslation());
+			double speed = m_controllerXY.calculate(t.getNorm());
+			t = t.getNorm() > 0 ? t.times(-speed / t.getNorm()) : t;
+			return new ChassisSpeeds(t.getX(), t.getY(), -m_controllerYaw
+					.calculate(targetPose.getRotation().minus(pose.getRotation()).getDegrees()));
+		}
+		return new ChassisSpeeds();
+	}
+
 	/**
 	 * Finds the {@code Pose2d} of the {@code AprilTag} that is closest (in terms of
 	 * angular displacement) to the robot when the specified {@code Translation2d}