diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp deleted file mode 100644 index 653aad3ce52..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// 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. - -#include "RapidReactCommandBot.h" - -#include -#include -#include - -#include "Constants.h" - -void RapidReactCommandBot::ConfigureBindings() { - // Automatically run the storage motor whenever the ball storage is not full, - // and turn it off whenever it fills. Uses subsystem-hosted trigger to - // improve readability and make inter-subsystem communication easier. - m_storage.HasCargo.WhileFalse(m_storage.RunCommand()); - - // Automatically disable and retract the intake whenever the ball storage is - // full. - m_storage.HasCargo.OnTrue(m_intake.RetractCommand()); - - // Control the drive with split-stick arcade controls - m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand( - [this] { return -m_driverController.GetLeftY(); }, - [this] { return -m_driverController.GetRightX(); })); - - // Deploy the intake with the X button - m_driverController.X().OnTrue(m_intake.IntakeCommand()); - // Retract the intake with the Y button - m_driverController.Y().OnTrue(m_intake.RetractCommand()); - - // Fire the shooter with the A button - m_driverController.A().OnTrue( - frc2::cmd::Parallel( - m_shooter.ShootCommand(ShooterConstants::kShooterTarget), - m_storage.RunCommand()) - // Since we composed this inline we should give it a name - .WithName("Shoot")); - - // Toggle compressor with the Start button - m_driverController.Start().ToggleOnTrue( - m_pneumatics.DisableCompressorCommand()); -} - -frc2::CommandPtr RapidReactCommandBot::GetAutonomousCommand() { - return m_drive - .DriveDistanceCommand(AutoConstants::kDriveDistance, - AutoConstants::kDriveSpeed) - .WithTimeout(AutoConstants::kTimeout); -} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp index d28c275f8ef..43e03222b19 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp @@ -4,53 +4,51 @@ #include "Robot.h" -Robot::Robot() { - // Configure default commands and condition bindings on robot startup - m_robot.ConfigureBindings(); -} - -void Robot::RobotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled commands, running already-scheduled commands, removing - // finished or interrupted commands, and running subsystem Periodic() methods. - // This must be called from the robot's periodic block in order for anything - // in the Command-based framework to work. - frc2::CommandScheduler::GetInstance().Run(); -} - -void Robot::DisabledInit() {} - -void Robot::DisabledPeriodic() {} - -void Robot::AutonomousInit() { - m_autonomousCommand = m_robot.GetAutonomousCommand(); +#include +#include +#include - if (m_autonomousCommand) { - m_autonomousCommand->Schedule(); - } -} - -void Robot::AutonomousPeriodic() {} - -void Robot::TeleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand) { - m_autonomousCommand->Cancel(); - } -} +#include "Constants.h" -void Robot::TeleopPeriodic() {} - -void Robot::TestInit() { - // Cancels all running commands at the start of test mode. - frc2::CommandScheduler::GetInstance().CancelAll(); +Robot::Robot() { + // Automatically run the storage motor whenever the ball storage is not full, + // and turn it off whenever it fills. Uses subsystem-hosted trigger to + // improve readability and make inter-subsystem communication easier. + m_storage.HasCargo.WhileFalse(m_storage.RunCommand()); + + // Automatically disable and retract the intake whenever the ball storage is + // full. + m_storage.HasCargo.OnTrue(m_intake.RetractCommand()); + + // Control the drive with split-stick arcade controls + m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand( + [this] { return -m_driverController.GetLeftY(); }, + [this] { return -m_driverController.GetRightX(); })); + + // Deploy the intake with the X button + m_driverController.X().OnTrue(m_intake.IntakeCommand()); + // Retract the intake with the Y button + m_driverController.Y().OnTrue(m_intake.RetractCommand()); + + // Fire the shooter with the A button + m_driverController.A().OnTrue( + frc2::cmd::Parallel( + m_shooter.ShootCommand(ShooterConstants::kShooterTarget), + m_storage.RunCommand()) + // Since we composed this inline we should give it a name + .WithName("Shoot")); + + // Toggle compressor with the Start button + m_driverController.Start().ToggleOnTrue( + m_pneumatics.DisableCompressorCommand()); + + m_exampleAuto = m_drive + .DriveDistanceCommand(AutoConstants::kDriveDistance, + AutoConstants::kDriveSpeed) + .WithTimeout(AutoConstants::kTimeout); + m_autoChooser.SetDefaultOption("Example Auto", m_exampleAuto.get()); } -void Robot::TestPeriodic() {} - #ifndef RUNNING_FRC_TESTS int main() { return frc::StartRobot(); diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h deleted file mode 100644 index 77f8340e6dc..00000000000 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h +++ /dev/null @@ -1,54 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include "Constants.h" -#include "subsystems/Drive.h" -#include "subsystems/Intake.h" -#include "subsystems/Pneumatics.h" -#include "subsystems/Shooter.h" -#include "subsystems/Storage.h" - -/** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic should - * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, - * commands, and button mappings) should be declared here. - */ -class RapidReactCommandBot { - public: - /** - * Use this method to define bindings between conditions and commands. These - * are useful for automating robot behaviors based on button and sensor input. - * - *

Should be called in the robot class constructor. - * - *

Event binding methods are available on the frc2::Trigger class. - */ - void ConfigureBindings(); - - /** - * Use this to define the command that runs during autonomous. - * - *

Scheduled during Robot::AutonomousInit(). - */ - frc2::CommandPtr GetAutonomousCommand(); - - private: - // The robot's subsystems - Drive m_drive; - Intake m_intake; - Shooter m_shooter; - Storage m_storage; - Pneumatics m_pneumatics; - - // The driver's controller - frc2::CommandXboxController m_driverController{ - OIConstants::kDriverControllerPort}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h index e88daeecef9..1dbcfcb030f 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h @@ -4,27 +4,32 @@ #pragma once -#include - -#include #include +#include +#include -#include "RapidReactCommandBot.h" +#include "Constants.h" +#include "subsystems/Drive.h" +#include "subsystems/Intake.h" +#include "subsystems/Pneumatics.h" +#include "subsystems/Shooter.h" +#include "subsystems/Storage.h" -class Robot : public frc::TimedRobot { +class Robot : public frc2::CommandRobot { public: Robot(); - void RobotPeriodic() override; - void DisabledInit() override; - void DisabledPeriodic() override; - void AutonomousInit() override; - void AutonomousPeriodic() override; - void TeleopInit() override; - void TeleopPeriodic() override; - void TestInit() override; - void TestPeriodic() override; private: - RapidReactCommandBot m_robot; - std::optional m_autonomousCommand; + // The robot's subsystems + Drive m_drive; + Intake m_intake; + Shooter m_shooter; + Storage m_storage; + Pneumatics m_pneumatics; + + // The driver's controller + frc2::CommandXboxController m_driverController{ + OIConstants::kDriverControllerPort}; + + frc2::CommandPtr m_exampleAuto; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java deleted file mode 100644 index a4273a481ae..00000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java +++ /dev/null @@ -1,93 +0,0 @@ -// 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 edu.wpi.first.wpilibj.examples.rapidreactcommandbot; - -import static edu.wpi.first.wpilibj2.command.Commands.parallel; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.AutoConstants; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Drive; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Intake; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Pneumatics; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Shooter; -import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Storage; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and button mappings) should be declared here. - */ -@Logged(name = "Rapid React Command Robot Container") -public class RapidReactCommandBot { - // The robot's subsystems - private final Drive m_drive = new Drive(); - private final Intake m_intake = new Intake(); - private final Storage m_storage = new Storage(); - private final Shooter m_shooter = new Shooter(); - private final Pneumatics m_pneumatics = new Pneumatics(); - - // The driver's controller - CommandXboxController m_driverController = - new CommandXboxController(OIConstants.kDriverControllerPort); - - /** - * Use this method to define bindings between conditions and commands. These are useful for - * automating robot behaviors based on button and sensor input. - * - *

Should be called in the robot class constructor. - * - *

Event binding methods are available on the {@link Trigger} class. - */ - public void configureBindings() { - // Automatically run the storage motor whenever the ball storage is not full, - // and turn it off whenever it fills. Uses subsystem-hosted trigger to - // improve readability and make inter-subsystem communication easier. - m_storage.hasCargo.whileFalse(m_storage.runCommand()); - - // Automatically disable and retract the intake whenever the ball storage is full. - m_storage.hasCargo.onTrue(m_intake.retractCommand()); - - // Control the drive with split-stick arcade controls - m_drive.setDefaultCommand( - m_drive.arcadeDriveCommand( - () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX())); - - // Deploy the intake with the X button - m_driverController.x().onTrue(m_intake.intakeCommand()); - // Retract the intake with the Y button - m_driverController.y().onTrue(m_intake.retractCommand()); - - // Fire the shooter with the A button - m_driverController - .a() - .onTrue( - parallel( - m_shooter.shootCommand(ShooterConstants.kShooterTargetRPS), - m_storage.runCommand()) - // Since we composed this inline we should give it a name - .withName("Shoot")); - - // Toggle compressor with the Start button - m_driverController.start().toggleOnTrue(m_pneumatics.disableCompressorCommand()); - } - - /** - * Use this to define the command that runs during autonomous. - * - *

Scheduled during {@link Robot#autonomousInit()}. - */ - public Command getAutonomousCommand() { - // Drive forward for 2 meters at half speed with a 3 second timeout - return m_drive - .driveDistanceCommand(AutoConstants.kDriveDistanceMeters, AutoConstants.kDriveSpeed) - .withTimeout(AutoConstants.kTimeoutSeconds); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java index bb0e24e5010..cd1aa1a4580 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java @@ -7,93 +7,81 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.OIConstants; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Drive; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Intake; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Pneumatics; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Shooter; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Storage; +import edu.wpi.first.wpilibj2.command.CommandRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. + * The VM is configured to automatically run this class, which in turn is configured to + * automatically run the {@link CommandScheduler}. */ @Logged(name = "Rapid React Command Robot") -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private final RapidReactCommandBot m_robot = new RapidReactCommandBot(); +public class Robot extends CommandRobot { + // The robot's subsystems + private final Drive m_drive = new Drive(); + private final Intake m_intake = new Intake(); + private final Storage m_storage = new Storage(); + private final Shooter m_shooter = new Shooter(); + private final Pneumatics m_pneumatics = new Pneumatics(); + + // The driver's controller + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); /** * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { - // Configure default commands and condition bindings on robot startup - m_robot.configureBindings(); + // Automatically run the storage motor whenever the ball storage is not full, + // and turn it off whenever it fills. Uses subsystem-hosted trigger to + // improve readability and make inter-subsystem communication easier. + m_storage.hasCargo.whileFalse(m_storage.runCommand()); + + // Automatically disable and retract the intake whenever the ball storage is + // full. + m_storage.hasCargo.onTrue(m_intake.retractCommand()); + + // Control the drive with split-stick arcade controls + m_drive.setDefaultCommand( + m_drive.arcadeDriveCommand( + () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX())); + + // Deploy the intake with the X button + m_driverController.x().onTrue(m_intake.intakeCommand()); + // Retract the intake with the Y button + m_driverController.y().onTrue(m_intake.retractCommand()); + + // Fire the shooter with the A button + m_driverController + .a() + .onTrue( + Commands.parallel( + m_shooter.shootCommand(ShooterConstants.kShooterTargetRPS), + m_storage.runCommand()) + // Since we composed this inline we should give it a name + .withName("Shoot")); + + // Toggle compressor with the Start button + m_driverController.start().toggleOnTrue(m_pneumatics.disableCompressorCommand()); // Initialize data logging. DataLogManager.start(); Epilogue.bind(this); - } - - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - @Override - public void autonomousInit() { - m_autonomousCommand = m_robot.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + m_autoChooser.setDefaultOption( + "Simple Auto", + m_drive + .driveDistanceCommand(AutoConstants.kDriveDistanceMeters, AutoConstants.kDriveSpeed) + .withTimeout(AutoConstants.kTimeoutSeconds)); } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java index 2dd1d395580..4d65a3d6d09 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java @@ -14,8 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * The VM is configured to automatically run this class, which in turn is - * configured to + * The VM is configured to automatically run this class, which in turn is configured to * automatically run the {@link CommandScheduler}. */ public class Robot extends CommandRobot { @@ -23,12 +22,11 @@ public class Robot extends CommandRobot { private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = new CommandXboxController( - OperatorConstants.kDriverControllerPort); + private final CommandXboxController m_driverController = + new CommandXboxController(OperatorConstants.kDriverControllerPort); /** - * This function is run when the robot is first started up and should be used - * for any + * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { @@ -41,7 +39,8 @@ public Robot() { // cancelling on release. m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); - m_autoChooser.setDefaultOption("Example Auto", Autos.exampleAuto(m_exampleSubsystem)); // An example command will be - // run in autonomous + m_autoChooser.setDefaultOption( + "Example Auto", Autos.exampleAuto(m_exampleSubsystem)); // An example command will be + // run in autonomous } }