fix code crash

LakotaRobotics1038 · Feb 19, 2025 · 7e83035 · 7e83035
1 parent f145057
commit 7e83035
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 32 deletions.
diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java
@@ -94,20 +94,21 @@ private DriverJoystick() {
 
         // Lock the wheels into an X formation
         super.xButton.whileTrue(this.driveTrain.setX());
-        super.aButton.whileTrue(
-                new InstantCommand(() -> {
-                    this.currentPose = this.driveTrain.getState().Pose;
-                }).andThen(AutoBuilder.followPath(new PathPlannerPath(
-                        PathPlannerPath.waypointsFromPoses(this.currentPose, DriveWaypoints.Algae23.getEndpoint()),
-                        new PathConstraints(
-                                DriveConstants.MaxSpeed,
-                                AutoConstants.kMaxAccelerationMetersPerSecondSquared,
-                                AutoConstants.kMaxAngularSpeedRadiansPerSecond,
-                                AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared),
-                        new IdealStartingState(
-                                driveTrain.getState().Speeds.vxMetersPerSecond,
-                                driveTrain.getState().Pose.getRotation()),
-                        new GoalEndState(0, Rotation2d.kZero)))));
+        // super.aButton.whileTrue(
+        // new InstantCommand(() -> {
+        // this.currentPose = this.driveTrain.getState().Pose;
+        // }).andThen(AutoBuilder.followPath(new PathPlannerPath(
+        // PathPlannerPath.waypointsFromPoses(this.currentPose,
+        // DriveWaypoints.Algae23.getEndpoint()),
+        // new PathConstraints(
+        // DriveConstants.MaxSpeed,
+        // AutoConstants.kMaxAccelerationMetersPerSecondSquared,
+        // AutoConstants.kMaxAngularSpeedRadiansPerSecond,
+        // AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared),
+        // new IdealStartingState(
+        // driveTrain.getState().Speeds.vxMetersPerSecond,
+        // driveTrain.getState().Pose.getRotation()),
+        // new GoalEndState(0, Rotation2d.kZero)))));
     }
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -151,24 +151,26 @@ private DriveTrain() {
                 SwerveConstants.FrontRight,
                 SwerveConstants.BackLeft,
                 SwerveConstants.FrontRight);
-        AutoBuilder.configure(
-                () -> this.getState().Pose,
-                this::resetPose,
-                () -> this.getState().Speeds,
-                (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> {
-                    this.setControl(
-                            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds)
-                                    .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons())
-                                    .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()));
-                },
-                new PPHolonomicDriveController(
-                        new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController,
-                                AutoConstants.kDController),
-                        new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController,
-                                AutoConstants.kDThetaController)),
-                AutoConstants.kRobotConfig.get(),
-                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
-                this);
+        if (AutoConstants.kRobotConfig.isPresent()) {
+            AutoBuilder.configure(
+                    () -> this.getState().Pose,
+                    this::resetPose,
+                    () -> this.getState().Speeds,
+                    (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> {
+                        this.setControl(
+                                new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds)
+                                        .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons())
+                                        .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()));
+                    },
+                    new PPHolonomicDriveController(
+                            new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController,
+                                    AutoConstants.kDController),
+                            new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController,
+                                    AutoConstants.kDThetaController)),
+                    AutoConstants.kRobotConfig.get(),
+                    () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                    this);
+        }
         if (Utils.isSimulation()) {
             startSimThread();
         }