edit wanted angle before changing state

FRC3476 · Apr 6, 2024 · b72d28b · b72d28b
1 parent 2fba208
commit b72d28b
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/src/main/java/org/codeorange/frc2024/robot/Robot.java b/src/main/java/org/codeorange/frc2024/robot/Robot.java
@@ -376,16 +376,16 @@ public void teleopPeriodic() {
             superstructure.setGoalState(Superstructure.States.SOURCE_INTAKE);
         }
         if(buttonPanel.getRisingEdge(5)) {
-            superstructure.setGoalState(Superstructure.States.SPEAKER);
             superstructure.isFlipped = false;
             superstructure.wantedAngle = 52;
             superstructure.manualOverride = true;
+            superstructure.setGoalState(Superstructure.States.SPEAKER);
         }
         if(buttonPanel.getRisingEdge(11)) {
-            superstructure.setGoalState(Superstructure.States.SPEAKER);
             superstructure.isFlipped = true;
             superstructure.wantedAngle = 52;
             superstructure.manualOverride = true;
+            superstructure.setGoalState(Superstructure.States.SPEAKER);
         }
         if(buttonPanel.getRisingEdge(6)) {
             superstructure.setGoalState(Superstructure.States.SHOOT_OVER_STAGE);
@@ -452,8 +452,9 @@ public void teleopPeriodic() {
             shooter.stop();
         }
         if(xbox.getRisingEdge(XboxButtons.Y)) {
-            superstructure.setGoalState(Superstructure.States.SPEAKER);
+            superstructure.wantedAngle = AngleLookupInterpolation.SHOOTER_ANGLE_BACK_LOW.get(drive.findDistanceToSpeaker())
             superstructure.isFlipped = drive.isForward();
+            superstructure.setGoalState(Superstructure.States.SPEAKER);
         }
         if(xbox.getFallingEdge(XboxButtons.Y)) {
             superstructure.setGoalState(Superstructure.States.STOW);