Amp Align

src/main/java/frc/team2412/robot/Controls.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -28,6 +29,7 @@
 import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand;
 import frc.team2412.robot.commands.launcher.SetPivotCommand;
 import frc.team2412.robot.subsystems.LauncherSubsystem;
+import frc.team2412.robot.util.AmpAlign;
 import frc.team2412.robot.util.TrapAlign;
 
 public class Controls {
@@ -58,6 +60,7 @@ public static class ControlConstants {
 	private final Trigger launcherLowerPresetButton;
 	// private final Trigger launcherPodiumPresetButton;
 	private final Trigger launcherTrapPresetButton;
+	private final Trigger launcherAmpAlignPresetButton;
 	private final Trigger launcherLaunchButton;
 
 	private final Subsystems s;
@@ -75,6 +78,7 @@ public Controls(Subsystems s) {
 		launcherLowerPresetButton = codriveController.y();
 		// launcherPodiumPresetButton = codriveController.povLeft();
 		launcherTrapPresetButton = codriveController.start();
+		launcherAmpAlignPresetButton = driveController.y();
 		launcherLaunchButton = codriveController.rightBumper();
 		// intake controls (confirmed with driveteam)
 		driveIntakeInButton = driveController.a();
@@ -213,6 +217,11 @@ private void bindLauncherControls() {
 						LauncherSubsystem.AMP_AIM_ANGLE));
 		launcherTrapPresetButton.onTrue(
 				TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem));
+		launcherAmpAlignPresetButton.onTrue(
+				Commands.either(
+						AmpAlign.ampPreset(s.drivebaseSubsystem),
+						Commands.none(),
+						() -> s.drivebaseSubsystem.getPose().getY() > 5.0));
 
 		codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher));
 

src/main/java/frc/team2412/robot/util/AmpAlign.java

@@ -0,0 +1,95 @@
+package frc.team2412.robot.util;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.DrivebaseSubsystem;
+import java.util.List;
+
+public class AmpAlign {
+	private static final Pose2d BLUE_AMP_POSES =
+			// amp on the blue side
+			new Pose2d(new Translation2d(1.8, 7.3), Rotation2d.fromDegrees(270));
+
+	private static final Pose2d RED_AMP_POSES =
+			// amp on the red side
+			new Pose2d(new Translation2d(14.5, 7.3), Rotation2d.fromDegrees(270));
+
+	private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) {
+		Pose2d robotPose = drivebaseSubsystem.getPose();
+		boolean isBlue;
+		if (!DriverStation.getAlliance().isEmpty()) {
+			isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue);
+		} else {
+			isBlue = false;
+		}
+		// figures out which amp to go to
+		Pose2d ampPose = (isBlue) ? BLUE_AMP_POSES : RED_AMP_POSES;
+		// sets the point for the path to go to
+		List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, ampPose);
+		// this is flipped
+		PathPlannerPath path =
+				new PathPlannerPath(
+						bezierPoints,
+						new PathConstraints(
+								DrivebaseSubsystem.MAX_SPEED,
+								DrivebaseSubsystem.MAX_ACCELERATION,
+								DrivebaseSubsystem.MAX_ANGULAR_VELOCITY,
+								DrivebaseSubsystem.MAX_ANGULAR_ACCELERAITON),
+						new GoalEndState(0.0, ampPose.getRotation()));
+		// path.flipPath(); Returns path except it's flipped
+		// this unflips it
+		if (!isBlue) {
+			path = path.flipPath();
+		}
+
+		return AutoBuilder.followPath(path);
+	}
+
+	public static Command ampPreset(DrivebaseSubsystem drivebaseSubsystem) {
+		return new AlignCommand(drivebaseSubsystem);
+	}
+
+	private static class AlignCommand extends Command {
+		private final DrivebaseSubsystem drivebaseSubsystem;
+		private Command ampCommand = null;
+
+		public AlignCommand(DrivebaseSubsystem drivebaseSubsystem) {
+			this.drivebaseSubsystem = drivebaseSubsystem;
+			addRequirements(drivebaseSubsystem);
+		}
+
+		@Override
+		public void initialize() {
+			ampCommand = ampAlign(drivebaseSubsystem);
+			ampCommand.initialize();
+			// launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE);
+			// launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM);
+		}
+
+		@Override
+		public void execute() {
+			ampCommand.execute();
+		}
+
+		@Override
+		public boolean isFinished() {
+			return ampCommand.isFinished();
+		}
+
+		@Override
+		public void end(boolean interrupted) {
+			if (interrupted && ampCommand != null) {
+				ampCommand.end(true);
+			}
+			ampCommand = null;
+		}
+	}
+}