Fix the point controller

FRCTeam3044 · Feb 15, 2025 · badca41 · badca41
1 parent 8a3d3a6
commit badca41
Show file tree

Hide file tree

Showing 7 changed files with 29 additions and 22 deletions.
diff --git a/simgui.json b/simgui.json
@@ -64,14 +64,13 @@
   "NetworkTables": {
     "transitory": {
       "AdvantageKit": {
-        "Drive": {
-          "open": false
-        },
         "RealOutputs": {
-          "FieldSimulation": {
-            "open": false
-          }
-        }
+          "StateMachine": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
       },
       "LiveWindow": {
         "open": true

diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java
@@ -115,7 +115,8 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                                 .withTransition(manual, () -> !driverController.rightTrigger()
                                                 .getAsBoolean(), "Score button released");
 
-                scoreGamePiece.withTransition(goToScoringPosition, () -> false, "Scoring location changed")
+                scoreGamePiece.withTransition(goToScoringPosition, () -> !buttonBoard.closeToScoringTarget(drive),
+                                "Far from scoring location")
                                 .withTransition(manual, () -> !driverController.rightTrigger()
                                                 .getAsBoolean(), "Score button released")
                                 .withTransition(manual, () -> !endEffector.hasCoral() && !endEffector.hasAlgae(),
@@ -126,7 +127,8 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                                 .withTransition(manual, () -> !driverController.leftTrigger()
                                                 .getAsBoolean(), "Intake button released");
 
-                intakeGamePiece.withTransition(goToIntake, () -> false, "Intake location changed")
+                intakeGamePiece.withTransition(goToIntake, () -> !buttonBoard.closeToIntakeTarget(drive),
+                                "Intake location changed")
                                 .withTransition(manual, () -> !driverController.leftTrigger()
                                                 .getAsBoolean(), "Intake button released")
                                 .withTransition(manual, () -> endEffector.hasCoral() || endEffector.hasAlgae(),

diff --git a/src/main/java/frc/robot/statemachine/states/tele/ManualState.java b/src/main/java/frc/robot/statemachine/states/tele/ManualState.java
@@ -21,7 +21,6 @@
 import frc.robot.util.AllianceUtil;
 
 public class ManualState extends State {
-        @SuppressWarnings("unused")
         public ManualState(StateMachineBase stateMachine, CommandXboxController driver, CommandXboxController operator,
                         Drive drive, Elevator elevator, Shoulder shoulder, EndEffector endEffector, LEDs LEDs) {
                 super(stateMachine);
@@ -60,7 +59,6 @@ public ManualState(StateMachineBase stateMachine, CommandXboxController driver,
                 DoubleSupplier rightY = () -> -MathUtil.applyDeadband(operatorController.getHID().getRightY(), 0.01);
                 DoubleSupplier leftY = () -> -MathUtil.applyDeadband(operatorController.getHID().getLeftY(), 0.05);
 
-                startWhenActive(LEDs.simMorseCode());
                 startWhenActive(elevator.elevatorMove(rightY));
                 startWhenActive(shoulder.manualPivot(leftY));
         }

diff --git a/src/main/java/frc/robot/subsystems/LEDs/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs/LEDs.java
@@ -54,7 +54,7 @@ public Command intakingAndScoringAlgae() {
     }
 
     public Command Default() {
-        return Commands.run(() -> io.setSpinningColor(Color.kPurple, Color.kYellow));
+        return Commands.run(() -> io.setSpinningColor(Color.kPurple, Color.kYellow)).withName("Spinning LEDs");
     }
 
     public Command simMorseCode() {

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java
@@ -213,13 +213,13 @@ 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()));
-
-                    System.out.println(pose.get().getY());
-                }).withName("Point Control");
+        return Commands.startRun(() -> {
+            DriveConstants.anglePointController.reset(drive.getPose().getRotation().getRadians());
+        }, () -> {
+            Pose2d targetPose = pose.get();
+            drive.runVelocity(DriveConstants.pointController.calculate(drive.getPose(), targetPose, 0,
+                    targetPose.getRotation()));
+        }).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,8 +131,9 @@ public class DriveConstants {
                         .withSwerveModule(COTS.ofMAXSwerve(driveGearbox, turnGearbox, wheelCOF, 2));
 
         public static final Pathfinder pathfinder = (new PathfinderBuilder(Field.REEFSCAPE_2025))
-                        .setNormalizeCorners(false).setRobotLength(mapleBumperSize.in(Meters) + 0.05)
-                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.05).build();
+                        .setNormalizeCorners(false).setCornerDist(0.5)
+                        .setRobotLength(mapleBumperSize.in(Meters) + 0.1)
+                        .setRobotWidth(mapleBumperSize.in(Meters) + 0.1).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
@@ -194,11 +194,18 @@ public boolean isProcessor() {
     public boolean closeToScoringTarget(Drive drive) {
         if (algaeMode) {
             if (isProcessor) {
+                if (coralReefTargetPose == null) {
+                    return false;
+                }
                 return AutoTargetUtils.robotDistToPose(drive, algaeReefTargetPose) < processorDistThreshold.get();
             } else {
-                return AutoTargetUtils.robotDistToPose(drive, coralReefTargetPose) < coralReefDistThreshold.get();
+                // TODO: Net
+                return false;
             }
         } else {
+            if (coralReefTargetPose == null) {
+                return false;
+            }
             return AutoTargetUtils.robotDistToPose(drive, coralReefTargetPose) < coralReefDistThreshold.get();
         }
     }