changes for dealing with orientation differences between the Field-Centric and Odometry-Centric systems.

Author: jhhbrown1

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

@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
 			ControllerConstants.kDriverControllerPort);
 	private final PowerDistribution m_pdh = new PowerDistribution();
 	private final VisionSimulator m_visionSimulator = new VisionSimulator(m_driveSubsystem,
-			pose(kFieldLayout.getFieldLength() / 2, 1.91, 0), 0.01);
+			pose(kFieldLayout.getFieldLength() / 2, 1.91, 180), 0.01);
 	private final PhotonCamera m_camera1 = RobotBase.isSimulation()
 			? new PhotonCameraSimulator("Camera1", kRobotToCamera1, m_visionSimulator, 3, 0.1)
 			: new PhotonCamera("Cool camera");
@@ -107,7 +107,7 @@ public void bindDriveControls() {
 						() -> -m_driverController.getLeftY(),
 						() -> -m_driverController.getLeftX(),
 						() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
-						m_driverController.getHID()::getSquareButton));
+						() -> !m_driverController.getHID().getSquareButton()));
 
 		m_driverController.button(Button.kSquare)
 				.whileTrue(

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

@@ -203,10 +203,10 @@ public ChassisSpeeds chassisSpeedsTowardClosestTag(Transform2d robotToTag, doubl
 	 * @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 controllerXY,
+	public ChassisSpeeds chassisSpeeds(Pose2d currentPose, Pose2d targetPose, PIDController controllerXY,
 			PIDController controllerYaw) {
-		Translation2d current2target = targetPose.getTranslation()
-				.minus(currentPose.getTranslation());
+		Translation2d current2target = targetPose.minus(currentPose).getTranslation()
+				.rotateBy(m_driveSubsystem.getPose().getRotation());
 		double velocityX = 0, velocityY = 0;
 		double distance = current2target.getNorm();
 		if (distance > 0) {