Restore subwoofer shot position of 59deg in teleop

team581 · Apr 20, 2024 · e986740 · e986740
1 parent 57ef9ba
commit e986740
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java
@@ -622,14 +622,22 @@ public void robotPeriodic() {
         break;
       case WAITING_SUBWOOFER_SHOT:
       case PREPARE_SUBWOOFER_SHOT:
-        wrist.setAngle(WristPositions.SUBWOOFER_SHOT);
+        if (DriverStation.isAutonomous()) {
+          wrist.setAngle(WristPositions.AUTO_SUBWOOFER_SHOT);
+        } else {
+          wrist.setAngle(WristPositions.SUBWOOFER_SHOT);
+        }
         elevator.setGoalHeight(ElevatorPositions.STOWED);
         shooter.setGoalMode(ShooterMode.SUBWOOFER_SHOT);
         climber.setGoalMode(ClimberMode.STOWED);
         noteManager.idleInQueuerRequest();
         break;
       case SUBWOOFER_SHOOT:
-        wrist.setAngle(WristPositions.SUBWOOFER_SHOT);
+        if (DriverStation.isAutonomous()) {
+          wrist.setAngle(WristPositions.AUTO_SUBWOOFER_SHOT);
+        } else {
+          wrist.setAngle(WristPositions.SUBWOOFER_SHOT);
+        }
         elevator.setGoalHeight(ElevatorPositions.STOWED);
         shooter.setGoalMode(ShooterMode.SUBWOOFER_SHOT);
         climber.setGoalMode(ClimberMode.STOWED);

diff --git a/src/main/java/frc/robot/wrist/WristPositions.java b/src/main/java/frc/robot/wrist/WristPositions.java
@@ -14,7 +14,8 @@ public class WristPositions {
 
   public static final Rotation2d SHOOTER_AMP = Rotation2d.fromDegrees(58.0);
 
-  public static final Rotation2d SUBWOOFER_SHOT = Rotation2d.fromDegrees(61.0);
+  public static final Rotation2d SUBWOOFER_SHOT = Rotation2d.fromDegrees(59.0);
+  public static final Rotation2d AUTO_SUBWOOFER_SHOT = Rotation2d.fromDegrees(61.0);
   public static final Rotation2d PRESET_AMP = Rotation2d.fromDegrees(43);
   public static final Rotation2d PRESET_SOURCE = Rotation2d.fromDegrees(58.1);
   public static final Rotation2d PRESET_MIDDLE = Rotation2d.fromDegrees(58.1);