New auto align

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

@@ -14,7 +14,9 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -320,4 +322,57 @@ public static final class NoteSim {
 		public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8);
 		public static final double OUT_OF_FIELD_MARGIN = .025;
 	}
+
+	public static final class AutoAlign {
+
+		public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0);
+		public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0);
+		public static final double TIME_TO_FEED = 1.0; // seconds
+		public static final double TIME_FOR_SCORING = .5; // seconds
+
+		public static final Pose2d redAmpSpeakerPose = new Pose2d(
+			15.59,
+			6.644,
+			new Rotation2d(Math.toRadians(120.5))
+		);
+		public static final Pose2d blueAmpSpeakerPose = new Pose2d(
+			0.909,
+			6.644,
+			new Rotation2d(Math.toRadians(55.5))
+		);
+		public static final Pose2d redSourceSpeakerPose = new Pose2d(
+			15.636,
+			4.39,
+			new Rotation2d(Math.toRadians(-122.5))
+		);
+		public static final Pose2d blueSourceSpeakerPose = new Pose2d(
+			0.864,
+			4.39,
+			new Rotation2d(Math.toRadians(-62.5))
+		);
+		public static final Pose2d redAmpPose = new Pose2d(
+			14.7,
+			7.72,
+			new Rotation2d(Math.toRadians(-90))
+		);
+		public static final Pose2d blueAmpPose = new Pose2d(
+			1.85,
+			7.72,
+			new Rotation2d(Math.toRadians(-90))
+		);
+
+		public static final Pose2d blueSpeakerPose = new Pose2d(
+			1.45,
+			5.50,
+			new Rotation2d(Math.toRadians(0))
+		);
+		public static final Pose2d redSpeakerPose = new Pose2d(
+			15.1,
+			5.50,
+			new Rotation2d(Math.toRadians(180))
+		);
+
+		public static final double TRANSLATION_ERROR_MARGIN = 0.4;
+		public static final double ROTATION_ERROR_MARGIN = 0.4;
+	}
 }

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

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.AutoCommands;
+import frc.robot.subsystems.drive.AutoAlign;
 import frc.robot.subsystems.manager.*;
 import frc.robot.util.NoteSimulator;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -51,6 +52,7 @@ public class Robot extends LoggedRobot {
 	@Override
 	public void robotInit() {
 		managerSubsystem = new Manager();
+		AutoAlign.setManager(managerSubsystem);
 
 		NamedCommands.registerCommand("Intaking", autoCommands.intaking());
 		NamedCommands.registerCommand("Shooting", autoCommands.shooting());

src/main/java/frc/robot/subsystems/drive/AutoAlign.java

@@ -0,0 +1,147 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.subsystems.manager.Manager;
+import frc.robot.subsystems.manager.ManagerStates;
+
+public class AutoAlign {
+
+	private static PIDController xPIDController = new PIDController(
+		Constants.AutoAlign.TRANSLATIONAL_PID.kP,
+		Constants.AutoAlign.TRANSLATIONAL_PID.kI,
+		Constants.AutoAlign.TRANSLATIONAL_PID.kD
+	);
+	private static PIDController yPIDController = new PIDController(
+		Constants.AutoAlign.TRANSLATIONAL_PID.kP,
+		Constants.AutoAlign.TRANSLATIONAL_PID.kI,
+		Constants.AutoAlign.TRANSLATIONAL_PID.kD
+	);
+	private static final PIDController rotationalPIDController = new PIDController(
+		Constants.AutoAlign.ROTATIONAL_PID.kP,
+		Constants.AutoAlign.ROTATIONAL_PID.kI,
+		Constants.AutoAlign.ROTATIONAL_PID.kD
+	);
+	private static Pose2d targetPose2d;
+	private static AutoAlignInstruction autoAlignInstruction;
+	private static Timer timer = new Timer();
+	private static boolean setStateAlready;
+
+	private static Drive drive;
+	private static Manager manager;
+
+	public static void setTargetPose(Pose2d target) {
+		targetPose2d = target;
+	}
+
+	public static void setDrive(Drive inputDrive) {
+		drive = inputDrive;
+	}
+
+	public static void setManager(Manager inputManager) {
+		manager = inputManager;
+	}
+
+	// calculate chassissSpeed obj
+	public static void calculateChassisSpeed() {
+		Pose2d currentPose2d = drive.getPose();
+
+		if (!nearSetPoint()) {
+			manager.setState(autoAlignInstruction.duringDrive);
+
+			drive.runVelocity(
+				ChassisSpeeds.fromFieldRelativeSpeeds(
+					xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()),
+					yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()),
+					rotationalPIDController.calculate(
+						currentPose2d.getRotation().getRadians(),
+						targetPose2d.getRotation().getRadians()
+					),
+					drive.getRotation()
+				)
+			);
+		} else {
+			// this is the only way i could think of to make the code pretty. please don't murder me
+			// Amp bar needs time to be able to be fed
+			if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) {
+				manager.setState(ManagerStates.IDLE);
+				drive.setState(DriveStates.REGULAR_DRIVE);
+			} else if (
+				autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR &&
+				timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)
+			) {
+				manager.setState(autoAlignInstruction.atSetPoint);
+			} else if (
+				autoAlignInstruction == AutoAlignInstruction.SHOOT &&
+				timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)
+			) {
+				manager.setState(autoAlignInstruction.atSetPoint);
+			}
+
+			if (!setStateAlready) {
+				setStateAlready = true;
+				timer.reset();
+				timer.start();
+			}
+		}
+	}
+
+	public static boolean nearSetPoint() {
+		Pose2d currentPose2d = drive.getPose();
+
+		if (
+			Math.abs(currentPose2d.getX() - targetPose2d.getX()) <
+				Constants.AutoAlign.TRANSLATION_ERROR_MARGIN &&
+			Math.abs(currentPose2d.getY() - targetPose2d.getY()) <
+			Constants.AutoAlign.TRANSLATION_ERROR_MARGIN &&
+			Math.abs(
+				currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()
+			) <
+			Constants.AutoAlign.ROTATION_ERROR_MARGIN
+		) {
+			return true;
+		}
+		return false;
+	}
+
+	public static void periodic() {
+		if (Constants.operatorController.getLeftBumper()) {
+			setTargetPose(
+				(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
+					? Constants.AutoAlign.redSourceSpeakerPose
+					: Constants.AutoAlign.blueAmpSpeakerPose
+			);
+			autoAlignInstruction = AutoAlignInstruction.SHOOT;
+			setStateAlready = false;
+			drive.setState(DriveStates.AUTO_ALIGN);
+		} else if (Constants.operatorController.getRightBumper()) {
+			setTargetPose(
+				(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
+					? Constants.AutoAlign.redAmpSpeakerPose
+					: Constants.AutoAlign.blueSourceSpeakerPose
+			);
+			autoAlignInstruction = AutoAlignInstruction.SHOOT;
+			setStateAlready = false;
+			drive.setState(DriveStates.AUTO_ALIGN);
+		} else if (Constants.operatorController.getAButton()) {
+			setTargetPose(
+				(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
+					? Constants.AutoAlign.redAmpPose
+					: Constants.AutoAlign.blueAmpPose
+			);
+			autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR;
+			timer.restart();
+			timer.start();
+			setStateAlready = false;
+			drive.setState(DriveStates.AUTO_ALIGN);
+		} else if (Constants.operatorController.getXButton()) {
+			drive.setState(DriveStates.REGULAR_DRIVE);
+		}
+	}
+	// #TODO add drive to amp bar and auto shoot.
+	// create timer for feeding and holding
+}

src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.drive;
+
+import frc.robot.subsystems.manager.ManagerStates;
+
+public enum AutoAlignInstruction {
+	SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP),
+	SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING);
+
+	AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) {
+		this.duringDrive = duringDrive;
+		this.atSetPoint = atSetPoint;
+	}
+
+	ManagerStates duringDrive;
+	ManagerStates atSetPoint;
+}

src/main/java/frc/robot/subsystems/manager/Manager.java

@@ -3,6 +3,7 @@
 import frc.robot.Constants;
 import frc.robot.subsystems.*;
 import frc.robot.subsystems.ampBar.*;
+import frc.robot.subsystems.drive.AutoAlign;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
@@ -66,6 +67,7 @@ public Manager() {
 		}
 
 		NoteSimulator.setDrive(driveSubsystem);
+		AutoAlign.setDrive(driveSubsystem);
 
 		// State Transitions (Nothing Automatic YET)