Merge branch 'auto-align' into unit_tests

Author: jhhbrown1

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

@@ -126,8 +126,8 @@ public void initialize() {
 		m_controllerXY.setTolerance(m_distanceTolerance);
 		m_controllerYaw.setTolerance(m_angleTolerance);
 		if (m_previous == null) {
-			m_controllerXY.reset(m_targetPose.getTranslation().minus(pose.getTranslation()).getNorm());
-			m_controllerYaw.reset(m_targetPose.getRotation().minus(pose.getRotation()).getDegrees());
+			m_controllerXY.reset(m_targetPose.minus(pose).getTranslation().getNorm());
+			m_controllerYaw.reset(pose.getRotation().getDegrees());
 		}
 	}
 

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

@@ -8,10 +8,10 @@
 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.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.PoseEstimationSubsystem;
 
 /**
  * This {@code DriveCommand} aims to maneuver the robot to a certain
@@ -133,8 +133,6 @@ public DriveCommand2Controllers(DriveSubsystem driveSubsystem, Supplier<Pose2d>
 						new TrapezoidProfile.Constraints(Math.toDegrees(kTurnMaxAngularSpeed),
 								Math.toDegrees(kTurnMaxAcceleration))));
 		m_controllerYaw.enableContinuousInput(-180, 180);
-		m_controllerXY.setGoal(0);
-		m_controllerYaw.setGoal(0);
 	}
 
 	/**
@@ -188,8 +186,8 @@ public void initialize() {
 		}
 		m_controllerXY.setTolerance(m_distanceTolerance);
 		m_controllerYaw.setTolerance(m_angleTolerance);
-		m_controllerXY.reset(m_targetPose.getTranslation().minus(pose.getTranslation()).getNorm());
-		m_controllerYaw.reset(m_targetPose.getRotation().minus(pose.getRotation()).getDegrees());
+		m_controllerXY.reset(m_targetPose.minus(pose).getTranslation().getNorm());
+		m_controllerYaw.reset(pose.getRotation().getDegrees());
 	}
 
 	/**
@@ -198,9 +196,21 @@ public void initialize() {
 	 */
 	@Override
 	public void execute() {
-		var speeds = PoseEstimationSubsystem
-				.chassisSpeeds(m_poseSupplier.get(), m_targetPose, m_controllerXY, m_controllerYaw);
-		m_driveSubsystem.drive(speeds, true);
+		var currentPose = m_poseSupplier.get();
+		Translation2d translationalDisplacement = m_targetPose.getTranslation()
+				.minus(currentPose.getTranslation());
+		double velocityX = 0, velocityY = 0;
+		double distance = translationalDisplacement.getNorm();
+		if (distance > 0) {
+			double speed = Math.abs(m_controllerXY.calculate(distance, 0));
+			speed = applyThreshold(speed, kDriveMinSpeed);
+			velocityX = speed * translationalDisplacement.getAngle().getCos();
+			velocityY = speed * translationalDisplacement.getAngle().getSin();
+		}
+		double angularVelocityRadiansPerSecond = m_controllerYaw
+				.calculate(currentPose.getRotation().getDegrees(), m_targetPose.getRotation().getDegrees());
+		angularVelocityRadiansPerSecond = applyThreshold(angularVelocityRadiansPerSecond, kTurnMinAngularSpeed);
+		m_driveSubsystem.drive(velocityX, velocityY, angularVelocityRadiansPerSecond, true);
 	}
 
 	/**

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

@@ -9,7 +9,6 @@
 import java.util.Map;
 import java.util.Map.Entry;
 import java.util.Optional;
-import java.util.function.Function;
 
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -19,7 +18,6 @@
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -118,11 +116,8 @@ public PoseEstimationSubsystem(DriveSubsystem driveSubsystem) {
 				.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);
 	}
 
 	/**
@@ -152,7 +147,7 @@ public void periodic() {
 			var poseEstimator = e.getValue();
 			for (var r : camera.getAllUnreadResults()) { // for every result r
 				if (firstCamera || r.getTargets().size() > 1) {
-					Optional<EstimatedRobotPose> p = poseEstimator.update(r); // assign estimated pose to e
+					Optional<EstimatedRobotPose> p = poseEstimator.update(r);
 					if (p.isPresent()) { // if successful
 						EstimatedRobotPose v = p.get(); // get successfully estimated pose
 						m_poseEstimator.addVisionMeasurement(v.estimatedPose.toPose2d(), v.timestampSeconds);
@@ -201,71 +196,29 @@ public ChassisSpeeds chassisSpeedsTowardClosestTag(Transform2d robotToTag, doubl
 	 * 
 	 * @param currentPose the current {@code Pose2d}
 	 * @param targetPose the target {@code Pose2d}
-	 * @param contollerXY 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 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 degrees (input: error in degrees, output:
 	 *        velocity in radians per second)
 	 * @return the calculated {@code ChassisSpeeds} to move from the current
 	 *         {@code Pose2d} toward the target {@code Pose2d}
 	 */
-	public static ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose, PIDController contollerXY,
+	public static ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose, PIDController controllerXY,
 			PIDController controllerYaw) {
-		return chassisSpeeds(currentPose, targetPose, d -> contollerXY.calculate(d), a -> controllerYaw.calculate(a));
-	}
-
-	/**
-	 * Calculates the {@code ChassisSpeeds} to move from the current {@code Pose2d}
-	 * towards the target {@code Pose2d}.
-	 * 
-	 * @param currentPose the current {@code Pose2d}
-	 * @param targetPose the target {@code Pose2d}
-	 * @param contollerXY 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)
-	 * @param controllerYaw the {@code ProfiledPIDController} for controlling the
-	 *        robot in the yaw dimension in degrees (input: error in degrees,
-	 *        output: velocity in radians per second)
-	 * @return the calculated {@code ChassisSpeeds} to move from the current
-	 *         {@code Pose2d} towards the target {@code Pose2d}
-	 */
-	public static ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose,
-			ProfiledPIDController contollerXY, ProfiledPIDController controllerYaw) {
-		return chassisSpeeds(currentPose, targetPose, d -> contollerXY.calculate(d), a -> controllerYaw.calculate(a));
-	}
-
-	/**
-	 * Calculates the {@code ChassisSpeeds} to move from the current {@code Pose2d}
-	 * towards the target {@code Pose2d}.
-	 * 
-	 * @param currentPose the current {@code Pose2d}
-	 * @param targetPose the target {@code Pose2d}
-	 * @param contollerXY the {@code Function<Double, Double>} 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 Function<Double, Double>} for controlling the
-	 *        robot in the yaw dimension in degrees (input: error in degrees,
-	 *        output: velocity in radians per second)
-	 * @return the calculated {@code ChassisSpeeds} to move from the current
-	 *         {@code Pose2d} towards the target {@code Pose2d}
-	 */
-	public static ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose,
-			Function<Double, Double> contollerXY, Function<Double, Double> controllerYaw) {
 		Translation2d translationalDisplacement = targetPose.getTranslation()
 				.minus(currentPose.getTranslation());
 		double velocityX = 0, velocityY = 0;
 		double distance = translationalDisplacement.getNorm();
 		if (distance > 0) {
-			// apply(double) returns a non-positive value (setpoint: 0)
-			double speed = -contollerXY.apply(distance);
+			double speed = controllerXY.calculate(0.0, distance);
 			speed = applyThreshold(speed, kDriveMinSpeed);
 			velocityX = speed * translationalDisplacement.getAngle().getCos();
 			velocityY = speed * translationalDisplacement.getAngle().getSin();
 		}
-		Rotation2d angularDisplacement = targetPose.getRotation().minus(currentPose.getRotation());
-		// apply(double) returns a non-positive value (setpoint: 0)
-		double angularVelocityRadiansPerSecond = -controllerYaw.apply(angularDisplacement.getDegrees());
+		double angularVelocityRadiansPerSecond = controllerYaw
+				.calculate(currentPose.getRotation().getDegrees(), targetPose.getRotation().getDegrees());
 		angularVelocityRadiansPerSecond = applyThreshold(angularVelocityRadiansPerSecond, kTurnMinAngularSpeed);
 		return new ChassisSpeeds(velocityX, velocityY, angularVelocityRadiansPerSecond);
 	}