Running better in sim

FRCTeam3044 · Feb 14, 2025 · ec48600 · ec48600
1 parent 15f0968
commit ec48600
Show file tree

Hide file tree

Showing 6 changed files with 43 additions and 12 deletions.
diff --git a/simgui.json b/simgui.json
@@ -71,6 +71,9 @@
           }
         }
       },
+      "LiveWindow": {
+        "open": true
+      },
       "SmartDashboard": {
         "Field": {
           "double[]##v_/SmartDashboard/Field/notes": {
@@ -95,8 +98,5 @@
       "open": false
     },
     "visible": true
-  },
-  "NetworkTables View": {
-    "visible": false
   }
 }
diff --git a/src/main/deploy/config.json b/src/main/deploy/config.json
@@ -74,20 +74,26 @@
         "Elevator L2 Heights/x1": "0.0",
         "Pathfinding Y Controller/I": "0.0",
         "Pathfinding Theta Controller/I": "0.0",
+        "X Point Controller/I": "0.0",
         "Pathfinding Theta Controller/P": "6.0",
         "Elevator L4 Heights/y1": "0.0",
         "Intake speed": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
         "Elevator L4 Heights/y2": "0.0",
+        "Point Y Controller/I": "0.0",
         "Shoulder L1 Angles/y2": "0.0",
         "Shoulder L1 Angles/y1": "0.0",
         "Pathfinding Y Controller/D": "0.0",
+        "X Point Controller/D": "0.0",
+        "Point Y Controller/D": "0.0",
         "Intake station dist": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
+        "X Point Controller/P": "1.0",
+        "Point Y Controller/P": "1.0",
         "Processor Flipped": {
             "comment": "Auto-Generated",
             "value": "false"
@@ -138,9 +144,12 @@
         },
         "Elevator L3 Heights/y2": "0.0",
         "Elevator L3 Heights/y1": "0.0",
+        "Point Theta Controller/D": "0.0",
+        "Point Theta Controller/P": "6.0",
         "Pathfinding Y Controller/P": "1.0",
         "Shoulder L3 Angles/x2": "0.0",
-        "Pathfinding X Controller/P": "1.0"
+        "Pathfinding X Controller/P": "1.0",
+        "Point Theta Controller/I": "0.0"
     },
     "mode": "simulation",
     "simulation": {
@@ -218,20 +227,26 @@
         "Elevator L2 Heights/x1": "0.0",
         "Pathfinding Y Controller/I": "0.0",
         "Pathfinding Theta Controller/I": "0.0",
+        "X Point Controller/I": "0.0",
         "Pathfinding Theta Controller/P": "6.0",
         "Elevator L4 Heights/y1": "0.0",
         "Intake speed": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
         "Elevator L4 Heights/y2": "0.0",
+        "Point Y Controller/I": "0.0",
         "Shoulder L1 Angles/y2": "0.0",
         "Shoulder L1 Angles/y1": "0.0",
         "Pathfinding Y Controller/D": "0.0",
+        "X Point Controller/D": "0.0",
+        "Point Y Controller/D": "0.0",
         "Intake station dist": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
+        "X Point Controller/P": "1.0",
+        "Point Y Controller/P": "1.0",
         "Processor Flipped": {
             "comment": "Auto-Generated",
             "value": "false"
@@ -282,9 +297,12 @@
         },
         "Elevator L3 Heights/y2": "0.0",
         "Elevator L3 Heights/y1": "0.0",
+        "Point Theta Controller/D": "0.0",
+        "Point Theta Controller/P": "6.0",
         "Pathfinding Y Controller/P": "1.0",
         "Shoulder L3 Angles/x2": "0.0",
-        "Pathfinding X Controller/P": "1.0"
+        "Pathfinding X Controller/P": "1.0",
+        "Point Theta Controller/I": "0.0"
     },
     "competition": {
         "Shoulder L3 Angles/y2": "0.0",
@@ -361,20 +379,26 @@
         "Elevator L2 Heights/x1": "0.0",
         "Pathfinding Y Controller/I": "0.0",
         "Pathfinding Theta Controller/I": "0.0",
+        "X Point Controller/I": "0.0",
         "Pathfinding Theta Controller/P": "6.0",
         "Elevator L4 Heights/y1": "0.0",
         "Intake speed": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
         "Elevator L4 Heights/y2": "0.0",
+        "Point Y Controller/I": "0.0",
         "Shoulder L1 Angles/y2": "0.0",
         "Shoulder L1 Angles/y1": "0.0",
         "Pathfinding Y Controller/D": "0.0",
+        "X Point Controller/D": "0.0",
+        "Point Y Controller/D": "0.0",
         "Intake station dist": {
             "comment": "Auto-Generated",
             "value": "1.0"
         },
+        "X Point Controller/P": "1.0",
+        "Point Y Controller/P": "1.0",
         "Processor Flipped": {
             "comment": "Auto-Generated",
             "value": "false"
@@ -425,8 +449,11 @@
         },
         "Elevator L3 Heights/y2": "0.0",
         "Elevator L3 Heights/y1": "0.0",
+        "Point Theta Controller/D": "0.0",
+        "Point Theta Controller/P": "6.0",
         "Pathfinding Y Controller/P": "1.0",
         "Shoulder L3 Angles/x2": "0.0",
-        "Pathfinding X Controller/P": "1.0"
+        "Pathfinding X Controller/P": "1.0",
+        "Point Theta Controller/I": "0.0"
     }
 }
diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java
@@ -84,8 +84,8 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                 goToIntake.withChild(goToReefIntake, buttonBoard::getAlgaeMode, 0, "Reef selected")
                                 .withChild(goToStationIntake, () -> !buttonBoard.getAlgaeMode(), 1, "Station selected");
 
-                intakeGamePiece.withChild(intakeCoral, buttonBoard::getAlgaeMode, 0, "Reef selected")
-                                .withChild(intakeAlgae, () -> !buttonBoard.getAlgaeMode(), 1, "Station selected");
+                intakeGamePiece.withChild(intakeCoral, () -> !buttonBoard.getAlgaeMode(), 0, "Reef selected")
+                                .withChild(intakeAlgae, buttonBoard::getAlgaeMode, 1, "Station selected");
 
                 // Specific Algae intake and score
                 goToScoreAlgae.withChild(goToScoreNet, () -> !buttonBoard

diff --git a/src/main/java/frc/robot/statemachine/states/tele/IntakeCoral.java b/src/main/java/frc/robot/statemachine/states/tele/IntakeCoral.java
@@ -14,9 +14,9 @@ public IntakeCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, D
             EndEffector endEffector, LEDs LEDs) {
         super(stateMachine);
 
-        // TODO: Get others to make heights and (Marcus) rotation
         startWhenActive(DriveCommands.pointControl(drive, buttonBoard::getIntakeStationTarget));
         startWhenActive(endEffector.runIntake());
+        // TODO: Elevator/Shoulder
         startWhenActive(LEDs.intakingAndScoringCoral());
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java
@@ -214,8 +214,12 @@ public static Command goToPoint(Drive drive, Supplier<Pose2d> pose) {
 
     public static Command pointControl(Drive drive, Supplier<Pose2d> pose) {
         return Commands
-                .run(() -> drive.runVelocity(DriveConstants.pointController.calculate(drive.getPose(), pose.get(), 0,
-                        pose.get().getRotation())));
+                .run(() -> {
+                    drive.runVelocity(DriveConstants.pointController.calculate(drive.getPose(), pose.get(), 0,
+                            pose.get().getRotation()));
+
+                    System.out.println(pose.get().getY());
+                }).withName("Point Control");
     }
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -131,7 +131,7 @@ public class DriveConstants {
                         .withSwerveModule(COTS.ofMAXSwerve(driveGearbox, turnGearbox, wheelCOF, 2));
 
         public static final Pathfinder pathfinder = (new PathfinderBuilder(Field.REEFSCAPE_2025))
-                        .setRobotLength(mapleBumperSize.in(Meters) + 0.05)
+                        .setNormalizeCorners(false).setRobotLength(mapleBumperSize.in(Meters) + 0.05)
                         .setRobotWidth(mapleBumperSize.in(Meters) + 0.05).build();
 
         public static final PIDController xController = new ConfigurablePIDController(1, 0, 0,