Seamless amp

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -427,6 +428,7 @@ private void configureModes() {
         testModeChooser.addOption("Shooter Tuning", "tuning-shooter");
         testModeChooser.addOption("Aimer Tuning", "tuning-aimer");
         testModeChooser.addOption("Shot Tuning", "tuning-shot");
+        testModeChooser.addOption("Amp Tuning", "tuning-amp");
 
         SmartDashboard.putData("Test Mode Chooser", testModeChooser);
     }
@@ -651,6 +653,58 @@ public void testInit() {
                                                         0)))
                         .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0)));
                 break;
+            case "tuning-amp":
+                SmartDashboard.putNumber(
+                        "Test-Mode/amp/aimerSetpointPosition",
+                        ScoringConstants.ampAimerAngleRotations);
+
+                // Let us drive
+                CommandScheduler.getInstance().cancelAll();
+                setUpDriveWithJoysticks();
+
+                // Reset bindings
+                masher = new CommandXboxController(2);
+
+                // Let us intake
+                masher.b()
+                        .onTrue(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.INTAKE)))
+                        .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE)));
+
+                // Let us reverse intake (in case a note gets jammed during amp tuning)
+                masher.a()
+                        .onTrue(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.REVERSE)))
+                        .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE)));
+
+                masher.rightTrigger()
+                        .onTrue(
+                                new InstantCommand(
+                                        () ->
+                                                scoringSubsystem.runToPosition(
+                                                        SmartDashboard.getNumber(
+                                                                "Test-Mode/amp/aimerSetpointPosition",
+                                                                0.0),
+                                                        0)))
+                        .onTrue(
+                                new InstantCommand(
+                                        () ->
+                                                scoringSubsystem.setAction(
+                                                        ScoringAction.TEMPORARY_SETPOINT)))
+                        .onFalse(
+                                new InstantCommand(
+                                        () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE)));
+
+                masher.rightBumper()
+                        .onTrue(
+                                new InstantCommand(
+                                        () ->
+                                                scoringSubsystem.setOverrideKickerVoltsDirectly(
+                                                        12.0)))
+                        .onFalse(
+                                new InstantCommand(
+                                        () ->
+                                                scoringSubsystem.setOverrideKickerVoltsDirectly(
+                                                        0.0)));
+                break;
         }
     }
 

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

@@ -59,7 +59,8 @@ public void execute() {
             // ScoringAction.SHOOT will shoot when ready.
             // Therefore, when shoot is pressed, check if we've transitioned to aim and then
             // transition to shoot.
-            if (scoring.getCurrentAction() == ScoringAction.AIM) {
+            if (scoring.getCurrentAction() == ScoringAction.AIM
+                    || scoring.getCurrentAction() == ScoringAction.AMP_AIM) {
                 scoring.setAction(ScoringAction.SHOOT);
             }
 

src/main/java/frc/robot/constants/FieldConstants.java

@@ -12,7 +12,7 @@ public final class FieldConstants {
     public static final double midfieldLowThresholdM = 5.87;
     public static final double midfieldHighThresholdM = 10.72;
 
-    public static final Rotation2d ampHeading = new Rotation2d(-Math.PI / 2);
+    public static final Rotation2d ampHeading = new Rotation2d(Math.PI / 2);
 
     public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0);
     public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI);

src/main/java/frc/robot/constants/ScoringConstants.java

@@ -61,6 +61,8 @@ public class ScoringConstants {
 
     public static final double aimAngleTolerance = 0.015;
 
+    public static final double ampAimerAngleRotations = 0.14;
+
     public static final double maxAimIntake = 0.0;
     public static final double minAimIntake = 0.0;
 

src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java

@@ -300,9 +300,7 @@ private void prime() {
     private void ampPrime() {
         // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM);
         // TODO: Test this out
-        aimerIo.setAimAngleRot(
-                0.25); // This is actually in rotations and not radians, I really need to rename all
-        // of the aimer IO functions
+        aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations);
         if (action != ScoringAction.SHOOT && action != ScoringAction.AMP_AIM) {
             state = ScoringState.IDLE;
         } else if (action == ScoringAction.SHOOT) {
@@ -373,6 +371,14 @@ private void shoot() {
 
     private void ampShoot() {
         shooterIo.setKickerVolts(-12); // TODO: Test if this kicks note forward or backward
+        aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations);
+
+        // Revert to amp prime after we shoot the note
+        // This will keep the arm up and allow the driver to drive away from the amp before the arm
+        // comes down
+        if (action == ScoringAction.WAIT || (!hasNote() && shootTimer.get() > 1.0)) {
+            state = ScoringState.AMP_PRIME;
+        }
 
         // I see no reason why amp should not shoot within 1 second but if it takes longer than
         // that, we should probably let it
@@ -773,4 +779,9 @@ public boolean readyToShoot() {
     public void setAimerStatorCurrentLimit(double limit) {
         aimerIo.setStatorCurrentLimit(limit);
     }
+
+    public void setOverrideKickerVoltsDirectly(double volts) {
+        /* Immediately sets kicker voltage, fully ignoring scoring state */
+        shooterIo.setKickerVolts(volts);
+    }
 }