Drive Code 2/18/24 with snapping to amp

Mechanical-Advantage · Feb 19, 2024 · a082aea · a082aea
1 parent b62d666
commit a082aea
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 18 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -327,9 +327,18 @@ private void configureButtonBindings() {
                 .withName("Eject To Floor"));
 
     // Amp scoring
-    driverController.b().and(allowingAutoAmpScore.negate()).whileTrue(superstructure.amp());
     driverController
-        .b()
+        .rightBumper()
+        .and(allowingAutoAmpScore.negate())
+        .whileTrue(
+            superstructure
+                .amp()
+                .alongWith(
+                    Commands.startEnd(
+                        () -> drive.setHeadingGoal(() -> new Rotation2d(-Math.PI / 2.0)),
+                        drive::clearHeadingGoal)));
+    driverController
+        .rightBumper()
         .and(allowingAutoAmpScore.negate())
         .and(driverController.rightTrigger())
         .whileTrue(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ampScore()));
@@ -344,7 +353,7 @@ private void configureButtonBindings() {
             new Rotation2d(-Math.PI / 2.0));
     Supplier<Pose2d> ampScoringPoseSupp = () -> AllianceFlipUtil.apply(goalAmpScorePose);
     driverController
-        .b()
+        .rightBumper()
         .and(allowingAutoAmpScore)
         .whileTrue(
             Commands.startEnd(
@@ -369,12 +378,13 @@ private void configureButtonBindings() {
     Function<Double, Command> changeCompensationDegrees =
         compensation ->
             Commands.runOnce(
-                () -> {
-                  double currentCompensation =
-                      RobotState.getInstance().getShotCompensationDegrees();
-                  RobotState.getInstance()
-                      .setShotCompensationDegrees(currentCompensation + compensation);
-                });
+                    () -> {
+                      double currentCompensation =
+                          RobotState.getInstance().getShotCompensationDegrees();
+                      RobotState.getInstance()
+                          .setShotCompensationDegrees(currentCompensation + compensation);
+                    })
+                .ignoringDisable(true);
     operatorController.povDown().onTrue(changeCompensationDegrees.apply(-0.1));
     operatorController.povUp().onTrue(changeCompensationDegrees.apply(0.1));
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -52,15 +52,18 @@ public record AimingParameters(
 
   static {
     armAngleMap.put(0.0, Units.degreesToRadians(90.0));
-    armAngleMap.put(1.017, .871);
-    armAngleMap.put(1.519, .746);
-    armAngleMap.put(1.956, .663);
-    armAngleMap.put(2.547, .592);
-    armAngleMap.put(3.102, .509);
-    armAngleMap.put(3.242, .488);
-    armAngleMap.put(3.503, .466);
-    armAngleMap.put(3.719, .466);
-    armAngleMap.put(4.145, .454);
+    armAngleMap.put(1.039, .890);
+    armAngleMap.put(1.258, .819);
+    armAngleMap.put(1.511, .749);
+    armAngleMap.put(1.745, .730);
+    armAngleMap.put(2.008, .678);
+    armAngleMap.put(2.266, .663);
+    armAngleMap.put(2.514, .592);
+    armAngleMap.put(2.749, .558);
+    armAngleMap.put(2.994, .528);
+    armAngleMap.put(3.260, .503);
+    armAngleMap.put(5.156, .408);
+    armAngleMap.put(1.229, .838);
   }
 
   @AutoLogOutput(key = "RobotState/ShotCompensationDegrees")

diff --git a/src/main/java/org/littletonrobotics/frc2024/util/NoteVisualizer.java b/src/main/java/org/littletonrobotics/frc2024/util/NoteVisualizer.java
@@ -24,6 +24,7 @@ public class NoteVisualizer {
   @Setter private static Supplier<Pose2d> robotPoseSupplier = Pose2d::new;
   @Setter private static Supplier<Rotation2d> armAngleSupplier = Rotation2d::new;
 
+  /** Shoots note from middle of arm to speaker */
   public static Command shoot() {
     return new ScheduleCommand( // Branch off and exit immediately
         Commands.defer(