From 1a59737f40dd267349987762268592d826ec95fb Mon Sep 17 00:00:00 2001 From: Eli Barnett Date: Mon, 28 Nov 2022 10:41:25 -0500 Subject: [PATCH] [commands] Add convenience factories (#4460) Co-authored-by: Starlight220 <53231611+Starlight220@users.noreply.github.com> --- .../wpi/first/wpilibj2/command/Subsystem.java | 48 ++++++++++ .../main/native/cpp/frc2/command/Command.cpp | 7 ++ .../native/cpp/frc2/command/CommandPtr.cpp | 6 ++ .../native/cpp/frc2/command/Subsystem.cpp | 19 ++++ .../native/include/frc2/command/Command.h | 22 +++++ .../native/include/frc2/command/CommandBase.h | 2 +- .../native/include/frc2/command/CommandPtr.h | 10 ++ .../native/include/frc2/command/Subsystem.h | 36 +++++++ .../cpp/RapidReactCommandBot.cpp | 50 ++++++++++ .../RapidReactCommandBot/cpp/Robot.cpp | 58 ++++++++++++ .../cpp/subsystems/Drive.cpp | 46 +++++++++ .../cpp/subsystems/Intake.cpp | 19 ++++ .../cpp/subsystems/Shooter.cpp | 38 ++++++++ .../cpp/subsystems/Storage.cpp | 18 ++++ .../RapidReactCommandBot/include/Constants.h | 79 ++++++++++++++++ .../include/RapidReactCommandBot.h | 52 ++++++++++ .../RapidReactCommandBot/include/Robot.h | 30 ++++++ .../include/subsystems/Drive.h | 58 ++++++++++++ .../include/subsystems/Intake.h | 32 +++++++ .../include/subsystems/Shooter.h | 44 +++++++++ .../include/subsystems/Storage.h | 26 +++++ .../src/main/cpp/examples/examples.json | 12 +++ .../wpi/first/wpilibj/examples/examples.json | 13 +++ .../rapidreactcommandbot/Constants.java | 83 ++++++++++++++++ .../examples/rapidreactcommandbot/Main.java | 25 +++++ .../RapidReactCommandBot.java | 84 +++++++++++++++++ .../examples/rapidreactcommandbot/Robot.java | 92 ++++++++++++++++++ .../subsystems/Drive.java | 94 +++++++++++++++++++ .../subsystems/Intake.java | 39 ++++++++ .../subsystems/Shooter.java | 66 +++++++++++++ .../subsystems/Storage.java | 33 +++++++ 31 files changed, 1240 insertions(+), 1 deletion(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h create mode 100644 wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Intake.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java index a42df7125c..147859b9b2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java @@ -77,4 +77,52 @@ public interface Subsystem { default void register() { CommandScheduler.getInstance().registerSubsystem(this); } + + /** + * Constructs a command that runs an action once and finishes. Requires this subsystem. + * + * @param action the action to run + * @return the command + * @see InstantCommand + */ + default Command runOnce(Runnable action) { + return Commands.runOnce(action, this); + } + + /** + * Constructs a command that runs an action every iteration until interrupted. Requires this + * subsystem. + * + * @param action the action to run + * @return the command + * @see RunCommand + */ + default Command run(Runnable action) { + return Commands.run(action, this); + } + + /** + * Constructs a command that runs an action once and another action when the command is + * interrupted. Requires this subsystem. + * + * @param start the action to run on start + * @param end the action to run on interrupt + * @return the command + * @see StartEndCommand + */ + default Command startEnd(Runnable start, Runnable end) { + return Commands.startEnd(start, end, this); + } + + /** + * Constructs a command that runs an action every iteration until interrupted, and then runs a + * second action. Requires this subsystem. + * + * @param run the action to run every iteration + * @param end the action to run on interrupt + * @return the command + */ + default Command runEnd(Runnable run, Runnable end) { + return Commands.runEnd(run, end, this); + } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 0abb5d820c..ee86b72ef3 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -105,6 +105,11 @@ CommandPtr Command::HandleInterrupt(std::function handler) && { return std::move(*this).ToPtr().HandleInterrupt(std::move(handler)); } +CommandPtr Command::WithName(std::string_view name) && { + SetName(name); + return std::move(*this).ToPtr(); +} + void Command::Schedule() { CommandScheduler::GetInstance().Schedule(this); } @@ -129,6 +134,8 @@ std::string Command::GetName() const { return GetTypeName(*this); } +void Command::SetName(std::string_view name) {} + bool Command::IsGrouped() const { return m_isGrouped; } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp index 8777e08023..a51be45ccb 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp @@ -219,6 +219,12 @@ CommandPtr CommandPtr::HandleInterrupt(std::function handler) && { }); } +CommandPtr CommandPtr::WithName(std::string_view name) && { + AssertValid(); + m_ptr->SetName(name); + return std::move(*this); +} + CommandBase* CommandPtr::get() const { AssertValid(); return m_ptr.get(); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp index 267a818979..3c388bbadf 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp @@ -5,6 +5,7 @@ #include "frc2/command/Subsystem.h" #include "frc2/command/CommandPtr.h" +#include "frc2/command/Commands.h" using namespace frc2; Subsystem::~Subsystem() { @@ -31,3 +32,21 @@ Command* Subsystem::GetCurrentCommand() const { void Subsystem::Register() { return CommandScheduler::GetInstance().RegisterSubsystem(this); } + +CommandPtr Subsystem::RunOnce(std::function action) { + return cmd::RunOnce(std::move(action), {this}); +} + +CommandPtr Subsystem::Run(std::function action) { + return cmd::Run(std::move(action), {this}); +} + +CommandPtr Subsystem::StartEnd(std::function start, + std::function end) { + return cmd::StartEnd(std::move(start), std::move(end), {this}); +} + +CommandPtr Subsystem::RunEnd(std::function run, + std::function end) { + return cmd::RunEnd(std::move(run), std::move(end), {this}); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index dcfc1c817b..c870a47095 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -282,6 +282,15 @@ safe) semantics. */ [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; + /** + * Decorates this Command with a name. Is an inline function for + * #SetName(std::string_view); + * + * @param name name + * @return the decorated Command + */ + [[nodiscard]] CommandPtr WithName(std::string_view name) &&; + /** * Schedules this command. */ @@ -343,8 +352,21 @@ safe) semantics. return InterruptionBehavior::kCancelSelf; } + /** + * Gets the name of this Command. Defaults to the simple class name if not + * overridden. + * + * @return The display name of the Command + */ virtual std::string GetName() const; + /** + * Sets the name of this Command. Nullop if not overridden. + * + * @param name The display name of the Command. + */ + virtual void SetName(std::string_view name); + /** * Transfers ownership of this command to a unique pointer. Used for * decorator methods. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h index 54326ff612..64bd88b7af 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h @@ -65,7 +65,7 @@ class CommandBase : public Command, * * @param name name */ - void SetName(std::string_view name); + void SetName(std::string_view name) override; /** * Gets the name of this Command. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h index d67ce777c3..b4fa60dcfe 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -219,6 +220,15 @@ class CommandPtr final { */ [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; + /** + * Decorates this Command with a name. Is an inline function for + * Command::SetName(std::string_view); + * + * @param name name + * @return the decorated Command + */ + [[nodiscard]] CommandPtr WithName(std::string_view name) &&; + /** * Get a raw pointer to the held command. */ diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index 67fc08db32..c38a099364 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h @@ -104,5 +104,41 @@ class Subsystem { * Periodic() method to be called when the scheduler runs. */ void Register(); + + /** + * Constructs a command that runs an action once and finishes. Requires this + * subsystem. + * + * @param action the action to run + */ + [[nodiscard]] CommandPtr RunOnce(std::function action); + + /** + * Constructs a command that runs an action every iteration until interrupted. + * Requires this subsystem. + * + * @param action the action to run + */ + [[nodiscard]] CommandPtr Run(std::function action); + + /** + * Constructs a command that runs an action once and another action when the + * command is interrupted. Requires this subsystem. + * + * @param start the action to run on start + * @param end the action to run on interrupt + */ + [[nodiscard]] CommandPtr StartEnd(std::function start, + std::function end); + + /** + * Constructs a command that runs an action every iteration until interrupted, + * and then runs a second action. Requires this subsystem. + * + * @param run the action to run every iteration + * @param end the action to run on interrupt + */ + [[nodiscard]] CommandPtr RunEnd(std::function run, + std::function end); }; } // namespace frc2 diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp new file mode 100644 index 0000000000..91058f001f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp @@ -0,0 +1,50 @@ +// 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. + frc2::Trigger([this] { + return m_storage.IsFull(); + }).WhileFalse(m_storage.RunCommand()); + + // Automatically disable and retract the intake whenever the ball storage is + // full. + frc2::Trigger([this] { + return m_storage.IsFull(); + }).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")); +} + +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 new file mode 100644 index 0000000000..06bbe40c9a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp @@ -0,0 +1,58 @@ +// 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 "Robot.h" + +void Robot::RobotInit() { + // 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(); + + 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(); + } +} + +void Robot::TeleopPeriodic() {} + +void Robot::TestInit() { + // Cancels all running commands at the start of test mode. + frc2::CommandScheduler::GetInstance().CancelAll(); +} + +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp new file mode 100644 index 0000000000..98ff0cac75 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.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 "subsystems/Drive.h" + +#include + +#include + +Drive::Drive() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.SetInverted(true); + + // Sets the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse); +} + +frc2::CommandPtr Drive::ArcadeDriveCommand(std::function fwd, + std::function rot) { + return Run([this, fwd = std::move(fwd), rot = std::move(rot)] { + m_drive.ArcadeDrive(fwd(), rot()); + }) + .WithName("ArcadeDrive"); +} + +frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance, + double speed) { + return RunOnce([this] { + // Reset encoders at the start of the command + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + }) + // Drive forward at specified speed + .AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); })) + .Until([this, distance] { + return units::math::max(units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())) >= + distance; + }) + // Stop the drive when the command ends + .FinallyDo([this](bool interrupted) { m_drive.StopMotor(); }); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp new file mode 100644 index 0000000000..20691850af --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp @@ -0,0 +1,19 @@ +// 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 "subsystems/Intake.h" + +frc2::CommandPtr Intake::IntakeCommand() { + return RunOnce([this] { m_piston.Set(frc::DoubleSolenoid::kForward); }) + .AndThen(Run([this] { m_motor.Set(1.0); })) + .WithName("Intake"); +} + +frc2::CommandPtr Intake::RetractCommand() { + return RunOnce([this] { + m_motor.Disable(); + m_piston.Set(frc::DoubleSolenoid::kReverse); + }) + .WithName("Retract"); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp new file mode 100644 index 0000000000..e8950ee464 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp @@ -0,0 +1,38 @@ +// 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 "subsystems/Shooter.h" + +#include + +Shooter::Shooter() { + m_shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value()); + m_shooterEncoder.SetDistancePerPulse( + ShooterConstants::kEncoderDistancePerPulse); + + SetDefaultCommand(RunOnce([this] { + m_shooterMotor.Disable(); + m_feederMotor.Disable(); + }) + .AndThen(Run([] {})) + .WithName("Idle")); +} + +frc2::CommandPtr Shooter::ShootCommand(units::turns_per_second_t setpoint) { + return frc2::cmd::Parallel( + // Run the shooter flywheel at the desired setpoint using + // feedforward and feedback + Run([this, setpoint] { + m_shooterMotor.SetVoltage( + m_shooterFeedforward.Calculate(setpoint) + + units::volt_t(m_shooterFeedback.Calculate( + m_shooterEncoder.GetRate(), setpoint.value()))); + }), + // Wait until the shooter has reached the setpoint, and then + // run the feeder + frc2::cmd::WaitUntil([this] { + return m_shooterFeedback.AtSetpoint(); + }).AndThen([this] { m_feederMotor.Set(1.0); })) + .WithName("Shoot"); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp new file mode 100644 index 0000000000..3e9dc6203b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp @@ -0,0 +1,18 @@ +// 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 "subsystems/Storage.h" + +Storage::Storage() { + SetDefaultCommand( + RunOnce([this] { m_motor.Disable(); }).AndThen([] {}).WithName("Idle")); +} + +frc2::CommandPtr Storage::RunCommand() { + return Run([this] { m_motor.Set(1.0); }).WithName("Run"); +} + +bool Storage::IsFull() const { + return m_ballSensor.Get(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h new file mode 100644 index 0000000000..330eebacc9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h @@ -0,0 +1,79 @@ +// 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 +#include +#include +#include + +namespace DriveConstants { +constexpr int kLeftMotor1Port = 0; +constexpr int kLeftMotor2Port = 1; +constexpr int kRightMotor1Port = 2; +constexpr int kRightMotor2Port = 3; + +constexpr int kLeftEncoderPorts[]{0, 1}; +constexpr int kRightEncoderPorts[]{2, 3}; +constexpr bool kLeftEncoderReversed = false; +constexpr bool kRightEncoderReversed = true; + +constexpr double kEncoderCPR = 1024; +constexpr units::meter_t kWheelDiameter = 6.0_in; +constexpr double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value(); + +} // namespace DriveConstants + +namespace IntakeConstants { +constexpr int kMotorPort = 6; +constexpr int kSolenoidPorts[]{0, 1}; +} // namespace IntakeConstants + +namespace StorageConstants { +constexpr int kMotorPort = 7; +constexpr int kBallSensorPort = 6; +} // namespace StorageConstants + +namespace ShooterConstants { +constexpr int kEncoderPorts[]{4, 5}; +constexpr bool kEncoderReversed = false; +constexpr double kEncoderCPR = 1024; +constexpr double kEncoderDistancePerPulse = + // Distance units will be rotations + 1.0 / kEncoderCPR; + +constexpr int kShooterMotorPort = 4; +constexpr int kFeederMotorPort = 5; + +constexpr auto kShooterFree = 5300_tps; +constexpr auto kShooterTarget = 4000_tps; +constexpr auto kShooterTolerance = 50_tps; + +// These are not real PID gains, and will have to be tuned for your specific +// robot. +constexpr double kP = 1; + +constexpr units::volt_t kS = 0.05_V; +constexpr auto kV = + // Should have value 12V at free speed... + 12.0_V / kShooterFree; + +constexpr double kFeederSpeed = 0.5; +} // namespace ShooterConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 0; +} // namespace OIConstants + +namespace AutoConstants { +constexpr units::second_t kTimeout = 3.0_s; +constexpr units::meter_t kDriveDistance = 2.0_m; +constexpr double kDriveSpeed = 0.5; +} // namespace AutoConstants diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h new file mode 100644 index 0000000000..4f733b7b49 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h @@ -0,0 +1,52 @@ +// 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/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 during Robot::RobotInit(). + * + *

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; + + // 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 new file mode 100644 index 0000000000..bab80f7039 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Robot.h @@ -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. + +#pragma once + +#include + +#include +#include + +#include "RapidReactCommandBot.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + 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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h new file mode 100644 index 0000000000..9a39a14ac9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -0,0 +1,58 @@ +// 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 +#include +#include +#include +#include +#include + +#include "Constants.h" + +class Drive : public frc2::SubsystemBase { + public: + Drive(); + /** + * Returns a command that drives the robot with arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + [[nodiscard]] frc2::CommandPtr ArcadeDriveCommand( + std::function fwd, std::function rot); + + /** + * Returns a command that drives the robot forward a specified distance at a + * specified speed. + * + * @param distance The distance to drive forward in meters + * @param speed The fraction of max speed at which to drive + */ + [[nodiscard]] frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, + double speed); + + private: + frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port}; + frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port}; + frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port}; + frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port}; + + frc::MotorControllerGroup m_leftMotors{m_leftLeader, m_leftFollower}; + frc::MotorControllerGroup m_rightMotors{m_rightLeader, m_rightFollower}; + + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0], + DriveConstants::kLeftEncoderPorts[1], + DriveConstants::kLeftEncoderReversed}; + frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0], + DriveConstants::kRightEncoderPorts[1], + DriveConstants::kRightEncoderReversed}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h new file mode 100644 index 0000000000..af8d39a70a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h @@ -0,0 +1,32 @@ +// 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 +#include +#include + +#include "Constants.h" + +class Intake : public frc2::SubsystemBase { + public: + Intake() = default; + + /** Returns a command that deploys the intake, and then runs the intake motor + * indefinitely. */ + [[nodiscard]] frc2::CommandPtr IntakeCommand(); + + /** Returns a command that turns off and retracts the intake. */ + [[nodiscard]] frc2::CommandPtr RetractCommand(); + + private: + frc::PWMSparkMax m_motor{IntakeConstants::kMotorPort}; + frc::DoubleSolenoid m_piston{frc::PneumaticsModuleType::REVPH, + IntakeConstants::kSolenoidPorts[0], + IntakeConstants::kSolenoidPorts[1]}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h new file mode 100644 index 0000000000..5ab2a63832 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h @@ -0,0 +1,44 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class Shooter : public frc2::SubsystemBase { + public: + Shooter(); + + /** + * Returns a command to shoot the balls currently stored in the robot. Spins + * the shooter flywheel up to the specified setpoint, and then runs the feeder + * motor. + * + * @param setpointRotationsPerSecond The desired shooter velocity + */ + [[nodiscard]] frc2::CommandPtr ShootCommand( + units::turns_per_second_t setpoint); + + private: + frc::PWMSparkMax m_shooterMotor{ShooterConstants::kShooterMotorPort}; + frc::PWMSparkMax m_feederMotor{ShooterConstants::kFeederMotorPort}; + + frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0], + ShooterConstants::kEncoderPorts[1], + ShooterConstants::kEncoderReversed}; + frc::SimpleMotorFeedforward m_shooterFeedforward{ + ShooterConstants::kS, ShooterConstants::kV}; + frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h new file mode 100644 index 0000000000..eab6da4917 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h @@ -0,0 +1,26 @@ +// 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 +#include + +#include "Constants.h" + +class Storage : frc2::SubsystemBase { + public: + Storage(); + /** Returns a command that runs the storage motor indefinitely. */ + [[nodiscard]] frc2::CommandPtr RunCommand(); + + /** Whether the ball storage is full. */ + bool IsFull() const; + + private: + frc::PWMSparkMax m_motor{StorageConstants::kMotorPort}; + frc::DigitalInput m_ballSensor{StorageConstants::kBallSensorPort}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 9fd88a178a..61f189de99 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -356,6 +356,18 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "Rapid React Command Bot", + "description": "A fully-functional command-based fender bot for the 2022 game using the new command framework.", + "tags": [ + "Complete robot", + "Command-based", + "Lambdas" + ], + "foldername": "RapidReactCommandBot", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "Select Command Example", "description": "An example showing how to use the SelectCommand class from the new command framework.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index e0e8755da8..9a03f82ecb 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -340,6 +340,19 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "Rapid React Command Bot", + "description": "A fully-functional command-based fender bot for the 2022 game using the new command framework.", + "tags": [ + "Complete robot", + "Command-based", + "Lambdas" + ], + "foldername": "rapidreactcommandbot", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Select Command Example", "description": "An example showing how to use the SelectCommand class from the new command framework.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java new file mode 100644 index 0000000000..6ee700c2f3 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java @@ -0,0 +1,83 @@ +// 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 edu.wpi.first.math.util.Units; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = {0, 1}; + public static final int[] kRightEncoderPorts = {2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = Units.inchesToMeters(6); + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + } + + public static final class ShooterConstants { + public static final int[] kEncoderPorts = {4, 5}; + public static final boolean kEncoderReversed = false; + public static final int kEncoderCPR = 1024; + public static final double kEncoderDistancePerPulse = + // Distance units will be rotations + 1.0 / (double) kEncoderCPR; + + public static final int kShooterMotorPort = 4; + public static final int kFeederMotorPort = 5; + + public static final double kShooterFreeRPS = 5300; + public static final double kShooterTargetRPS = 4000; + public static final double kShooterToleranceRPS = 50; + + // These are not real PID gains, and will have to be tuned for your specific robot. + public static final double kP = 1; + + // On a real robot the feedforward constants should be empirically determined; these are + // reasonable guesses. + public static final double kSVolts = 0.05; + public static final double kVVoltSecondsPerRotation = + // Should have value 12V at free speed... + 12.0 / kShooterFreeRPS; + + public static final double kFeederSpeed = 0.5; + } + + public static final class IntakeConstants { + public static final int kMotorPort = 6; + public static final int[] kSolenoidPorts = {0, 1}; + } + + public static final class StorageConstants { + public static final int kMotorPort = 7; + public static final int kBallSensorPort = 6; + } + + public static final class AutoConstants { + public static final double kTimeoutSeconds = 3; + public static final double kDriveDistanceMeters = 2; + public static final double kDriveSpeed = 0.5; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Main.java new file mode 100644 index 0000000000..b72cd916c8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Main.java @@ -0,0 +1,25 @@ +// 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 edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} 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 new file mode 100644 index 0000000000..6c5ff1fe2e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java @@ -0,0 +1,84 @@ +// 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.CommandGroupBase.parallel; + +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.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. + */ +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(); + + // 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 during {@link Robot#robotInit()}. + * + *

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. + new Trigger(m_storage::isFull).whileFalse(m_storage.runCommand()); + + // Automatically disable and retract the intake whenever the ball storage is full. + new Trigger(m_storage::isFull).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")); + } + + /** + * 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 new file mode 100644 index 0000000000..3588677ba4 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java @@ -0,0 +1,92 @@ +// 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 edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * 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. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private final RapidReactCommandBot m_robot = new RapidReactCommandBot(); + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Configure default commands and condition bindings on robot startup + m_robot.configureBindings(); + } + + /** + * 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(); + } + } + + /** 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/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java new file mode 100644 index 0000000000..736ff446da --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -0,0 +1,94 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; + +public class Drive extends SubsystemBase { + // The motors on the left side of the drive. + private final MotorControllerGroup m_leftMotors = + new MotorControllerGroup( + new PWMSparkMax(DriveConstants.kLeftMotor1Port), + new PWMSparkMax(DriveConstants.kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final MotorControllerGroup m_rightMotors = + new MotorControllerGroup( + new PWMSparkMax(DriveConstants.kRightMotor1Port), + new PWMSparkMax(DriveConstants.kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder( + DriveConstants.kLeftEncoderPorts[0], + DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder( + DriveConstants.kRightEncoderPorts[0], + DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); + + /** Creates a new Drive subsystem. */ + public Drive() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.setInverted(true); + + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + } + + /** + * Returns a command that drives the robot with arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble())) + .withName("arcadeDrive"); + } + + /** + * Returns a command that drives the robot forward a specified distance at a specified speed. + * + * @param distanceMeters The distance to drive forward in meters + * @param speed The fraction of max speed at which to drive + */ + public Command driveDistanceCommand(double distanceMeters, double speed) { + return runOnce( + () -> { + // Reset encoders at the start of the command + m_leftEncoder.reset(); + m_rightEncoder.reset(); + }) + // Drive forward at specified speed + .andThen(run(() -> m_drive.arcadeDrive(speed, 0))) + // End command when we've traveled the specified distance + .until( + () -> + Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) + >= distanceMeters) + // Stop the drive when the command ends + .finallyDo(interrupted -> m_drive.stopMotor()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Intake.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Intake.java new file mode 100644 index 0000000000..e091cd6822 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Intake.java @@ -0,0 +1,39 @@ +// 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.subsystems; + +import static edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.IntakeConstants; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + private final PWMSparkMax m_motor = new PWMSparkMax(IntakeConstants.kMotorPort); + private final DoubleSolenoid m_pistons = + new DoubleSolenoid( + PneumaticsModuleType.REVPH, + IntakeConstants.kSolenoidPorts[1], + IntakeConstants.kSolenoidPorts[2]); + + /** Returns a command that deploys the intake, and then runs the intake motor indefinitely. */ + public Command intakeCommand() { + return runOnce(() -> m_pistons.set(DoubleSolenoid.Value.kForward)) + .andThen(run(() -> m_motor.set(1.0))) + .withName("Intake"); + } + + /** Returns a command that turns off and retracts the intake. */ + public Command retractCommand() { + return runOnce( + () -> { + m_motor.disable(); + m_pistons.set(DoubleSolenoid.Value.kReverse); + }) + .withName("Retract"); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java new file mode 100644 index 0000000000..0e7f9b56ce --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -0,0 +1,66 @@ +// 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.subsystems; + +import static edu.wpi.first.wpilibj2.command.Commands.parallel; +import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort); + private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort); + private final Encoder m_shooterEncoder = + new Encoder( + ShooterConstants.kEncoderPorts[0], + ShooterConstants.kEncoderPorts[1], + ShooterConstants.kEncoderReversed); + private final SimpleMotorFeedforward m_shooterFeedforward = + new SimpleMotorFeedforward( + ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation); + private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0); + + /** The shooter subsystem for the robot. */ + public Shooter() { + m_shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS); + m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse); + + // Set default command to turn off both the shooter and feeder motors, and then idle + setDefaultCommand( + runOnce( + () -> { + m_shooterMotor.disable(); + m_feederMotor.disable(); + }) + .andThen(run(() -> {})) + .withName("Idle")); + } + + /** + * Returns a command to shoot the balls currently stored in the robot. Spins the shooter flywheel + * up to the specified setpoint, and then runs the feeder motor. + * + * @param setpointRotationsPerSecond The desired shooter velocity + */ + public Command shootCommand(double setpointRotationsPerSecond) { + return parallel( + // Run the shooter flywheel at the desired setpoint using feedforward and feedback + run( + () -> + m_shooterMotor.set( + m_shooterFeedforward.calculate(setpointRotationsPerSecond) + + m_shooterFeedback.calculate( + m_shooterEncoder.getRate(), setpointRotationsPerSecond))), + // Wait until the shooter has reached the setpoint, and then run the feeder + waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1))) + .withName("Shoot"); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java new file mode 100644 index 0000000000..dac61a43fc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java @@ -0,0 +1,33 @@ +// 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.subsystems; + +import static edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.StorageConstants; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Storage extends SubsystemBase { + private final PWMSparkMax m_motor = new PWMSparkMax(StorageConstants.kMotorPort); + private final DigitalInput m_ballSensor = new DigitalInput(StorageConstants.kBallSensorPort); + + /** Create a new Storage subsystem. */ + public Storage() { + // Set default command to turn off the storage motor and then idle + setDefaultCommand(runOnce(m_motor::disable).andThen(run(() -> {})).withName("Idle")); + } + + /** Whether the ball storage is full. */ + public boolean isFull() { + return m_ballSensor.get(); + } + + /** Returns a command that runs the storage motor indefinitely. */ + public Command runCommand() { + return run(() -> m_motor.set(1)).withName("run"); + } +}