diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d48e1ca3..b690c880 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ 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.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -11,6 +12,7 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.CircleAutoCmd; import frc.robot.commands.CommandFactory; +import frc.robot.commands.CustomTestCmd; import frc.robot.commands.StraightAutoCmd; import frc.robot.commands.SwerveJoystickCmd; import frc.robot.subsystems.SwerveSubsystem; @@ -31,7 +33,9 @@ public class RobotContainer { private final Command m_circleAuto = new CircleAutoCmd(m_swerveSubsystem); - SendableChooser m_chooser = new SendableChooser<>(); + private final Command m_customAuto = new CustomTestCmd(m_swerveSubsystem); + + public SendableChooser m_chooser = new SendableChooser<>(); public RobotContainer() { commandFactory = new CommandFactory(m_swerveSubsystem); @@ -58,8 +62,10 @@ public RobotContainer() { m_chooser.addOption("Circle Auto", m_circleAuto); + m_chooser.addOption("Custom Auto", m_customAuto); + // Put the chooser on the dashboard - Shuffleboard.getTab("Auto Options").add(m_chooser); + SmartDashboard.putData("Auto Options", m_chooser); } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/CustomTestCmd.java b/src/main/java/frc/robot/commands/CustomTestCmd.java new file mode 100644 index 00000000..ea84b392 --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomTestCmd.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import frc.robot.commands.DriveStraightCmd; +import frc.robot.commands.RotateDegreesCmd; +import frc.robot.subsystems.SwerveSubsystem; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class CustomTestCmd extends SequentialCommandGroup{ + + public CustomTestCmd(SwerveSubsystem swerveSubsystem) { + addCommands( + new DriveStraightCmd(0.0, 2.0, swerveSubsystem), + new RotateDegreesCmd(180.0, true, swerveSubsystem), + new DriveStraightCmd(0.0, 2.0, swerveSubsystem) + ); + } + + + +}