Add Simple Auto (Leave)

src/main/java/frc/robot/RobotContainer.java

@@ -8,9 +8,13 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+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.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.autonomous.Leave;
 import frc.robot.subsystems.drive.DriveSubsystem;
 
 public class RobotContainer {
@@ -28,6 +32,8 @@ public class RobotContainer {
 
   private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
 
+  private static SendableChooser<SequentialCommandGroup> m_automodeChooser = new SendableChooser<>();
+
   public RobotContainer() {
     // Set drive command
     DRIVE_SUBSYSTEM.setDefaultCommand(
@@ -43,6 +49,9 @@ public RobotContainer() {
 
     // Bind buttons and triggers
     configureBindings();
+
+    // Configure ShuffleBoard
+    defaultShuffleboardTab();
   }
 
   private void configureBindings() {
@@ -61,6 +70,14 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.a().whileTrue(DRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SOURCE));
   }
 
+  /**
+   * Add auto modes to chooser
+   */
+  private void autoModeChooser() {
+    m_automodeChooser.setDefaultOption("Do nothing", new SequentialCommandGroup());
+    m_automodeChooser.addOption("Balance", new Leave(DRIVE_SUBSYSTEM));
+  }
+
   /**
    * Run simlation related methods
    */
@@ -73,6 +90,15 @@ public void simulationPeriodic() {
    * @return Autonomous command
    */
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return m_automodeChooser.getSelected();
+  }
+
+  /**
+   * Configure default Shuffleboard tab
+   */
+  public void defaultShuffleboardTab() {
+    Shuffleboard.selectTab(Constants.SmartDashboard.SMARTDASHBOARD_DEFAULT_TAB);
+    autoModeChooser();
+    SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser);
   }
 }

src/main/java/frc/robot/commands/autonomous/Leave.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.autonomous;
+
+import java.util.List;
+
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPoint;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.drive.AutoTrajectory;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class Leave extends SequentialCommandGroup {
+  /** Creates a new Leave. */
+  public Leave(DriveSubsystem driveSubsystem) {
+    List<PathPoint> path = List.of(
+      new PathPoint(new Translation2d(0.0, 0.0)),
+      new PathPoint(new Translation2d(3.2, 0.0))
+    );
+    PathConstraints constraints = new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI);
+
+    addCommands(
+      new AutoTrajectory(driveSubsystem, path, constraints).getCommandAndStop()
+    );
+  }
+}