Fixup some stuff in sim

FRCTeam3044 · Feb 15, 2025 · 62c262a · 62c262a
1 parent 9e4480a
commit 62c262a
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 7 deletions.
diff --git a/simgui.json b/simgui.json
@@ -63,6 +63,9 @@
     "transitory": {
       "AdvantageKit": {
         "RealOutputs": {
+          "ButtonBoard": {
+            "open": true
+          },
           "StateMachine": {
             "open": true
           },
@@ -74,6 +77,9 @@
         "open": true
       },
       "SmartDashboard": {
+        "ButtonBoardSim": {
+          "open": true
+        },
         "Field": {
           "double[]##v_/SmartDashboard/Field/notes": {
             "open": false
@@ -101,6 +107,7 @@
     },
     "Server": {
       "open": true
-    }
+    },
+    "visible": true
   }
 }
diff --git a/src/main/deploy/config.json b/src/main/deploy/config.json
@@ -245,8 +245,8 @@
             "comment": "Auto-Generated",
             "value": "1.0"
         },
-        "X Point Controller/P": "1.0",
-        "Point Y Controller/P": "1.0",
+        "X Point Controller/P": "1.5",
+        "Point Y Controller/P": "1.5",
         "Processor Flipped": {
             "comment": "Auto-Generated",
             "value": "false"
@@ -299,9 +299,9 @@
         "Elevator L3 Heights/y1": "0.0",
         "Point Theta Controller/D": "0.0",
         "Point Theta Controller/P": "6.0",
-        "Pathfinding Y Controller/P": "1.0",
+        "Pathfinding Y Controller/P": "3.0",
         "Shoulder L3 Angles/x2": "0.0",
-        "Pathfinding X Controller/P": "1.0",
+        "Pathfinding X Controller/P": "3.0",
         "Point Theta Controller/I": "0.0"
     },
     "competition": {

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.2)
-                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.2).build();
+                        .setRobotLength(mapleBumperSize.in(Meters) + 0.3)
+                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.3).build();
 
         public static final PIDController xController = new ConfigurablePIDController(1, 0, 0,
                         "Pathfinding X Controller");

diff --git a/src/main/java/frc/robot/util/bboard/ButtonBoard.java b/src/main/java/frc/robot/util/bboard/ButtonBoard.java
@@ -8,7 +8,9 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.util.AutoTargetUtils.IntakeStations.IntakeStation;
+import frc.robot.util.AllianceUtil;
 import frc.robot.util.AutoTargetUtils;
+import frc.robot.util.AllianceUtil.AllianceColor;
 import frc.robot.util.AutoTargetUtils.IntakeStations;
 import frc.robot.util.AutoTargetUtils.Reef;
 import frc.robot.util.AutoTargetUtils.Reef.CoralLevel;
@@ -88,7 +90,23 @@ public boolean getSelectedAlgaeLocation() {
         return isProcessor;
     }
 
+    private AllianceColor lastAllianceColor = AllianceColor.UNKNOWN;
+
     public void periodic(Drive drive) {
+        if(lastAllianceColor != AllianceUtil.getAlliance()){
+            if(coralReefLevel != null && coralReefLocation != null){
+                coralReefTargetPose = Reef.coral(coralReefLocation, coralReefLevel);
+                coralReefReferencePose = coralReefLocation.pose();
+            }
+            if(coralReefLocation != null){
+                algaeReefTargetPose = Reef.algae(coralReefLocation.algae());
+                algaeReefReferencePose = coralReefLocation.algae().pose();
+            }
+            if(intakeStation != null){
+                intakeStationPose = IntakeStations.intakeStation(intakeStation);
+                intakeStationReferencePose = intakeStation.pose();
+            }
+        }
         boardIO.periodic();
         for (SelectButtonInfo<CoralReefLocation> button : reefButtons) {
             if (boardIO.isPressed(button)) {