Button board!!!!!

FRCTeam3044 · Feb 15, 2025 · bc806bc · bc806bc
1 parent a3b1838
commit bc806bc
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 22 deletions.
diff --git a/simgui.json b/simgui.json
@@ -2,9 +2,7 @@
   "HALProvider": {
     "Addressable LEDs": {
       "0": {
-        "order": 1,
-        "serpentine": true,
-        "start": 1
+        "serpentine": true
       },
       "window": {
         "visible": true
@@ -86,6 +84,9 @@
     }
   },
   "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
     "GearGlimpse@1": {
       "Publishers": {
         "open": false
@@ -98,6 +99,8 @@
     "Nt4Unity@1": {
       "open": false
     },
-    "visible": true
+    "Server": {
+      "open": true
+    }
   }
 }
diff --git a/src/main/java/frc/robot/statemachine/states/tele/ManualState.java b/src/main/java/frc/robot/statemachine/states/tele/ManualState.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
@@ -44,12 +45,13 @@ public ManualState(StateMachineBase stateMachine, CommandXboxController driver,
                         rotVel = () -> -driver.getRightX();
                 }
 
-                startWhenActive(DriveCommands.joystickDrive(drive, xVel, yVel, rotVel));
+                Command joystickDrive = DriveCommands.joystickDrive(drive, xVel, yVel, rotVel);
+                startWhenActive(joystickDrive);
                 startWhenActive(LEDs.Default());
 
                 // Switch to X pattern when X button is pressed
-                driverController.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-
+                driverController.x().onTrue(Commands.runOnce(drive::stopWithX, drive).withName("X mode"));
+                driverController.x().onFalse(joystickDrive);
                 driverController.a().onTrue(
                                 DriveCommands.goToPointJoystickRot(drive, new Pose2d(3, 3, new Rotation2d()), rotVel));
 

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -132,8 +132,8 @@ public class DriveConstants {
 
         public static final Pathfinder pathfinder = (new PathfinderBuilder(Field.REEFSCAPE_2025))
                         .setNormalizeCorners(false).setCornerDist(0.5)
-                        .setRobotLength(mapleBumperSize.in(Meters) + 0.1)
-                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.1).build();
+                        .setRobotLength(mapleBumperSize.in(Meters) + 0.2)
+                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.2).build();
 
         public static final PIDController xController = new ConfigurablePIDController(1, 0, 0,
                         "Pathfinding X Controller");

diff --git a/src/main/java/frc/robot/util/ButtonBoardUtil.java b/src/main/java/frc/robot/util/ButtonBoardUtil.java
@@ -57,19 +57,19 @@ public boolean isPressed() {
             new SelectButtonInfo<IntakeStation>(1, 1, IntakeStation.LeftOne),
             new SelectButtonInfo<IntakeStation>(1, 2, IntakeStation.LeftTwo),
             new SelectButtonInfo<IntakeStation>(1, 3, IntakeStation.LeftThree),
-            new SelectButtonInfo<IntakeStation>(1, 4, IntakeStation.RightOne),
-            new SelectButtonInfo<IntakeStation>(1, 5, IntakeStation.RightTwo),
-            new SelectButtonInfo<IntakeStation>(1, 6, IntakeStation.RightThree));
-
-    private ButtonInfo processor = new ButtonInfo(0, 19);
-    private List<SelectButtonInfo<CoralLevel>> levels = List.of(new SelectButtonInfo<CoralLevel>(2, 1, CoralLevel.L1),
-            new SelectButtonInfo<CoralLevel>(2, 2, CoralLevel.L2),
-            new SelectButtonInfo<CoralLevel>(2, 3, CoralLevel.L3),
-            new SelectButtonInfo<CoralLevel>(2, 4, CoralLevel.L4));
-    private ButtonInfo algaeModeToggle = new ButtonInfo(0, 24);
-    private ButtonInfo net = new ButtonInfo(0, 25);
-    private ButtonInfo climbUp = new ButtonInfo(0, 26);
-    private ButtonInfo climbDown = new ButtonInfo(0, 27);
+            new SelectButtonInfo<IntakeStation>(1, 7, IntakeStation.RightOne),
+            new SelectButtonInfo<IntakeStation>(1, 6, IntakeStation.RightTwo),
+            new SelectButtonInfo<IntakeStation>(1, 5, IntakeStation.RightThree));
+
+    private ButtonInfo processor = new ButtonInfo(2, 12);
+    private List<SelectButtonInfo<CoralLevel>> levels = List.of(new SelectButtonInfo<CoralLevel>(2, 10, CoralLevel.L1),
+            new SelectButtonInfo<CoralLevel>(2, 9, CoralLevel.L2),
+            new SelectButtonInfo<CoralLevel>(2, 8, CoralLevel.L3),
+            new SelectButtonInfo<CoralLevel>(2, 7, CoralLevel.L4));
+    private ButtonInfo algaeModeToggle = new ButtonInfo(1, 4);
+    private ButtonInfo net = new ButtonInfo(0, 11);
+    private ButtonInfo climbUp = new ButtonInfo(0, 5);
+    private ButtonInfo climbDown = new ButtonInfo(0, 6);
 
     // Reef
     private Pose2d coralReefTargetPose = null;
@@ -217,8 +217,15 @@ public boolean closeToScoringTarget(Drive drive) {
 
     public boolean closeToIntakeTarget(Drive drive) {
         if (algaeMode) {
+            if (algaeReefTargetPose == null) {
+                return false;
+            }
             return AutoTargetUtils.robotDistToPose(drive, algaeReefTargetPose) < algaeReefDistThreshold.get();
         } else {
+            if (intakeStationPose == null) {
+                return false;
+            }
+
             return AutoTargetUtils.robotDistToPose(drive, intakeStationPose) < intakeStationDistThreshold.get();
         }
     }