[commands] Add CommandRobot

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandRobot.java

@@ -0,0 +1,96 @@
+// 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.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * CommandRobot is a wrapper over TimedRobot designed to work well with Command based programming.
+ *
+ * <p>The CommandRobot class is intended to be subclassed by a user when creating a robot program.
+ */
+public class CommandRobot extends TimedRobot {
+  /** The CommandScheduler instance. */
+  protected CommandScheduler m_scheduler = CommandScheduler.getInstance();
+
+  /** The autonomous chooser. */
+  protected SendableChooser<Command> m_autoChooser = new SendableChooser<>();
+
+  private Command m_autonomousCommand;
+
+  /** Constructor for CommandRobot. */
+  protected CommandRobot() {
+    this(kDefaultPeriod);
+  }
+
+  /**
+   * Constructor for CommandRobot.
+   *
+   * @param period Period in seconds.
+   */
+  protected CommandRobot(double period) {
+    super(period);
+    addPeriodic(() -> m_scheduler.run(), 0.02);
+    m_autoChooser.setDefaultOption(
+        "No autos configured.",
+        Commands.print("No autos configured. Add Commands to m_autoChooser to fix this."));
+    SmartDashboard.putData(m_autoChooser);
+  }
+
+  @Override
+  public final void robotInit() {}
+
+  @Override
+  public final void robotPeriodic() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_autoChooser.getSelected();
+    if (m_autonomousCommand != null) {
+      m_scheduler.schedule(m_autonomousCommand);
+    }
+  }
+
+  @Override
+  public final void autonomousPeriodic() {}
+
+  @Override
+  public void autonomousExit() {}
+
+  @Override
+  public final void teleopInit() {
+    if (m_autonomousCommand != null) {
+      m_scheduler.cancel(m_autonomousCommand);
+    }
+  }
+
+  @Override
+  public final void teleopPeriodic() {}
+
+  @Override
+  public final void teleopExit() {}
+
+  @Override
+  public final void disabledInit() {}
+
+  @Override
+  public final void disabledPeriodic() {}
+
+  @Override
+  public final void disabledExit() {}
+
+  @Override
+  public void testInit() {
+    m_scheduler.cancelAll();
+  }
+
+  @Override
+  public void testPeriodic() {}
+
+  @Override
+  public void testExit() {}
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandRobot.cpp

@@ -0,0 +1,46 @@
+// 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 "frc2/command/CommandRobot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/time.h>
+
+frc2::CommandRobot::CommandRobot(units::second_t period)
+    : frc::TimedRobot(period) {
+  AddPeriodic([this] { m_scheduler.Run(); }, 20_ms);
+  m_autoChooser.SetDefaultOption("No auto configured.", m_defaultCommand.get());
+  frc::SmartDashboard::PutData(&m_autoChooser);
+}
+
+void frc2::CommandRobot::RobotInit() {}
+
+void frc2::CommandRobot::RobotPeriodic() {}
+
+void frc2::CommandRobot::AutonomousInit() {
+  m_autonomousCommand = m_autoChooser.GetSelected();
+  if (m_autonomousCommand) {
+    m_scheduler.Schedule(m_autonomousCommand);
+  }
+}
+
+void frc2::CommandRobot::AutonomousPeriodic() {}
+
+void frc2::CommandRobot::AutonomousExit() {}
+
+void frc2::CommandRobot::TeleopInit() {
+  if (m_autonomousCommand) {
+    m_scheduler.Cancel(m_autonomousCommand);
+  }
+}
+
+void frc2::CommandRobot::TeleopPeriodic() {}
+
+void frc2::CommandRobot::TeleopExit() {}
+
+void frc2::CommandRobot::DisabledInit() {}
+
+void frc2::CommandRobot::DisabledPeriodic() {}
+
+void frc2::CommandRobot::DisabledExit() {}

wpilibNewCommands/src/main/native/include/frc2/command/CommandRobot.h

@@ -0,0 +1,67 @@
+// 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 <frc/TimedRobot.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <units/time.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/Commands.h"
+
+namespace frc2 {
+/**
+  CommandRobot is a wrapper over TimedRobot designed to work well with Command
+ * based programming.
+ *
+ * <p>The CommandRobot class is intended to be subclassed by a user when
+ creating a
+ * robot program.
+ */
+class CommandRobot : public frc::TimedRobot {
+ public:
+  void RobotInit() final;
+
+  void RobotPeriodic() final;
+
+  void AutonomousInit() final;
+
+  void AutonomousPeriodic() final;
+
+  void AutonomousExit() final;
+
+  void TeleopInit() final;
+
+  void TeleopPeriodic() final;
+
+  void TeleopExit() final;
+
+  void DisabledInit() final;
+
+  void DisabledPeriodic() final;
+
+  void DisabledExit() final;
+
+ protected:
+  /**
+   * Constructor for CommandRobot.
+   *
+   * @param period Period in seconds.
+   */
+  explicit CommandRobot(units::second_t period = kDefaultPeriod);
+
+  /// The CommandScheduler instance.
+  frc2::CommandScheduler& m_scheduler = frc2::CommandScheduler::GetInstance();
+
+  /// The autonomous chooser.
+  frc::SendableChooser<frc2::Command*> m_autoChooser;
+
+ private:
+  frc2::CommandPtr m_defaultCommand = frc2::cmd::Print(
+      "No autos configured. Add Commands to m_autoChooser to fix this.");
+  frc2::Command* m_autonomousCommand = nullptr;
+};
+}  // namespace frc2

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp

@@ -4,53 +4,47 @@
 
 #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 <frc2/command/Command.h>
+#include <frc2/command/Commands.h>
+#include <frc2/command/button/Trigger.h>
 
-  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_autoChooser.SetDefaultOption("Example Auto", m_exampleAuto.get());
 }
 
-void Robot::TestPeriodic() {}
-
 #ifndef RUNNING_FRC_TESTS
 int main() {
   return frc::StartRobot<Robot>();