diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a13ca6..dfe3032 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -79,7 +79,8 @@ public void autonomousInit() { // FIXME: Add - if the match starts, then get the autonomous command (we already - // are running the program through the following statements below - defaultAutoCommand.schedule(); + robotContainer.getAutonomousCommand().schedule(); + } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cb6a4f8..bf193c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -13,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.Mode; +import frc.robot.commands.DefaultAutoCommand; import frc.robot.subsystems.RGBSubsystem; import frc.robot.subsystems.flywheels.Flywheels; import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX; @@ -518,5 +520,13 @@ public void containerMatchStarting() { CommandScheduler.getInstance().schedule(/*FIXME - Vibrate the controller */ ); } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new WaitCommand(1).andThen(new DefaultAutoCommand(rollers, swerve, flywheels, superstructure)); + } private void configureAutos() {} } diff --git a/src/main/java/frc/robot/commands/DefaultAutoCommand.java b/src/main/java/frc/robot/commands/DefaultAutoCommand.java index fc0d59b..fa96bf8 100644 --- a/src/main/java/frc/robot/commands/DefaultAutoCommand.java +++ b/src/main/java/frc/robot/commands/DefaultAutoCommand.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.rollers.Rollers; import frc.robot.subsystems.rollers.Rollers.RollerState; import frc.robot.subsystems.superstructure.Superstructure; +import frc.robot.subsystems.superstructure.Superstructure.SuperstructureState; import frc.robot.subsystems.swerve.Drive; public class DefaultAutoCommand extends SequentialCommandGroup { @@ -22,6 +23,7 @@ public DefaultAutoCommand( interrupted -> { rollers.setTargetState(RollerState.IDLE); flywheels.setVelocityTarget(Flywheels.VelocityTarget.SHOOT); + superstructure.setTargetState(SuperstructureState.SUBWOOF_SHOT); }, () -> rollers.acceleratorDetected(), rollers,