fix: Corrected logic on target pose calculation

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

@@ -4,14 +4,15 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
 
 public final class Constants {
 	public final static class TagAlignConstants {
-		public final static double kTagPIDkPxy = 0;
+		public final static double kTagPIDkPxy = 0; // xy PID constants
 		public final static double kTagPIDkIxy = 0;
 		public final static double kTagPIDkDxy = 0;
 
-		public final static double kTagPIDkPomega = 0;
+		public final static double kTagPIDkPomega = 0; // omega PID constants
 		public final static double kTagPIDkIomega = 0;
 		public final static double kTagPIDkDomega = 0;
 
@@ -58,6 +59,17 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i
 			StageCenterCenter,
 			StageCenterRight // the right side of the center stage
 		} //stage left, stage right, and stage center are the locations written in the game manual pg. 28
+
+		public static final double kFieldWidth = 651.25 * 0.0254; // Width of the field in meters
+
+		public static final NetworkTableInstance kNTInstance = NetworkTableInstance.create(); // Ideally all network table
+																																													// instances should be put in
+																																													// a wrapper and only the
+																																													// default instance should be
+																																													// used.
+		// TODO: write unit tests that write to NT limelight and verify PID output
+		// direction is correct. Needs to be implemented after drive since it uses pose
+		// estimator.
 	}
 
 	public final static class DriveConstants {

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

@@ -7,8 +7,9 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.DoubleArrayEntry;
 import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants.DriveConstants;
@@ -18,9 +19,9 @@
 public class AlignToTagCommand extends CommandBase {
 
 	final Subsystem m_subsystem; // TODO: change type to drive subsystem type
-	final Pose2d m_target;
+	final Pose2d m_target; // Desired pose
 	final PIDController m_PIDxy;
-	final PIDController m_PIDomega;
+	final PIDController m_PIDomega; // Omega is used to denote robot heading in radians
 
 	double m_consecCount; // counter of number of consecutive cycles within tolerance
 
@@ -33,17 +34,31 @@ public class AlignToTagCommand extends CommandBase {
 	public AlignToTagCommand(Subsystem subsystem, AlignPosition alignPos) {
 		m_subsystem = subsystem;
 		addRequirements(m_subsystem);
-		m_target = TagAlignConstants.kTargetPoses[alignPos.ordinal()];
+
+		final Pose2d redTarget = TagAlignConstants.kTargetPoses[alignPos.ordinal()];
+
+		// Blue positions are determined by flipping the x position and angle across the
+		// center. If this logic does not work, then hard coded values may be better
+		// (and faster). Indexing can be done by adding a constant to the ordinal when
+		// indexing the array. This will allows accessing the blue positions which
+		// should be put in the array after the red ones.
+		m_target = DriverStation.getAlliance().compareTo(Alliance.Red) == 0 ? redTarget
+				: new Pose2d(new Translation2d( // Flip poses of alliance is blue
+						TagAlignConstants.kFieldWidth - redTarget.getX(),
+						redTarget.getY()),
+						new Rotation2d(MathUtil.angleModulus(Math.PI - redTarget.getRotation().getRadians())));
+
 		m_PIDxy = new PIDController(TagAlignConstants.kTagPIDkPxy, TagAlignConstants.kTagPIDkIxy,
-				TagAlignConstants.kTagPIDkDxy);
+				TagAlignConstants.kTagPIDkDxy); // A feed forward control might also be needed here to overcome static friction
 		m_PIDomega = new PIDController(TagAlignConstants.kTagPIDkPomega, TagAlignConstants.kTagPIDkIomega,
 				TagAlignConstants.kTagPIDkDomega);
+
 		m_PIDomega.enableContinuousInput(-Math.PI, Math.PI);
 	}
 
 	@Override
 	public void initialize() {
-		m_PIDxy.reset();
+		m_PIDxy.reset(); // If a command instance is never reused then these three lines won't be needed
 		m_PIDomega.reset();
 		m_consecCount = 0;
 	}
@@ -57,11 +72,11 @@ public void execute() {
 
 		double ySpeed = MathUtil.clamp(
 				m_PIDxy.calculate(current.getY(), m_target.getY()),
-				-0, 0);
+				-0, 0); // TODO: change clamp parameters and units
 
 		double omegaSpeed = MathUtil.clamp(
 				m_PIDomega.calculate(current.getRotation().getRadians(), m_target.getRotation().getRadians()),
-				-0, 0);
+				-0, 0); // TODO: change clamp parameters and units
 
 		// Check if within tolerance
 		if (Math.abs(current.getX() - m_target.getX()) < TagAlignConstants.kTolxy && // x within range
@@ -78,13 +93,15 @@ public void execute() {
 	@Override
 	public void end(boolean interrupted) {
 		// TODO: log finish reason to NT (either reached, proximity, or interrupted)
+		// This is also a good place to start an instant command to move the outake to
+		// the correct position
 	}
 
 	@Override
 	public boolean isFinished() {
 		Pose2d current = poseFromAprilTag();
 		return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math
-				.pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity
+				.pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity.
 				m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint
 	}
 
@@ -94,7 +111,8 @@ public boolean isFinished() {
 	 * @return Robot Pose
 	 */
 	Pose2d poseFromAprilTag() { // TODO: move this to drive subsystem
-		NetworkTable tb = NetworkTableInstance.getDefault().getTable("limelight");
+		NetworkTable tb = TagAlignConstants.kNTInstance.getTable("limelight");
+
 		DoubleArrayEntry botpose = null; // index 0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw, 6: timestamp. all angles
 																			// are in degrees