Clean up Path planner stuff. (#17)

FRC-Team-620 · Jan 9, 2024 · a6d7771 · a6d7771
1 parent 18666fb
commit a6d7771
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 32 deletions.
diff --git a/src/main/java/org/jmhsrobotics/frc2024/Robot.java b/src/main/java/org/jmhsrobotics/frc2024/Robot.java
@@ -8,8 +8,6 @@
 
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
@@ -18,29 +16,21 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 public class Robot extends TimedRobot {
-  private Command m_autonomousCommand;
+  private Command autonomousCommand;
 
   private RobotContainer m_robotContainer;
-  private SendableChooser<Command> autoChooser;
 
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
-    NamedCommands.registerCommand("Intake", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5));
-    NamedCommands.registerCommand("Wait", new WaitCommand(30));
-    autoChooser = AutoBuilder.buildAutoChooser();
-    SmartDashboard.putData("Auto Chooser", autoChooser);
     SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5));
 
   }
-
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
@@ -60,11 +50,10 @@ public void disabledExit() {
 
   @Override
   public void autonomousInit() {
-    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-    m_autonomousCommand = autoChooser.getSelected();
+    autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
+    if (autonomousCommand != null) {
+      autonomousCommand.schedule();
     }
 
     List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
@@ -95,8 +84,8 @@ public void autonomousExit() {
 
   @Override
   public void teleopInit() {
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
   }
 

diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java
@@ -6,31 +6,77 @@
 
 import org.jmhsrobotics.frc2024.controlBoard.CompControl;
 import org.jmhsrobotics.frc2024.controlBoard.ControlBoard;
+import org.jmhsrobotics.frc2024.subsystems.drive.DriveConstants;
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
+import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 public class RobotContainer {
 
   private ControlBoard control = new CompControl();
-  private DriveSubsystem driveSubsystem = new DriveSubsystem();
+  // Subsystems
+  private final DriveSubsystem driveSubsystem = new DriveSubsystem();
+
+  private final SendableChooser<Command> autoChooser;
 
   public RobotContainer() {
+    // Named commands must be added before building the chooser.
+    NamedCommands.registerCommand("Intake", new IntakeCommand(driveSubsystem, 5));
+    NamedCommands.registerCommand("Wait", new WaitCommand(30));
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
 
     this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control));
     SmartDashboard.putData("Schedular", CommandScheduler.getInstance());
     configureBindings();
+    configurePathPlanner();
+  }
+
+  private void configurePathPlanner() {
+    // Add path planner auto chooser.
+
+    AutoBuilder.configureHolonomic(driveSubsystem::getPose, driveSubsystem::resetOdometry,
+        driveSubsystem::getChassisSpeeds, driveSubsystem::drive,
+        new HolonomicPathFollowerConfig(new PIDConstants(.5, 0, 0), new PIDConstants(1.5, 0, 0),
+            DriveConstants.SwerveConstants.kMaxSpeedMetersPerSecond, .5, new ReplanningConfig()),
+        this::getAllianceFlipState,
+        driveSubsystem);
+    NamedCommands.registerCommand("Intake", new IntakeCommand(driveSubsystem, 5));
+    NamedCommands.registerCommand("Wait", new WaitCommand(30));
+  }
+
+  // TODO: fix this later to flip correctly based on side color
+  // TODO: Check parity of this
+  private boolean getAllianceFlipState() {
+    return DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() == Alliance.Blue
+        : false;
   }
 
   private void configureBindings() {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    Command picked = autoChooser.getSelected();
+    if (picked == null) {
+      return Commands.print("No autonomous command configured");
+    } else {
+      return picked;
+    }
   }
 
   public DriveSubsystem getDriveSubsystem() {

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java
@@ -90,19 +90,6 @@ public DriveSubsystem() {
       m_rearLeft = new SimSwerveModule();
       m_rearRight = new SimSwerveModule();
     }
-
-    AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, this::getChassisSpeeds, this::drive,
-        new HolonomicPathFollowerConfig(new PIDConstants(.5, 0, 0), new PIDConstants(1.5, 0, 0),
-            DriveConstants.SwerveConstants.kMaxSpeedMetersPerSecond, .5, new ReplanningConfig()),
-        this::getAllianceFlipState,
-        this);
-
-  }
-
-  // TODO: fix this later to flip correctly based on side color
-  private boolean getAllianceFlipState() {
-    return DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() == Alliance.Blue
-        : false;
   }
 
   @Override