diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp new file mode 100644 index 0000000000..9003338207 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/Command.h" + +#include + +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/PerpetualCommand.h" +#include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; + +Command::~Command() { CommandScheduler::GetInstance().Cancel(this); } + +void Command::Initialize() {} +void Command::Execute() {} +void Command::End(bool interrupted) {} + +ParallelRaceGroup Command::WithTimeout(double seconds) && { + std::vector> temp; + temp.emplace_back(std::make_unique(seconds)); + temp.emplace_back(std::move(*this).TransferOwnership()); + return ParallelRaceGroup(std::move(temp)); +} + +ParallelRaceGroup Command::InterruptOn(std::function condition) && { + std::vector> temp; + temp.emplace_back(std::make_unique(std::move(condition))); + temp.emplace_back(std::move(*this).TransferOwnership()); + return ParallelRaceGroup(std::move(temp)); +} + +SequentialCommandGroup Command::BeforeStarting(std::function toRun) && { + std::vector> temp; + temp.emplace_back(std::make_unique( + std::move(toRun), std::initializer_list{})); + temp.emplace_back(std::move(*this).TransferOwnership()); + return SequentialCommandGroup(std::move(temp)); +} + +SequentialCommandGroup Command::WhenFinished(std::function toRun) && { + std::vector> temp; + temp.emplace_back(std::move(*this).TransferOwnership()); + temp.emplace_back(std::make_unique( + std::move(toRun), std::initializer_list{})); + return SequentialCommandGroup(std::move(temp)); +} + +PerpetualCommand Command::Perpetually() && { + return PerpetualCommand(std::move(*this).TransferOwnership()); +} + +ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); } + +void Command::Schedule(bool interruptible) { + CommandScheduler::GetInstance().Schedule(interruptible, this); +} + +void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); } + +bool Command::IsScheduled() const { + return CommandScheduler::GetInstance().IsScheduled(this); +} + +bool Command::HasRequirement(Subsystem* requirement) const { + bool hasRequirement = false; + for (auto&& subsystem : GetRequirements()) { + hasRequirement |= requirement == subsystem; + } + return hasRequirement; +} + +std::string Command::GetName() const { return GetTypeName(*this); } + +bool Command::IsGrouped() const { return m_isGrouped; } + +void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; } + +namespace frc2 { +bool RequirementsDisjoint(Command* first, Command* second) { + bool disjoint = true; + auto&& requirements = second->GetRequirements(); + for (auto&& requirement : first->GetRequirements()) { + disjoint &= requirements.find(requirement) == requirements.end(); + } + return disjoint; +} +} // namespace frc2 diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp new file mode 100644 index 0000000000..c3cf024ca9 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/CommandBase.h" + +#include +#include +#include + +using namespace frc2; + +CommandBase::CommandBase() { + m_name = Command::GetName(); + m_subsystem = "Unknown"; +} + +CommandBase::CommandBase(const CommandBase& other) : Sendable{}, Command{} { + m_name = other.m_name; + m_subsystem = other.m_subsystem; + m_requirements = other.m_requirements; +} + +void CommandBase::AddRequirements( + std::initializer_list requirements) { + m_requirements.insert(requirements.begin(), requirements.end()); +} + +void CommandBase::AddRequirements(wpi::SmallSet requirements) { + m_requirements.insert(requirements.begin(), requirements.end()); +} + +wpi::SmallSet CommandBase::GetRequirements() const { + return m_requirements; +} + +void CommandBase::SetName(const wpi::Twine& name) { m_name = name.str(); } + +std::string CommandBase::GetName() const { return m_name; } + +std::string CommandBase::GetSubsystem() const { return m_subsystem; } + +void CommandBase::SetSubsystem(const wpi::Twine& subsystem) { + m_subsystem = subsystem.str(); +} + +void CommandBase::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Command"); + builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr); + builder.AddBooleanProperty("running", [this] { return IsScheduled(); }, + [this](bool value) { + bool isScheduled = IsScheduled(); + if (value && !isScheduled) { + Schedule(); + } else if (!value && isScheduled) { + Cancel(); + } + }); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp new file mode 100644 index 0000000000..ba53397039 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/CommandGroupBase.h" + +#include + +#include "frc/WPIErrors.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +template +static bool ContainsKey(const TMap& map, TKey keyToCheck) { + return map.find(keyToCheck) != map.end(); +} +bool CommandGroupBase::RequireUngrouped(Command& command) { + if (command.IsGrouped()) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + return false; + } else { + return true; + } +} + +bool CommandGroupBase::RequireUngrouped( + wpi::ArrayRef> commands) { + bool allUngrouped = true; + for (auto&& command : commands) { + allUngrouped &= !command.get()->IsGrouped(); + } + if (!allUngrouped) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + } + return allUngrouped; +} + +bool CommandGroupBase::RequireUngrouped( + std::initializer_list commands) { + bool allUngrouped = true; + for (auto&& command : commands) { + allUngrouped &= !command->IsGrouped(); + } + if (!allUngrouped) { + wpi_setGlobalWPIErrorWithContext( + CommandIllegalUse, + "Commands cannot be added to more than one CommandGroup"); + } + return allUngrouped; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp new file mode 100644 index 0000000000..f1423d18fa --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -0,0 +1,320 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/CommandScheduler.h" + +#include +#include +#include +#include +#include +#include + +#include + +using namespace frc2; +template +static bool ContainsKey(const TMap& map, TKey keyToCheck) { + return map.find(keyToCheck) != map.end(); +} + +CommandScheduler::CommandScheduler() { SetName("Scheduler"); } + +CommandScheduler& CommandScheduler::GetInstance() { + static CommandScheduler scheduler; + return scheduler; +} + +void CommandScheduler::AddButton(wpi::unique_function button) { + m_buttons.emplace_back(std::move(button)); +} + +void CommandScheduler::ClearButtons() { m_buttons.clear(); } + +void CommandScheduler::Schedule(bool interruptible, Command* command) { + if (command->IsGrouped()) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "A command that is part of a command group " + "cannot be independently scheduled"); + return; + } + if (m_disabled || + (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) || + ContainsKey(m_scheduledCommands, command)) { + return; + } + + const auto& requirements = command->GetRequirements(); + + wpi::SmallVector intersection; + + bool isDisjoint = true; + bool allInterruptible = true; + for (auto&& i1 : m_requirements) { + if (requirements.find(i1.first) != requirements.end()) { + isDisjoint = false; + allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible(); + intersection.emplace_back(i1.second); + } + } + + if (isDisjoint || allInterruptible) { + if (allInterruptible) { + for (auto&& cmdToCancel : intersection) { + Cancel(cmdToCancel); + } + } + command->Initialize(); + m_scheduledCommands[command] = CommandState{interruptible}; + for (auto&& action : m_initActions) { + action(*command); + } + for (auto&& requirement : requirements) { + m_requirements[requirement] = command; + } + } +} + +void CommandScheduler::Schedule(Command* command) { Schedule(true, command); } + +void CommandScheduler::Schedule(bool interruptible, + wpi::ArrayRef commands) { + for (auto command : commands) { + Schedule(interruptible, command); + } +} + +void CommandScheduler::Schedule(bool interruptible, + std::initializer_list commands) { + for (auto command : commands) { + Schedule(interruptible, command); + } +} + +void CommandScheduler::Schedule(wpi::ArrayRef commands) { + for (auto command : commands) { + Schedule(true, command); + } +} + +void CommandScheduler::Schedule(std::initializer_list commands) { + for (auto command : commands) { + Schedule(true, command); + } +} + +void CommandScheduler::Run() { + if (m_disabled) { + return; + } + + // Run the periodic method of all registered subsystems. + for (auto&& subsystem : m_subsystems) { + subsystem.getFirst()->Periodic(); + } + + // Poll buttons for new commands to add. + for (auto&& button : m_buttons) { + button(); + } + + // Run scheduled commands, remove finished commands. + for (auto iterator = m_scheduledCommands.begin(); + iterator != m_scheduledCommands.end(); iterator++) { + Command* command = iterator->getFirst(); + + if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) { + Cancel(command); + continue; + } + + command->Execute(); + for (auto&& action : m_executeActions) { + action(*command); + } + + if (command->IsFinished()) { + command->End(false); + for (auto&& action : m_finishActions) { + action(*command); + } + + for (auto&& requirement : command->GetRequirements()) { + m_requirements.erase(requirement); + } + + m_scheduledCommands.erase(iterator); + } + } + + // Add default commands for un-required registered subsystems. + for (auto&& subsystem : m_subsystems) { + auto s = m_requirements.find(subsystem.getFirst()); + if (s == m_requirements.end()) { + Schedule({subsystem.getSecond().get()}); + } + } +} + +void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) { + m_subsystems[subsystem] = nullptr; +} + +void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) { + auto s = m_subsystems.find(subsystem); + if (s != m_subsystems.end()) { + m_subsystems.erase(s); + } +} + +void CommandScheduler::RegisterSubsystem( + std::initializer_list subsystems) { + for (auto* subsystem : subsystems) { + RegisterSubsystem(subsystem); + } +} + +void CommandScheduler::UnregisterSubsystem( + std::initializer_list subsystems) { + for (auto* subsystem : subsystems) { + UnregisterSubsystem(subsystem); + } +} + +Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { + auto&& find = m_subsystems.find(subsystem); + if (find != m_subsystems.end()) { + return find->second.get(); + } else { + return nullptr; + } +} + +void CommandScheduler::Cancel(Command* command) { + auto find = m_scheduledCommands.find(command); + if (find == m_scheduledCommands.end()) return; + command->End(true); + for (auto&& action : m_interruptActions) { + action(*command); + } + m_scheduledCommands.erase(find); + for (auto&& requirement : m_requirements) { + if (requirement.second == command) { + m_requirements.erase(requirement.first); + } + } +} + +void CommandScheduler::Cancel(wpi::ArrayRef commands) { + for (auto command : commands) { + Cancel(command); + } +} + +void CommandScheduler::Cancel(std::initializer_list commands) { + for (auto command : commands) { + Cancel(command); + } +} + +void CommandScheduler::CancelAll() { + for (auto&& command : m_scheduledCommands) { + Cancel(command.first); + } +} + +double CommandScheduler::TimeSinceScheduled(const Command* command) const { + auto find = m_scheduledCommands.find(command); + if (find != m_scheduledCommands.end()) { + return find->second.TimeSinceInitialized(); + } else { + return -1; + } +} +bool CommandScheduler::IsScheduled( + wpi::ArrayRef commands) const { + for (auto command : commands) { + if (!IsScheduled(command)) { + return false; + } + } + return true; +} + +bool CommandScheduler::IsScheduled( + std::initializer_list commands) const { + for (auto command : commands) { + if (!IsScheduled(command)) { + return false; + } + } + return true; +} + +bool CommandScheduler::IsScheduled(const Command* command) const { + return m_scheduledCommands.find(command) != m_scheduledCommands.end(); +} + +Command* CommandScheduler::Requiring(const Subsystem* subsystem) const { + auto find = m_requirements.find(subsystem); + if (find != m_requirements.end()) { + return find->second; + } else { + return nullptr; + } +} + +void CommandScheduler::Disable() { m_disabled = true; } + +void CommandScheduler::Enable() { m_disabled = false; } + +void CommandScheduler::OnCommandInitialize(Action action) { + m_initActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandExecute(Action action) { + m_executeActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandInterrupt(Action action) { + m_interruptActions.emplace_back(std::move(action)); +} + +void CommandScheduler::OnCommandFinish(Action action) { + m_finishActions.emplace_back(std::move(action)); +} + +void CommandScheduler::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Scheduler"); + m_namesEntry = builder.GetEntry("Names"); + m_idsEntry = builder.GetEntry("Ids"); + m_cancelEntry = builder.GetEntry("Cancel"); + + builder.SetUpdateTable([this] { + double tmp[1]; + tmp[0] = 0; + auto toCancel = m_cancelEntry.GetDoubleArray(tmp); + for (auto cancel : toCancel) { + uintptr_t ptrTmp = static_cast(cancel); + Command* command = reinterpret_cast(ptrTmp); + if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) { + Cancel(command); + } + m_cancelEntry.SetDoubleArray(wpi::ArrayRef{}); + } + + wpi::SmallVector names; + wpi::SmallVector ids; + for (auto&& command : m_scheduledCommands) { + names.emplace_back(command.first->GetName()); + uintptr_t ptrTmp = reinterpret_cast(command.first); + ids.emplace_back(static_cast(ptrTmp)); + } + m_namesEntry.SetStringArray(names); + m_idsEntry.SetDoubleArray(ids); + }); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp new file mode 100644 index 0000000000..78ae006e64 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/CommandState.h" + +#include "frc/Timer.h" + +using namespace frc2; +CommandState::CommandState(bool interruptible) + : m_interruptible{interruptible} { + StartTiming(); + StartRunning(); +} + +void CommandState::StartTiming() { + m_startTime = frc::Timer::GetFPGATimestamp(); +} +void CommandState::StartRunning() { m_startTime = -1; } +double CommandState::TimeSinceInitialized() const { + return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp new file mode 100644 index 0000000000..23445135e6 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ConditionalCommand.h" + +using namespace frc2; + +ConditionalCommand::ConditionalCommand(std::unique_ptr&& onTrue, + std::unique_ptr&& onFalse, + std::function condition) + : m_condition{std::move(condition)} { + if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) { + return; + } + + m_onTrue = std::move(onTrue); + m_onFalse = std::move(onFalse); + + m_onTrue->SetGrouped(true); + m_onFalse->SetGrouped(true); + + m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled(); + m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled(); + + AddRequirements(m_onTrue->GetRequirements()); + AddRequirements(m_onFalse->GetRequirements()); +} + +void ConditionalCommand::Initialize() { + if (m_condition()) { + m_selectedCommand = m_onTrue.get(); + } else { + m_selectedCommand = m_onFalse.get(); + } + m_selectedCommand->Initialize(); +} + +void ConditionalCommand::Execute() { m_selectedCommand->Execute(); } + +void ConditionalCommand::End(bool interrupted) { + m_selectedCommand->End(interrupted); +} + +bool ConditionalCommand::IsFinished() { + return m_selectedCommand->IsFinished(); +} + +bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp new file mode 100644 index 0000000000..63c3179633 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/FunctionalCommand.h" + +using namespace frc2; + +FunctionalCommand::FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished) + : m_onInit{std::move(onInit)}, + m_onExecute{std::move(onExecute)}, + m_onEnd{std::move(onEnd)}, + m_isFinished{std::move(isFinished)} {} + +void FunctionalCommand::Initialize() { m_onInit(); } + +void FunctionalCommand::Execute() { m_onExecute(); } + +void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); } + +bool FunctionalCommand::IsFinished() { return m_isFinished(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp new file mode 100644 index 0000000000..b199074780 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/InstantCommand.h" + +using namespace frc2; + +InstantCommand::InstantCommand(std::function toRun, + std::initializer_list requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + +InstantCommand::InstantCommand() : m_toRun{[] {}} {} + +void InstantCommand::Initialize() { m_toRun(); } + +bool InstantCommand::IsFinished() { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp new file mode 100644 index 0000000000..086a2a1d0e --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/NotifierCommand.h" + +using namespace frc2; + +NotifierCommand::NotifierCommand(std::function toRun, double period, + std::initializer_list requirements) + : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { + AddRequirements(requirements); +} + +NotifierCommand::NotifierCommand(NotifierCommand&& other) + : CommandHelper(std::move(other)), + m_toRun(other.m_toRun), + m_notifier(other.m_toRun), + m_period(other.m_period) {} + +NotifierCommand::NotifierCommand(const NotifierCommand& other) + : CommandHelper(other), + m_toRun(other.m_toRun), + m_notifier(frc::Notifier(other.m_toRun)), + m_period(other.m_period) {} + +void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); } + +void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp new file mode 100644 index 0000000000..03be847d6f --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/PIDCommand.h" + +using namespace frc2; + +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + std::initializer_list requirements) + : m_controller{controller}, + m_measurement{std::move(measurementSource)}, + m_setpoint{std::move(setpointSource)}, + m_useOutput{std::move(useOutput)} { + AddRequirements(requirements); +} + +PIDCommand::PIDCommand(PIDController controller, + std::function measurementSource, + double setpoint, std::function useOutput, + std::initializer_list requirements) + : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; }, + useOutput, requirements) {} + +void PIDCommand::Initialize() { m_controller.Reset(); } + +void PIDCommand::Execute() { + m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint())); +} + +void PIDCommand::End(bool interrupted) { m_useOutput(0); } + +void PIDCommand::SetOutput(std::function useOutput) { + m_useOutput = useOutput; +} + +void PIDCommand::SetSetpoint(std::function setpointSource) { + m_setpoint = setpointSource; +} + +void PIDCommand::SetSetpoint(double setpoint) { + m_setpoint = [setpoint] { return setpoint; }; +} + +void PIDCommand::SetSetpointRelative(double relativeSetpoint) { + SetSetpoint(m_setpoint() + relativeSetpoint); +} + +PIDController& PIDCommand::getController() { return m_controller; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp new file mode 100644 index 0000000000..b81f6f6d96 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/PIDSubsystem.h" + +using namespace frc2; + +PIDSubsystem::PIDSubsystem(PIDController controller) + : m_controller{controller} {} + +void PIDSubsystem::Periodic() { + if (m_enabled) { + UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint())); + } +} + +void PIDSubsystem::Enable() { + m_controller.Reset(); + m_enabled = true; +} + +void PIDSubsystem::Disable() { + UseOutput(0); + m_enabled = false; +} + +PIDController& PIDSubsystem::GetController() { return m_controller; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp new file mode 100644 index 0000000000..d8a4159d23 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ParallelCommandGroup.h" + +using namespace frc2; + +ParallelCommandGroup::ParallelCommandGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void ParallelCommandGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning.first->Initialize(); + commandRunning.second = true; + } + isRunning = true; +} + +void ParallelCommandGroup::Execute() { + for (auto& commandRunning : m_commands) { + if (!commandRunning.second) continue; + commandRunning.first->Execute(); + if (commandRunning.first->IsFinished()) { + commandRunning.first->End(false); + commandRunning.second = false; + } + } +} + +void ParallelCommandGroup::End(bool interrupted) { + if (interrupted) { + for (auto& commandRunning : m_commands) { + if (commandRunning.second) { + commandRunning.first->End(true); + } + } + } + isRunning = false; +} + +bool ParallelCommandGroup::IsFinished() { + for (auto& command : m_commands) { + if (command.second) return false; + } + return true; +} + +bool ParallelCommandGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void ParallelCommandGroup::AddCommands( + std::vector>&& commands) { + for (auto&& command : commands) { + if (!RequireUngrouped(*command)) return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands[std::move(command)] = false; + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp new file mode 100644 index 0000000000..d967e67b48 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ParallelDeadlineGroup.h" + +using namespace frc2; + +ParallelDeadlineGroup::ParallelDeadlineGroup( + std::unique_ptr&& deadline, + std::vector>&& commands) { + SetDeadline(std::move(deadline)); + AddCommands(std::move(commands)); +} + +void ParallelDeadlineGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning.first->Initialize(); + commandRunning.second = true; + } + isRunning = true; +} + +void ParallelDeadlineGroup::Execute() { + for (auto& commandRunning : m_commands) { + if (!commandRunning.second) continue; + commandRunning.first->Execute(); + if (commandRunning.first->IsFinished()) { + commandRunning.first->End(false); + commandRunning.second = false; + } + } +} + +void ParallelDeadlineGroup::End(bool interrupted) { + for (auto& commandRunning : m_commands) { + if (commandRunning.second) { + commandRunning.first->End(true); + } + } + isRunning = false; +} + +bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); } + +bool ParallelDeadlineGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void ParallelDeadlineGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands[std::move(command)] = false; + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} + +void ParallelDeadlineGroup::SetDeadline(std::unique_ptr&& deadline) { + m_deadline = deadline.get(); + m_deadline->SetGrouped(true); + m_commands[std::move(deadline)] = false; + AddRequirements(m_deadline->GetRequirements()); + m_runWhenDisabled &= m_deadline->RunsWhenDisabled(); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp new file mode 100644 index 0000000000..4f9881b7c5 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ParallelRaceGroup.h" + +using namespace frc2; + +ParallelRaceGroup::ParallelRaceGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void ParallelRaceGroup::Initialize() { + for (auto& commandRunning : m_commands) { + commandRunning->Initialize(); + } + isRunning = true; +} + +void ParallelRaceGroup::Execute() { + for (auto& commandRunning : m_commands) { + commandRunning->Execute(); + if (commandRunning->IsFinished()) { + m_finished = true; + commandRunning->End(false); + } + } +} + +void ParallelRaceGroup::End(bool interrupted) { + for (auto& commandRunning : m_commands) { + if (!commandRunning->IsFinished()) { + commandRunning->End(true); + } + } + isRunning = false; +} + +bool ParallelRaceGroup::IsFinished() { return m_finished; } + +bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; } + +void ParallelRaceGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (isRunning) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + if (RequirementsDisjoint(this, command.get())) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands.emplace(std::move(command)); + } else { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Multiple commands in a parallel group cannot " + "require the same subsystems"); + return; + } + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp new file mode 100644 index 0000000000..f29850bbaf --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/PerpetualCommand.h" + +using namespace frc2; + +PerpetualCommand::PerpetualCommand(std::unique_ptr&& command) { + if (!CommandGroupBase::RequireUngrouped(command)) { + return; + } + m_command = std::move(command); + m_command->SetGrouped(true); + AddRequirements(m_command->GetRequirements()); +} + +void PerpetualCommand::Initialize() { m_command->Initialize(); } + +void PerpetualCommand::Execute() { m_command->Execute(); } + +void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); } diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp similarity index 56% rename from wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h rename to wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp index ce25e76f30..8bd62ea0ff 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h +++ b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp @@ -1,20 +1,16 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ -#pragma once +#include "frc2/command/PrintCommand.h" -#include +using namespace frc2; -class MyAutoCommand : public frc::Command { - public: - MyAutoCommand(); - void Initialize() override; - void Execute() override; - bool IsFinished() override; - void End() override; - void Interrupted() override; -}; +PrintCommand::PrintCommand(const wpi::Twine& message) + : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} { +} + +bool PrintCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp new file mode 100644 index 0000000000..6f96315365 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ProxyScheduleCommand.h" + +using namespace frc2; + +ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef toSchedule) { + SetInsert(m_toSchedule, toSchedule); +} + +void ProxyScheduleCommand::Initialize() { + for (auto* command : m_toSchedule) { + command->Schedule(); + } +} + +void ProxyScheduleCommand::End(bool interrupted) { + if (interrupted) { + for (auto* command : m_toSchedule) { + command->Cancel(); + } + } +} + +void ProxyScheduleCommand::Execute() { + m_finished = true; + for (auto* command : m_toSchedule) { + m_finished &= !command->IsScheduled(); + } +} + +bool ProxyScheduleCommand::IsFinished() { return m_finished; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp new file mode 100644 index 0000000000..5c2c75534d --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/RunCommand.h" + +using namespace frc2; + +RunCommand::RunCommand(std::function toRun, + std::initializer_list requirements) + : m_toRun{std::move(toRun)} { + AddRequirements(requirements); +} + +void RunCommand::Execute() { m_toRun(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp new file mode 100644 index 0000000000..ea1ea8d6d2 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ScheduleCommand.h" + +using namespace frc2; + +ScheduleCommand::ScheduleCommand(wpi::ArrayRef toSchedule) { + SetInsert(m_toSchedule, toSchedule); +} + +void ScheduleCommand::Initialize() { + for (auto command : m_toSchedule) { + command->Schedule(); + } +} + +bool ScheduleCommand::IsFinished() { return true; } + +bool ScheduleCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp new file mode 100644 index 0000000000..9d51609a4a --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; + +SequentialCommandGroup::SequentialCommandGroup( + std::vector>&& commands) { + AddCommands(std::move(commands)); +} + +void SequentialCommandGroup::Initialize() { + m_currentCommandIndex = 0; + + if (!m_commands.empty()) { + m_commands[0]->Initialize(); + } +} + +void SequentialCommandGroup::Execute() { + if (m_commands.empty()) return; + + auto& currentCommand = m_commands[m_currentCommandIndex]; + + currentCommand->Execute(); + if (currentCommand->IsFinished()) { + currentCommand->End(false); + m_currentCommandIndex++; + if (m_currentCommandIndex < m_commands.size()) { + m_commands[m_currentCommandIndex]->Initialize(); + } + } +} + +void SequentialCommandGroup::End(bool interrupted) { + if (interrupted && !m_commands.empty()) { + m_commands[m_currentCommandIndex]->End(interrupted); + } + m_currentCommandIndex = invalid_index; +} + +bool SequentialCommandGroup::IsFinished() { + return m_currentCommandIndex == m_commands.size(); +} + +bool SequentialCommandGroup::RunsWhenDisabled() const { + return m_runWhenDisabled; +} + +void SequentialCommandGroup::AddCommands( + std::vector>&& commands) { + if (!RequireUngrouped(commands)) { + return; + } + + if (m_currentCommandIndex != invalid_index) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Commands cannot be added to a CommandGroup " + "while the group is running"); + } + + for (auto&& command : commands) { + command->SetGrouped(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + m_commands.emplace_back(std::move(command)); + } +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp new file mode 100644 index 0000000000..33407bf718 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/StartEndCommand.h" + +using namespace frc2; + +StartEndCommand::StartEndCommand(std::function onInit, + std::function onEnd, + std::initializer_list requirements) + : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} { + AddRequirements(requirements); +} + +StartEndCommand::StartEndCommand(const StartEndCommand& other) + : CommandHelper(other) { + m_onInit = other.m_onInit; + m_onEnd = other.m_onEnd; +} + +void StartEndCommand::Initialize() { m_onInit(); } + +void StartEndCommand::End(bool interrupted) { m_onEnd(); } diff --git a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp new file mode 100644 index 0000000000..cccb55bc17 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/Subsystem.h" + +using namespace frc2; +Subsystem::~Subsystem() { + CommandScheduler::GetInstance().UnregisterSubsystem(this); +} + +void Subsystem::Periodic() {} + +Command* Subsystem::GetDefaultCommand() const { + return CommandScheduler::GetInstance().GetDefaultCommand(this); +} + +Command* Subsystem::GetCurrentCommand() const { + return CommandScheduler::GetInstance().Requiring(this); +} + +void Subsystem::Register() { + return CommandScheduler::GetInstance().RegisterSubsystem(this); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp new file mode 100644 index 0000000000..62080435dd --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/SubsystemBase.h" + +#include +#include +#include +#include + +using namespace frc2; + +SubsystemBase::SubsystemBase() { + m_name = GetTypeName(*this); + CommandScheduler::GetInstance().RegisterSubsystem({this}); +} + +void SubsystemBase::InitSendable(frc::SendableBuilder& builder) { + builder.SetSmartDashboardType("Subsystem"); + builder.AddBooleanProperty(".hasDefault", + [this] { return GetDefaultCommand() != nullptr; }, + nullptr); + builder.AddStringProperty(".default", + [this]() -> std::string { + auto command = GetDefaultCommand(); + if (command == nullptr) return "none"; + return command->GetName(); + }, + nullptr); + builder.AddBooleanProperty(".hasCommand", + [this] { return GetCurrentCommand() != nullptr; }, + nullptr); + builder.AddStringProperty(".command", + [this]() -> std::string { + auto command = GetCurrentCommand(); + if (command == nullptr) return "none"; + return command->GetName(); + }, + nullptr); +} + +std::string SubsystemBase::GetName() const { return m_name; } + +void SubsystemBase::SetName(const wpi::Twine& name) { m_name = name.str(); } + +std::string SubsystemBase::GetSubsystem() const { return GetName(); } + +void SubsystemBase::SetSubsystem(const wpi::Twine& name) { SetName(name); } + +void SubsystemBase::AddChild(std::string name, frc::Sendable* child) { + child->SetName(name); + frc::LiveWindow::GetInstance()->Add(child); +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp new file mode 100644 index 0000000000..fa4472018b --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/WaitCommand.h" + +using namespace frc2; + +WaitCommand::WaitCommand(double seconds) : m_duration{seconds} { + auto secondsStr = std::to_string(seconds); + SetName(wpi::Twine(m_name) + ": " + wpi::Twine(secondsStr) + " seconds"); + m_timer = std::make_unique(); +} + +void WaitCommand::Initialize() { + m_timer->Reset(); + m_timer->Start(); +} + +void WaitCommand::End(bool interrupted) { m_timer->Stop(); } + +bool WaitCommand::IsFinished() { return m_timer->HasPeriodPassed(m_duration); } + +bool WaitCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp new file mode 100644 index 0000000000..d7a0daf374 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; + +WaitUntilCommand::WaitUntilCommand(std::function condition) + : m_condition{std::move(condition)} {} + +WaitUntilCommand::WaitUntilCommand(double time) + : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {} + +bool WaitUntilCommand::IsFinished() { return m_condition(); } + +bool WaitUntilCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp new file mode 100644 index 0000000000..e519a9fdaf --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/button/Button.h" + +using namespace frc2; + +Button::Button(std::function isPressed) : Trigger(isPressed) {} + +Button Button::WhenPressed(Command* command, bool interruptible) { + WhenActive(command, interruptible); + return *this; +} + +Button Button::WhenPressed(std::function toRun) { + WhenActive(std::move(toRun)); + return *this; +} + +Button Button::WhileHeld(Command* command, bool interruptible) { + WhileActiveContinous(command, interruptible); + return *this; +} + +Button Button::WhileHeld(std::function toRun) { + WhileActiveContinous(std::move(toRun)); + return *this; +} + +Button Button::WhenHeld(Command* command, bool interruptible) { + WhileActiveOnce(command, interruptible); + return *this; +} + +Button Button::WhenReleased(Command* command, bool interruptible) { + WhenInactive(command, interruptible); + return *this; +} + +Button Button::WhenReleased(std::function toRun) { + WhenInactive(std::move(toRun)); + return *this; +} + +Button Button::ToggleWhenPressed(Command* command, bool interruptible) { + ToggleWhenActive(command, interruptible); + return *this; +} + +Button Button::CancelWhenPressed(Command* command) { + CancelWhenActive(command); + return *this; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp new file mode 100644 index 0000000000..304bf98a49 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/button/Trigger.h" + +#include + +using namespace frc2; + +Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {} + +Trigger Trigger::WhenActive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + + return *this; +} + +Trigger Trigger::WhenActive(std::function toRun) { + return WhenActive(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhileActiveContinous(std::function toRun) { + return WhileActiveContinous(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhenInactive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (pressedLast && !pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::WhenInactive(std::function toRun) { + return WhenInactive(InstantCommand(std::move(toRun), {})); +} + +Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command, interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + if (command->IsScheduled()) { + command->Cancel(); + } else { + command->Schedule(interruptible); + } + } + + pressedLast = pressed; + }); + return *this; +} + +Trigger Trigger::CancelWhenActive(Command* command) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, command]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; +} diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h index 383c169ac6..761b0bbcfe 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -20,6 +20,8 @@ class Sendable { Sendable() = default; virtual ~Sendable() = default; + Sendable(const Sendable&) = default; + Sendable& operator=(const Sendable&) = default; Sendable(Sendable&&) = default; Sendable& operator=(Sendable&&) = default; diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibc/src/main/native/include/frc2/command/Command.h new file mode 100644 index 0000000000..c9ecb45a01 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/Command.h @@ -0,0 +1,237 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace frc2 { + +template +std::string GetTypeName(const T& type) { + return wpi::Demangle(typeid(type).name()); +} + +class ParallelCommandGroup; +class ParallelRaceGroup; +class ParallelDeadlineGroup; +class SequentialCommandGroup; +class PerpetualCommand; +class ProxyScheduleCommand; + +/** + * A state machine representing a complete action to be performed by the robot. + * Commands are run by the CommandScheduler, and can be composed into + * CommandGroups to allow users to build complicated multi-step actions without + * the need to roll the state machine logic themselves. + * + *

Commands are run synchronously from the main robot loop; no multithreading + * is used, unless specified explicitly from the command implementation. + * + *

Note: ALWAYS create a subclass by extending CommandHelper, + * or decorators will not function! + * + * @see CommandScheduler + * @see CommandHelper + */ +class Command : public frc::ErrorBase { + public: + Command() = default; + Command(Command&& other) = default; + virtual ~Command(); + + /** + * The initial subroutine of a command. Called once when the command is + * initially scheduled. + */ + virtual void Initialize(); + + /** + * The main body of a command. Called repeatedly while the command is + * scheduled. + */ + virtual void Execute(); + + /** + * The action to take when the command ends. Called when either the command + * finishes normally, or when it interrupted/canceled. + * + * @param interrupted whether the command was interrupted/canceled + */ + virtual void End(bool interrupted); + + /** + * Whether the command has finished. Once a command finishes, the scheduler + * will call its end() method and un-schedule it. + * + * @return whether the command has finished. + */ + virtual bool IsFinished() { return false; } + + /** + * Specifies the set of subsystems used by this command. Two commands cannot + * use the same subsystem at the same time. If the command is scheduled as + * interruptible and another command is scheduled that shares a requirement, + * the command will be interrupted. Else, the command will not be scheduled. + * If no subsystems are required, return an empty set. + * + *

Note: it is recommended that user implementations contain the + * requirements as a field, and return that field here, rather than allocating + * a new set every time this is called. + * + * @return the set of subsystems that are required + */ + virtual wpi::SmallSet GetRequirements() const = 0; + + /** + * Decorates this command with a timeout. If the specified timeout is + * exceeded before the command finishes normally, the command will be + * interrupted and un-scheduled. Note that the timeout only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param seconds the timeout duration + * @return the command with the timeout added + */ + ParallelRaceGroup WithTimeout(double seconds) &&; + + /** + * Decorates this command with an interrupt condition. If the specified + * condition becomes true before the command finishes normally, the command + * will be interrupted and un-scheduled. Note that this only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param condition the interrupt condition + * @return the command with the interrupt condition added + */ + ParallelRaceGroup InterruptOn(std::function condition) &&; + + /** + * Decorates this command with a runnable to run before this command starts. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + SequentialCommandGroup BeforeStarting(std::function toRun) &&; + + /** + * Decorates this command with a runnable to run after the command finishes. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + SequentialCommandGroup WhenFinished(std::function toRun) &&; + + /** + * Decorates this command to run perpetually, ignoring its ordinary end + * conditions. The decorated command can still be interrupted or canceled. + * + * @return the decorated command + */ + PerpetualCommand Perpetually() &&; + + /** + * Decorates this command to run "by proxy" by wrapping it in a {@link + * ProxyScheduleCommand}. This is useful for "forking off" from command groups + * when the user does not wish to extend the command's requirements to the + * entire command group. + * + * @return the decorated command + */ + ProxyScheduleCommand AsProxy(); + + /** + * Schedules this command. + * + * @param interruptible whether this command can be interrupted by another + * command that shares one of its requirements + */ + void Schedule(bool interruptible); + + /** + * Schedules this command, defaulting to interruptible. + */ + void Schedule() { Schedule(true); } + + /** + * Cancels this command. Will call the command's interrupted() method. + * Commands will be canceled even if they are not marked as interruptible. + */ + void Cancel(); + + /** + * Whether or not the command is currently scheduled. Note that this does not + * detect whether the command is being run by a CommandGroup, only whether it + * is directly being run by the scheduler. + * + * @return Whether the command is scheduled. + */ + bool IsScheduled() const; + + /** + * Whether the command requires a given subsystem. Named "hasRequirement" + * rather than "requires" to avoid confusion with + * {@link + * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)} + * - this may be able to be changed in a few years. + * + * @param requirement the subsystem to inquire about + * @return whether the subsystem is required + */ + bool HasRequirement(Subsystem* requirement) const; + + /** + * Whether the command is currently grouped in a command group. Used as extra + * insurance to prevent accidental independent use of grouped commands. + */ + bool IsGrouped() const; + + /** + * Sets whether the command is currently grouped in a command group. Can be + * used to "reclaim" a command if a group is no longer going to use it. NOT + * ADVISED! + */ + void SetGrouped(bool grouped); + + /** + * Whether the given command should run when the robot is disabled. Override + * to return true if the command should run when disabled. + * + * @return whether the command should run when the robot is disabled + */ + virtual bool RunsWhenDisabled() const { return false; } + + virtual std::string GetName() const; + + protected: + /** + * Transfers ownership of this command to a unique pointer. Used for + * decorator methods. + */ + virtual std::unique_ptr TransferOwnership() && = 0; + + bool m_isGrouped = false; +}; + +/** + * Checks if two commands have disjoint requirement sets. + * + * @param first The first command to check. + * @param second The second command to check. + * @return False if first and second share a requirement. + */ +bool RequirementsDisjoint(Command* first, Command* second); +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibc/src/main/native/include/frc2/command/CommandBase.h new file mode 100644 index 0000000000..7597d210a1 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandBase.h @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include +#include + +#include "Command.h" + +namespace frc2 { +/** + * A Sendable base class for Commands. + */ +class CommandBase : public frc::Sendable, public Command { + public: + CommandBase(CommandBase&& other) = default; + + CommandBase(const CommandBase& other); + + /** + * Adds the specified requirements to the command. + * + * @param requirements the requirements to add + */ + void AddRequirements(std::initializer_list requirements); + + void AddRequirements(wpi::SmallSet requirements); + + wpi::SmallSet GetRequirements() const override; + + void SetName(const wpi::Twine& name) override; + + std::string GetName() const override; + + std::string GetSubsystem() const override; + + void SetSubsystem(const wpi::Twine& subsystem) override; + + void InitSendable(frc::SendableBuilder& builder) override; + + protected: + CommandBase(); + std::string m_name; + std::string m_subsystem; + wpi::SmallSet m_requirements; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h new file mode 100644 index 0000000000..9d265b493e --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "CommandBase.h" + +namespace frc2 { + +/** + * A base for CommandGroups. Statically tracks commands that have been + * allocated to groups to ensure those commands are not also used independently, + * which can result in inconsistent command state and unpredictable execution. + */ +class CommandGroupBase : public CommandBase { + public: + /** + * Requires that the specified command not have been already allocated to a + * CommandGroup. Reports an error if the command is already grouped. + * + * @param commands The command to check + * @return True if all the command is ungrouped. + */ + static bool RequireUngrouped(Command& command); + + /** + * Requires that the specified commands not have been already allocated to a + * CommandGroup. Reports an error if any of the commands are already grouped. + * + * @param commands The commands to check + * @return True if all the commands are ungrouped. + */ + static bool RequireUngrouped(wpi::ArrayRef>); + + /** + * Requires that the specified commands not have been already allocated to a + * CommandGroup. Reports an error if any of the commands are already grouped. + * + * @param commands The commands to check + * @return True if all the commands are ungrouped. + */ + static bool RequireUngrouped(std::initializer_list); + + /** + * Adds the given commands to the command group. + * + * @param commands The commands to add. + */ + virtual void AddCommands( + std::vector>&& commands) = 0; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h new file mode 100644 index 0000000000..ec15f88020 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include "Command.h" + +namespace frc2 { + +/** + * CRTP implementation to allow polymorphic decorator functions in Command. + * + *

Note: ALWAYS create a subclass by extending CommandHelper, + * or decorators will not function! + */ +template >> +class CommandHelper : public Base { + using Base::Base; + + public: + CommandHelper() = default; + + protected: + std::unique_ptr TransferOwnership() && override { + return std::make_unique(std::move(*static_cast(this))); + } +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h new file mode 100644 index 0000000000..ac6179d0b2 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h @@ -0,0 +1,362 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "CommandState.h" + +namespace frc2 { +class Command; +class Subsystem; + +/** + * The scheduler responsible for running Commands. A Command-based robot should + * call Run() on the singleton instance in its periodic block in order to run + * commands synchronously from the main loop. Subsystems should be registered + * with the scheduler using RegisterSubsystem() in order for their Periodic() + * methods to be called and for their default commands to be scheduled. + */ +class CommandScheduler final : public frc::SendableBase, frc::ErrorBase { + public: + /** + * Returns the Scheduler instance. + * + * @return the instance + */ + static CommandScheduler& GetInstance(); + + using Action = std::function; + + /** + * Adds a button binding to the scheduler, which will be polled to schedule + * commands. + * + * @param button The button to add + */ + void AddButton(wpi::unique_function button); + + /** + * Removes all button bindings from the scheduler. + */ + void ClearButtons(); + + /** + * Schedules a command for execution. Does nothing if the command is already + * scheduled. If a command's requirements are not available, it will only be + * started if all the commands currently using those requirements have been + * scheduled as interruptible. If this is the case, they will be interrupted + * and the command will be scheduled. + * + * @param interruptible whether this command can be interrupted + * @param command the command to schedule + */ + void Schedule(bool interruptible, Command* command); + + /** + * Schedules a command for execution, with interruptible defaulted to true. + * Does nothing if the command is already scheduled. + * + * @param command the command to schedule + */ + void Schedule(Command* command); + + /** + * Schedules multiple commands for execution. Does nothing if the command is + * already scheduled. If a command's requirements are not available, it will + * only be started if all the commands currently using those requirements have + * been scheduled as interruptible. If this is the case, they will be + * interrupted and the command will be scheduled. + * + * @param interruptible whether the commands should be interruptible + * @param commands the commands to schedule + */ + void Schedule(bool interruptible, wpi::ArrayRef commands); + + /** + * Schedules multiple commands for execution. Does nothing if the command is + * already scheduled. If a command's requirements are not available, it will + * only be started if all the commands currently using those requirements have + * been scheduled as interruptible. If this is the case, they will be + * interrupted and the command will be scheduled. + * + * @param interruptible whether the commands should be interruptible + * @param commands the commands to schedule + */ + void Schedule(bool interruptible, std::initializer_list commands); + + /** + * Schedules multiple commands for execution, with interruptible defaulted to + * true. Does nothing if the command is already scheduled. + * + * @param commands the commands to schedule + */ + void Schedule(wpi::ArrayRef commands); + + /** + * Schedules multiple commands for execution, with interruptible defaulted to + * true. Does nothing if the command is already scheduled. + * + * @param commands the commands to schedule + */ + void Schedule(std::initializer_list commands); + + /** + * Runs a single iteration of the scheduler. The execution occurs in the + * following order: + * + *

Subsystem periodic methods are called. + * + *

Button bindings are polled, and new commands are scheduled from them. + * + *

Currently-scheduled commands are executed. + * + *

End conditions are checked on currently-scheduled commands, and commands + * that are finished have their end methods called and are removed. + * + *

Any subsystems not being used as requirements have their default methods + * started. + */ + void Run(); + + /** + * Registers subsystems with the scheduler. This must be called for the + * subsystem's periodic block to run when the scheduler is run, and for the + * subsystem's default command to be scheduled. It is recommended to call + * this from the constructor of your subsystem implementations. + * + * @param subsystem the subsystem to register + */ + void RegisterSubsystem(Subsystem* subsystem); + + /** + * Un-registers subsystems with the scheduler. The subsystem will no longer + * have its periodic block called, and will not have its default command + * scheduled. + * + * @param subsystem the subsystem to un-register + */ + void UnregisterSubsystem(Subsystem* subsystem); + + void RegisterSubsystem(std::initializer_list subsystems); + + void UnregisterSubsystem(std::initializer_list subsystems); + + /** + * Sets the default command for a subsystem. Registers that subsystem if it + * is not already registered. Default commands will run whenever there is no + * other command currently scheduled that requires the subsystem. Default + * commands should be written to never end (i.e. their IsFinished() method + * should return false), as they would simply be re-scheduled if they do. + * Default commands must also require their subsystem. + * + * @param subsystem the subsystem whose default command will be set + * @param defaultCommand the default command to associate with the subsystem + */ + template >::value>> + void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) { + if (!defaultCommand.HasRequirement(subsystem)) { + wpi_setWPIErrorWithContext( + CommandIllegalUse, "Default commands must require their subsystem!"); + return; + } + if (defaultCommand.IsFinished()) { + wpi_setWPIErrorWithContext(CommandIllegalUse, + "Default commands should not end!"); + return; + } + m_subsystems[subsystem] = std::make_unique>( + std::forward(defaultCommand)); + } + + /** + * Gets the default command associated with this subsystem. Null if this + * subsystem has no default command associated with it. + * + * @param subsystem the subsystem to inquire about + * @return the default command associated with the subsystem + */ + Command* GetDefaultCommand(const Subsystem* subsystem) const; + + /** + * Cancels a command. The scheduler will only call the interrupted method of + * a canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param command the command to cancel + */ + void Cancel(Command* command); + + /** + * Cancels commands. The scheduler will only call the interrupted method of a + * canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param commands the commands to cancel + */ + void Cancel(wpi::ArrayRef commands); + + /** + * Cancels commands. The scheduler will only call the interrupted method of a + * canceled command, not the end method (though the interrupted method may + * itself call the end method). Commands will be canceled even if they are + * not scheduled as interruptible. + * + * @param commands the commands to cancel + */ + void Cancel(std::initializer_list commands); + + /** + * Cancels all commands that are currently scheduled. + */ + void CancelAll(); + + /** + * Returns the time since a given command was scheduled. Note that this only + * works on commands that are directly scheduled by the scheduler; it will not + * work on commands inside of commandgroups, as the scheduler does not see + * them. + * + * @param command the command to query + * @return the time since the command was scheduled, in seconds + */ + double TimeSinceScheduled(const Command* command) const; + + /** + * Whether the given commands are running. Note that this only works on + * commands that are directly scheduled by the scheduler; it will not work on + * commands inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(wpi::ArrayRef commands) const; + + /** + * Whether the given commands are running. Note that this only works on + * commands that are directly scheduled by the scheduler; it will not work on + * commands inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(std::initializer_list commands) const; + + /** + * Whether a given command is running. Note that this only works on commands + * that are directly scheduled by the scheduler; it will not work on commands + * inside of CommandGroups, as the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(const Command* command) const; + + /** + * Returns the command currently requiring a given subsystem. Null if no + * command is currently requiring the subsystem + * + * @param subsystem the subsystem to be inquired about + * @return the command currently requiring the subsystem + */ + Command* Requiring(const Subsystem* subsystem) const; + + /** + * Disables the command scheduler. + */ + void Disable(); + + /** + * Enables the command scheduler. + */ + void Enable(); + + /** + * Adds an action to perform on the initialization of any command by the + * scheduler. + * + * @param action the action to perform + */ + void OnCommandInitialize(Action action); + + /** + * Adds an action to perform on the execution of any command by the scheduler. + * + * @param action the action to perform + */ + void OnCommandExecute(Action action); + + /** + * Adds an action to perform on the interruption of any command by the + * scheduler. + * + * @param action the action to perform + */ + void OnCommandInterrupt(Action action); + + /** + * Adds an action to perform on the finishing of any command by the scheduler. + * + * @param action the action to perform + */ + void OnCommandFinish(Action action); + + void InitSendable(frc::SendableBuilder& builder) override; + + private: + // Constructor; private as this is a singleton + CommandScheduler(); + + // A map from commands to their scheduling state. Also used as a set of the + // currently-running commands. + wpi::DenseMap m_scheduledCommands; + + // A map from required subsystems to their requiring commands. Also used as a + // set of the currently-required subsystems. + wpi::DenseMap m_requirements; + + // A map from subsystems registered with the scheduler to their default + // commands. Also used as a list of currently-registered subsystems. + wpi::DenseMap> m_subsystems; + + // The set of currently-registered buttons that will be polled every + // iteration. + wpi::SmallVector, 4> m_buttons; + + bool m_disabled{false}; + + // NetworkTable entries for use in Sendable impl + nt::NetworkTableEntry m_namesEntry; + nt::NetworkTableEntry m_idsEntry; + nt::NetworkTableEntry m_cancelEntry; + + // Lists of user-supplied actions to be executed on scheduling events for + // every command. + wpi::SmallVector m_initActions; + wpi::SmallVector m_executeActions; + wpi::SmallVector m_interruptActions; + wpi::SmallVector m_finishActions; + + friend class CommandTestBase; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibc/src/main/native/include/frc2/command/CommandState.h new file mode 100644 index 0000000000..41fc5d036b --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/CommandState.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace frc2 { +/** + * Class that holds scheduling state for a command. Used internally by the + * CommandScheduler + */ +class CommandState final { + public: + CommandState() = default; + + explicit CommandState(bool interruptible); + + bool IsInterruptible() const { return m_interruptible; } + + // The time since this command was initialized. + double TimeSinceInitialized() const; + + private: + double m_startTime = -1; + bool m_interruptible; + + void StartTiming(); + void StartRunning(); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h new file mode 100644 index 0000000000..01571861d7 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * Runs one of two commands, depending on the value of the given condition when + * this command is initialized. Does not actually schedule the selected command + * - rather, the command is run through this command; this ensures that the + * command will behave as expected if used as part of a CommandGroup. Requires + * the requirements of both commands, again to ensure proper functioning when + * used in a CommandGroup. If this is undesired, consider using + * ScheduleCommand. + * + *

As this command contains multiple component commands within it, it is + * technically a command group; the command instances that are passed to it + * cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + * + * @see ScheduleCommand + */ +class ConditionalCommand + : public CommandHelper { + public: + /** + * Creates a new ConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + template >::value>, + typename = std::enable_if_t< + std::is_base_of>::value>> + ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function condition) + : ConditionalCommand(std::make_unique>( + std::forward(onTrue)), + std::make_unique>( + std::forward(onFalse)), + condition) {} + + /** + * Creates a new ConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + ConditionalCommand(std::unique_ptr&& onTrue, + std::unique_ptr&& onFalse, + std::function condition); + + ConditionalCommand(ConditionalCommand&& other) = default; + + // No copy constructors for command groups + ConditionalCommand(const ConditionalCommand& other) = delete; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + std::unique_ptr m_onTrue; + std::unique_ptr m_onFalse; + std::function m_condition; + Command* m_selectedCommand{nullptr}; + bool m_runsWhenDisabled = true; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h new file mode 100644 index 0000000000..b47135c52f --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that allows the user to pass in functions for each of the basic + * command methods through the constructor. Useful for inline definitions of + * complex commands - note, however, that if a command is beyond a certain + * complexity it is usually better practice to write a proper class for it than + * to inline it. + */ +class FunctionalCommand : public CommandHelper { + public: + /** + * Creates a new FunctionalCommand. + * + * @param onInit the function to run on command initialization + * @param onExecute the function to run on command execution + * @param onEnd the function to run on command end + * @param isFinished the function that determines whether the command has + * finished + * @param requirements the subsystems required by this command + */ + FunctionalCommand(std::function onInit, + std::function onExecute, + std::function onEnd, + std::function isFinished); + + FunctionalCommand(FunctionalCommand&& other) = default; + + FunctionalCommand(const FunctionalCommand& other) = default; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + std::function m_onInit; + std::function m_onExecute; + std::function m_onEnd; + std::function m_isFinished; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h new file mode 100644 index 0000000000..ec28a11023 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A Command that runs instantly; it will initialize, execute once, and end on + * the same iteration of the scheduler. Users can either pass in a Runnable and + * a set of requirements, or else subclass this command if desired. + */ +class InstantCommand : public CommandHelper { + public: + /** + * Creates a new InstantCommand that runs the given Runnable with the given + * requirements. + * + * @param toRun the Runnable to run + * @param requirements the subsystems required by this command + */ + InstantCommand(std::function toRun, + std::initializer_list requirements); + + InstantCommand(InstantCommand&& other) = default; + + InstantCommand(const InstantCommand& other) = default; + + /** + * Creates a new InstantCommand with a Runnable that does nothing. Useful + * only as a no-arg constructor to call implicitly from subclass constructors. + */ + InstantCommand(); + + void Initialize() override; + + bool IsFinished() final; + + private: + std::function m_toRun; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h new file mode 100644 index 0000000000..037e5c3d71 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that starts a notifier to run the given runnable periodically in a + * separate thread. Has no end condition as-is; either subclass it or use {@link + * Command#withTimeout(double)} or + * {@link Command#interruptOn(BooleanSupplier)} to give it one. + * + *

WARNING: Do not use this class unless you are confident in your ability to + * make the executed code thread-safe. If you do not know what "thread-safe" + * means, that is a good sign that you should not use this class. + */ +class NotifierCommand : public CommandHelper { + public: + /** + * Creates a new NotifierCommand. + * + * @param toRun the runnable for the notifier to run + * @param period the period at which the notifier should run, in seconds + * @param requirements the subsystems required by this command + */ + NotifierCommand(std::function toRun, double period, + std::initializer_list requirements); + + NotifierCommand(NotifierCommand&& other); + + NotifierCommand(const NotifierCommand& other); + + void Initialize() override; + + void End(bool interrupted) override; + + private: + std::function m_toRun; + frc::Notifier m_notifier; + double m_period; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h new file mode 100644 index 0000000000..a54a485d73 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h @@ -0,0 +1,108 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "frc/controller/PIDController.h" +#include "frc2/command/CommandBase.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that controls an output with a PIDController. Runs forever by + * default - to add exit conditions and/or other behavior, subclass this class. + * The controller calculation and output are performed synchronously in the + * command's execute() method. + * + * @see PIDController + */ +class PIDCommand : public CommandHelper { + public: + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpointSource the controller's reference (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, + std::function setpointSource, + std::function useOutput, + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * PIDController with a constant setpoint. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpoint the controller's setpoint (aka setpoint) + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + PIDCommand(PIDController controller, + std::function measurementSource, double setpoint, + std::function useOutput, + std::initializer_list requirements); + + PIDCommand(PIDCommand&& other) = default; + + PIDCommand(const PIDCommand& other) = default; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + /** + * Sets the function that uses the output of the PIDController. + * + * @param useOutput The function that uses the output. + */ + void SetOutput(std::function useOutput); + + /** + * Sets the setpoint for the controller to track the given source. + * + * @param setpointSource The setpoint source + */ + void SetSetpoint(std::function setpointSource); + + /** + * Sets the setpoint for the controller to a constant value. + * + * @param setpoint The setpoint + */ + void SetSetpoint(double setpoint); + + /** + * Sets the setpoint for the controller to a constant value relative (i.e. + * added to) the current setpoint. + * + * @param relativeReference The change in setpoint + */ + void SetSetpointRelative(double relativeSetpoint); + + /** + * Returns the PIDController used by the command. + * + * @return The PIDController + */ + PIDController& getController(); + + protected: + PIDController m_controller; + std::function m_measurement; + std::function m_setpoint; + std::function m_useOutput; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h new file mode 100644 index 0000000000..7aa21c00fa --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "frc/controller/PIDController.h" +#include "frc2/command/SubsystemBase.h" + +namespace frc2 { +/** + * A subsystem that uses a PIDController to control an output. The controller + * is run synchronously from the subsystem's periodic() method. + * + * @see PIDController + */ +class PIDSubsystem : public SubsystemBase { + public: + /** + * Creates a new PIDSubsystem. + * + * @param controller the PIDController to use + */ + explicit PIDSubsystem(PIDController controller); + + void Periodic() override; + + /** + * Uses the output from the PIDController. + * + * @param output the output of the PIDController + */ + virtual void UseOutput(double output) = 0; + + /** + * Returns the reference (setpoint) used by the PIDController. + * + * @return the reference (setpoint) to be used by the controller + */ + virtual double GetSetpoint() = 0; + + /** + * Returns the measurement of the process variable used by the PIDController. + * + * @return the measurement of the process variable + */ + virtual double GetMeasurement() = 0; + + /** + * Enables the PID control. Resets the controller. + */ + virtual void Enable(); + + /** + * Disables the PID control. Sets output to zero. + */ + virtual void Disable(); + + /** + * Returns the PIDController. + * + * @return The controller. + */ + PIDController& GetController(); + + protected: + PIDController m_controller; + bool m_enabled; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h new file mode 100644 index 0000000000..53aca10be0 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h @@ -0,0 +1,86 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending when the last + * command ends. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelCommandGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelCommandGroup. The given commands will be executed + * simultaneously. The command group will finish when the last command + * finishes. If the CommandGroup is interrupted, only the commands that are + * still running will be interrupted. + * + * @param commands the commands to include in this group. + */ + explicit ParallelCommandGroup( + std::vector>&& commands); + + /** + * Creates a new ParallelCommandGroup. The given commands will be executed + * simultaneously. The command group will finish when the last command + * finishes. If the CommandGroup is interrupted, only the commands that are + * still running will be interrupted. + * + * @param commands the commands to include in this group. + */ + template >...>>> + explicit ParallelCommandGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + ParallelCommandGroup(ParallelCommandGroup&& other) = default; + + // No copy constructors for commandgroups + ParallelCommandGroup(const ParallelCommandGroup&) = delete; + + template + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + std::unordered_map, bool> m_commands; + bool m_runWhenDisabled{true}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h new file mode 100644 index 0000000000..d76f033841 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h @@ -0,0 +1,99 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending only when a + * specific command (the "deadline") ends, interrupting all other commands that + * are still running at that point. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelDeadlineGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelDeadlineGroup. The given commands (including the + * deadline) will be executed simultaneously. The CommandGroup will finish + * when the deadline finishes, interrupting all other still-running commands. + * If the CommandGroup is interrupted, only the commands still running will be + * interrupted. + * + * @param deadline the command that determines when the group ends + * @param commands the commands to be executed + */ + ParallelDeadlineGroup(std::unique_ptr&& deadline, + std::vector>&& commands); + /** + * Creates a new ParallelDeadlineGroup. The given commands (including the + * deadline) will be executed simultaneously. The CommandGroup will finish + * when the deadline finishes, interrupting all other still-running commands. + * If the CommandGroup is interrupted, only the commands still running will be + * interrupted. + * + * @param deadline the command that determines when the group ends + * @param commands the commands to be executed + */ + template >::value>, + typename = std::enable_if_t>...>>> + explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) { + SetDeadline(std::make_unique>( + std::forward(deadline))); + AddCommands(std::forward(commands)...); + } + + ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default; + + // No copy constructors for command groups + ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete; + + template >...>>> + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + void SetDeadline(std::unique_ptr&& deadline); + + std::unordered_map, bool> m_commands; + Command* m_deadline; + bool m_runWhenDisabled{true}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h new file mode 100644 index 0000000000..ba249cf5a5 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A CommandGroup that runs a set of commands in parallel, ending when any one + * of the commands ends and interrupting all the others. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class ParallelRaceGroup + : public CommandHelper { + public: + /** + * Creates a new ParallelCommandRace. The given commands will be executed + * simultaneously, and will "race to the finish" - the first command to finish + * ends the entire command, with all other commands being interrupted. + * + * @param commands the commands to include in this group. + */ + explicit ParallelRaceGroup(std::vector>&& commands); + + template >...>>> + explicit ParallelRaceGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + ParallelRaceGroup(ParallelRaceGroup&& other) = default; + + // No copy constructors for command groups + ParallelRaceGroup(const ParallelRaceGroup&) = delete; + + template + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) override; + + std::set> m_commands; + bool m_runWhenDisabled{true}; + bool m_finished{false}; + bool isRunning = false; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h new file mode 100644 index 0000000000..7ed2dfc2b6 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs another command in perpetuity, ignoring that command's + * end conditions. While this class does not extend {@link CommandGroupBase}, + * it is still considered a CommandGroup, as it allows one to compose another + * command within it; the command instances that are passed to it cannot be + * added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class PerpetualCommand : public CommandHelper { + public: + /** + * Creates a new PerpetualCommand. Will run another command in perpetuity, + * ignoring that command's end conditions, unless this command itself is + * interrupted. + * + * @param command the command to run perpetually + */ + explicit PerpetualCommand(std::unique_ptr&& command); + + /** + * Creates a new PerpetualCommand. Will run another command in perpetuity, + * ignoring that command's end conditions, unless this command itself is + * interrupted. + * + * @param command the command to run perpetually + */ + template >::value>> + explicit PerpetualCommand(T&& command) + : PerpetualCommand(std::make_unique>( + std::forward(command))) {} + + PerpetualCommand(PerpetualCommand&& other) = default; + + // No copy constructors for command groups + PerpetualCommand(const PerpetualCommand& other) = delete; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + private: + std::unique_ptr m_command; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h new file mode 100644 index 0000000000..fb420cb179 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "CommandHelper.h" +#include "InstantCommand.h" + +namespace frc2 { +/** + * A command that prints a string when initialized. + */ +class PrintCommand : public CommandHelper { + public: + /** + * Creates a new a PrintCommand. + * + * @param message the message to print + */ + explicit PrintCommand(const wpi::Twine& message); + + PrintCommand(PrintCommand&& other) = default; + + PrintCommand(const PrintCommand& other) = default; + + bool RunsWhenDisabled() const override; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h new file mode 100644 index 0000000000..8906424d7b --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "SetUtilities.h" + +namespace frc2 { +/** + * Schedules the given commands when this command is initialized, and ends when + * all the commands are no longer scheduled. Useful for forking off from + * CommandGroups. If this command is interrupted, it will cancel all of the + * commands. + */ +class ProxyScheduleCommand + : public CommandHelper { + public: + /** + * Creates a new ProxyScheduleCommand that schedules the given commands when + * initialized, and ends when they are all no longer scheduled. + * + * @param toSchedule the commands to schedule + */ + explicit ProxyScheduleCommand(wpi::ArrayRef toSchedule); + + ProxyScheduleCommand(ProxyScheduleCommand&& other) = default; + + ProxyScheduleCommand(const ProxyScheduleCommand& other) = default; + + void Initialize() override; + + void End(bool interrupted) override; + + void Execute() override; + + bool IsFinished() override; + + private: + wpi::SmallVector m_toSchedule; + bool m_finished{false}; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibc/src/main/native/include/frc2/command/RunCommand.h new file mode 100644 index 0000000000..63b1ab8fc4 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/RunCommand.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs a Runnable continuously. Has no end condition as-is; + * either subclass it or use Command.WithTimeout() or + * Command.InterruptOn() to give it one. If you only wish + * to execute a Runnable once, use InstantCommand. + */ +class RunCommand : public CommandHelper { + public: + /** + * Creates a new RunCommand. The Runnable will be run continuously until the + * command ends. Does not run when disabled. + * + * @param toRun the Runnable to run + * @param requirements the subsystems to require + */ + RunCommand(std::function toRun, + std::initializer_list requirements); + + RunCommand(RunCommand&& other) = default; + + RunCommand(const RunCommand& other) = default; + + void Execute(); + + protected: + std::function m_toRun; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h new file mode 100644 index 0000000000..422823b349 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "SetUtilities.h" + +namespace frc2 { +/** + * Schedules the given commands when this command is initialized. Useful for + * forking off from CommandGroups. Note that if run from a CommandGroup, the + * group will not know about the status of the scheduled commands, and will + * treat this command as finishing instantly. + */ +class ScheduleCommand : public CommandHelper { + public: + /** + * Creates a new ScheduleCommand that schedules the given commands when + * initialized. + * + * @param toSchedule the commands to schedule + */ + explicit ScheduleCommand(wpi::ArrayRef toSchedule); + + ScheduleCommand(ScheduleCommand&& other) = default; + + ScheduleCommand(const ScheduleCommand& other) = default; + + void Initialize() override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + wpi::SmallVector m_toSchedule; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h new file mode 100644 index 0000000000..903b8d159a --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include "CommandBase.h" +#include "CommandGroupBase.h" +#include "PrintCommand.h" + +namespace frc2 { +template +/** + * Runs one of a selection of commands, either using a selector and a key to + * command mapping, or a supplier that returns the command directly at runtime. + * Does not actually schedule the selected command - rather, the command is run + * through this command; this ensures that the command will behave as expected + * if used as part of a CommandGroup. Requires the requirements of all included + * commands, again to ensure proper functioning when used in a CommandGroup. If + * this is undesired, consider using ScheduleCommand. + * + *

As this command contains multiple component commands within it, it is + * technically a command group; the command instances that are passed to it + * cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class SelectCommand : public CommandHelper> { + public: + /** + * Creates a new selectcommand. + * + * @param commands the map of commands to choose from + * @param selector the selector to determine which command to run + */ + template >...>>> + SelectCommand(std::function selector, + std::pair... commands) + : m_selector{std::move(selector)} { + std::vector>> foo; + + ((void)foo.emplace_back(commands.first, + std::make_unique>( + std::move(commands.second))), + ...); + + for (auto&& command : foo) { + if (!CommandGroupBase::RequireUngrouped(command.second)) { + return; + } + } + + for (auto&& command : foo) { + this->AddRequirements(command.second->GetRequirements()); + m_runsWhenDisabled &= command.second->RunsWhenDisabled(); + m_commands.emplace(std::move(command.first), std::move(command.second)); + } + } + + SelectCommand( + std::function selector, + std::vector>>&& commands) + : m_selector{std::move(selector)} { + for (auto&& command : commands) { + if (!CommandGroupBase::RequireUngrouped(command.second)) { + return; + } + } + + for (auto&& command : commands) { + this->AddRequirements(command.second->GetRequirements()); + m_runsWhenDisabled &= command.second->RunsWhenDisabled(); + m_commands.emplace(std::move(command.first), std::move(command.second)); + } + } + + // No copy constructors for command groups + SelectCommand(const SelectCommand& other) = delete; + + /** + * Creates a new selectcommand. + * + * @param toRun a supplier providing the command to run + */ + explicit SelectCommand(std::function toRun) : m_toRun{toRun} {} + + SelectCommand(SelectCommand&& other) = default; + + void Initialize() override; + + void Execute() override { m_selectedCommand->Execute(); } + + void End(bool interrupted) override { + return m_selectedCommand->End(interrupted); + } + + bool IsFinished() override { return m_selectedCommand->IsFinished(); } + + bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } + + protected: + std::unique_ptr TransferOwnership() && override { + return std::make_unique(std::move(*this)); + } + + private: + std::unordered_map> m_commands; + std::function m_selector; + std::function m_toRun; + Command* m_selectedCommand; + bool m_runsWhenDisabled = true; +}; + +template +void SelectCommand::Initialize() { + if (m_selector) { + auto find = m_commands.find(m_selector()); + if (find == m_commands.end()) { + m_selectedCommand = new PrintCommand( + "SelectCommand selector value does not correspond to any command!"); + return; + } + m_selectedCommand = find->second.get(); + } else { + m_selectedCommand = m_toRun(); + } + m_selectedCommand->Initialize(); +} + +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h new file mode 100644 index 0000000000..ebb1b4dd43 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include + +#include "CommandGroupBase.h" +#include "CommandHelper.h" +#include "frc/ErrorBase.h" +#include "frc/WPIErrors.h" + +namespace frc2 { + +const size_t invalid_index = std::numeric_limits::max(); + +/** + * A CommandGroups that runs a list of commands in sequence. + * + *

As a rule, CommandGroups require the union of the requirements of their + * component commands. + */ +class SequentialCommandGroup + : public CommandHelper { + public: + /** + * Creates a new SequentialCommandGroup. The given commands will be run + * sequentially, with the CommandGroup finishing when the last command + * finishes. + * + * @param commands the commands to include in this group. + */ + explicit SequentialCommandGroup( + std::vector>&& commands); + + /** + * Creates a new SequentialCommandGroup. The given commands will be run + * sequentially, with the CommandGroup finishing when the last command + * finishes. + * + * @param commands the commands to include in this group. + */ + template >...>>> + explicit SequentialCommandGroup(Types&&... commands) { + AddCommands(std::forward(commands)...); + } + + SequentialCommandGroup(SequentialCommandGroup&& other) = default; + + // No copy constructors for command groups + SequentialCommandGroup(const SequentialCommandGroup&) = delete; + + template >...>>> + void AddCommands(Types&&... commands) { + std::vector> foo; + ((void)foo.emplace_back(std::make_unique>( + std::forward(commands))), + ...); + AddCommands(std::move(foo)); + } + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + void AddCommands(std::vector>&& commands) final; + + wpi::SmallVector, 4> m_commands; + size_t m_currentCommandIndex{invalid_index}; + bool m_runWhenDisabled{true}; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h new file mode 100644 index 0000000000..d21f5725c8 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +namespace frc2 { +template +void SetInsert(wpi::SmallVectorImpl& vector, wpi::ArrayRef toAdd) { + for (auto addCommand : toAdd) { + bool exists = false; + for (auto existingCommand : vector) { + if (addCommand == existingCommand) { + exists = true; + break; + } + } + if (!exists) { + vector.emplace_back(addCommand); + } + } +} +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h new file mode 100644 index 0000000000..de59f828ad --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "CommandBase.h" +#include "CommandHelper.h" + +namespace frc2 { +/** + * A command that runs a given runnable when it is initalized, and another + * runnable when it ends. Useful for running and then stopping a motor, or + * extending and then retracting a solenoid. Has no end condition as-is; either + * subclass it or use Command.WithTimeout() or Command.InterruptOn() to give it + * one. + */ +class StartEndCommand : public CommandHelper { + public: + /** + * Creates a new StartEndCommand. Will run the given runnables when the + * command starts and when it ends. + * + * @param onInit the Runnable to run on command init + * @param onEnd the Runnable to run on command end + * @param requirements the subsystems required by this command + */ + StartEndCommand(std::function onInit, std::function onEnd, + std::initializer_list requirements); + + StartEndCommand(StartEndCommand&& other) = default; + + StartEndCommand(const StartEndCommand& other); + + void Initialize() override; + + void End(bool interrupted) override; + + protected: + std::function m_onInit; + std::function m_onEnd; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibc/src/main/native/include/frc2/command/Subsystem.h new file mode 100644 index 0000000000..f549e64264 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/Subsystem.h @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +namespace frc2 { +class Command; +/** + * A robot subsystem. Subsystems are the basic unit of robot organization in + * the Command-based framework; they encapsulate low-level hardware objects + * (motor controllers, sensors, etc) and provide methods through which they can + * be used by Commands. Subsystems are used by the CommandScheduler's resource + * management system to ensure multiple robot actions are not "fighting" over + * the same hardware; Commands that use a subsystem should include that + * subsystem in their GetRequirements() method, and resources used within a + * subsystem should generally remain encapsulated and not be shared by other + * parts of the robot. + * + *

Subsystems must be registered with the scheduler with the + * CommandScheduler.RegisterSubsystem() method in order for the + * Periodic() method to be called. It is recommended that this method be called + * from the constructor of users' Subsystem implementations. The + * SubsystemBase class offers a simple base for user implementations + * that handles this. + * + * @see Command + * @see CommandScheduler + * @see SubsystemBase + */ +class Subsystem { + public: + ~Subsystem(); + /** + * This method is called periodically by the CommandScheduler. Useful for + * updating subsystem-specific state that you don't want to offload to a + * Command. Teams should try to be consistent within their own codebases + * about which responsibilities will be handled by Commands, and which will be + * handled here. + */ + virtual void Periodic(); + + /** + * Sets the default Command of the subsystem. The default command will be + * automatically scheduled when no other commands are scheduled that require + * the subsystem. Default commands should generally not end on their own, i.e. + * their IsFinished() method should always return false. Will automatically + * register this subsystem with the CommandScheduler. + * + * @param defaultCommand the default command to associate with this subsystem + */ + template >::value>> + void SetDefaultCommand(T&& defaultCommand) { + CommandScheduler::GetInstance().SetDefaultCommand( + this, std::forward(defaultCommand)); + } + + /** + * Gets the default command for this subsystem. Returns null if no default + * command is currently associated with the subsystem. + * + * @return the default command associated with this subsystem + */ + Command* GetDefaultCommand() const; + + /** + * Returns the command currently running on this subsystem. Returns null if + * no command is currently scheduled that requires this subsystem. + * + * @return the scheduled command currently requiring this subsystem + */ + Command* GetCurrentCommand() const; + + /** + * Registers this subsystem with the CommandScheduler, allowing its + * Periodic() method to be called when the scheduler runs. + */ + void Register(); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h new file mode 100644 index 0000000000..2546f5bfab --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "Subsystem.h" + +namespace frc2 { +/** + * A base for subsystems that handles registration in the constructor, and + * provides a more intuitive method for setting the default command. + */ +class SubsystemBase : public Subsystem, public frc::Sendable { + public: + void InitSendable(frc::SendableBuilder& builder) override; + std::string GetName() const override; + void SetName(const wpi::Twine& name) override; + std::string GetSubsystem() const override; + void SetSubsystem(const wpi::Twine& name) override; + void AddChild(std::string name, frc::Sendable* child); + + protected: + SubsystemBase(); + std::string m_name; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h new file mode 100644 index 0000000000..070790b358 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" +#include "frc/Timer.h" + +namespace frc2 { +/** + * A command that does nothing but takes a specified amount of time to finish. + * Useful for CommandGroups. Can also be subclassed to make a command with an + * internal timer. + */ +class WaitCommand : public CommandHelper { + public: + /** + * Creates a new WaitCommand. This command will do nothing, and end after the + * specified duration. + * + * @param seconds the time to wait, in seconds + */ + explicit WaitCommand(double seconds); + + WaitCommand(WaitCommand&& other) = default; + + WaitCommand(const WaitCommand& other) = default; + + void Initialize() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + protected: + std::unique_ptr m_timer; + + private: + double m_duration; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h new file mode 100644 index 0000000000..8da18e80c6 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "CommandBase.h" +#include "frc/Timer.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that does nothing but ends after a specified match time or + * condition. Useful for CommandGroups. + */ +class WaitUntilCommand : public CommandHelper { + public: + /** + * Creates a new WaitUntilCommand that ends after a given condition becomes + * true. + * + * @param condition the condition to determine when to end + */ + explicit WaitUntilCommand(std::function condition); + + /** + * Creates a new WaitUntilCommand that ends after a given match time. + * + *

NOTE: The match timer used for this command is UNOFFICIAL. Using this + * command does NOT guarantee that the time at which the action is performed + * will be judged to be legal by the referees. When in doubt, add a safety + * factor or time the action manually. + * + * @param time the match time after which to end, in seconds + */ + explicit WaitUntilCommand(double time); + + WaitUntilCommand(WaitUntilCommand&& other) = default; + + WaitUntilCommand(const WaitUntilCommand& other) = default; + + bool IsFinished() override; + + bool RunsWhenDisabled() const override; + + private: + std::function m_condition; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibc/src/main/native/include/frc2/command/button/Button.h new file mode 100644 index 0000000000..dc4c54a837 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/Button.h @@ -0,0 +1,207 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once +#include + +#include "Trigger.h" + +namespace frc2 { +class Command; +/** + * A class used to bind command scheduling to button presses. Can be composed + * with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class Button : public Trigger { + public: + /** + * Create a new button that is pressed when the given condition is true. + * + * @param isActive Whether the button is pressed. + */ + explicit Button(std::function isPressed); + + /** + * Create a new button that is pressed active (default constructor) - activity + * can be further determined by subclass code. + */ + Button() = default; + + /** + * Binds a command to start when the button is pressed. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Button WhenPressed(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Button WhenPressed(T&& command, bool interruptible = true) { + WhenActive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute when the button is pressed. + * + * @param toRun the runnable to execute. + */ + Button WhenPressed(std::function toRun); + + /** + * Binds a command to be started repeatedly while the button is pressed, and + * cancelled when it is released. Takes a raw pointer, and so is non-owning; + * users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhileHeld(Command* command, bool interruptible = true); + + /** + * Binds a command to be started repeatedly while the button is pressed, and + * cancelled when it is released. Transfers command ownership to the button + * scheduler, so the user does not have to worry about lifespan - rvalue refs + * will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhileHeld(T&& command, bool interruptible = true) { + WhileActiveContinous(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute repeatedly while the button is pressed. + * + * @param toRun the runnable to execute. + */ + Button WhileHeld(std::function toRun); + + /** + * Binds a command to be started when the button is pressed, and cancelled + * when it is released. Takes a raw pointer, and so is non-owning; users are + * responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhenHeld(Command* command, bool interruptible = true); + + /** + * Binds a command to be started when the button is pressed, and cancelled + * when it is released. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan - rvalue refs will be + * *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhenHeld(T&& command, bool interruptible = true) { + WhileActiveOnce(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a command to start when the button is released. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button WhenReleased(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button WhenReleased(T&& command, bool interruptible = true) { + WhenInactive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a runnable to execute when the button is released. + * + * @param toRun the runnable to execute. + */ + Button WhenReleased(std::function toRun); + + /** + * Binds a command to start when the button is pressed, and be cancelled when + * it is pressed again. Takes a raw pointer, and so is non-owning; users are + * responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + Button ToggleWhenPressed(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the button is pressed, and be cancelled when + * it is pessed again. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan - rvalue refs will be + * *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The button, for chained calls. + */ + template >::value>> + Button ToggleWhenPressed(T&& command, bool interruptible = true) { + ToggleWhenActive(std::forward(command), interruptible); + return *this; + } + + /** + * Binds a command to be cancelled when the button is pressed. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * and scheduling of the command. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button CancelWhenPressed(Command* command); +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h new file mode 100644 index 0000000000..a23738bfa8 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once +#include + +#include "Button.h" + +namespace frc2 { +/** + * A class used to bind command scheduling to joystick button presses. Can be + * composed with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class JoystickButton : public Button { + public: + /** + * Creates a JoystickButton that commands can be bound to. + * + * @param joystick The joystick on which the button is located. + * @param buttonNumber The number of the button on the joystic. + */ + explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber) + : m_joystick{joystick}, m_buttonNumber{buttonNumber} {} + + bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); } + + private: + frc::GenericHID* m_joystick; + int m_buttonNumber; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h new file mode 100644 index 0000000000..758cab2056 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once +#include + +#include "Button.h" + +namespace frc2 { +/** + * A class used to bind command scheduling to joystick POV presses. Can be + * composed with other buttons with the operators in Trigger. + * + * @see Trigger + */ +class POVButton : public Button { + public: + /** + * Creates a POVButton that commands can be bound to. + * + * @param joystick The joystick on which the button is located. + * @param angle The angle of the POV corresponding to a button press. + * @param povNumber The number of the POV on the joystick. + */ + POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0) + : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {} + + bool Get() const override { + return m_joystick->GetPOV(m_povNumber) == m_angle; + } + + private: + frc::GenericHID* m_joystick; + int m_angle; + int m_povNumber; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h new file mode 100644 index 0000000000..30de38d394 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h @@ -0,0 +1,325 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace frc2 { +class Command; +/** + * A class used to bind command scheduling to events. The + * Trigger class is a base for all command-event-binding classes, and so the + * methods are named fairly abstractly; for purpose-specific wrappers, see + * Button. + * + * @see Button + */ +class Trigger { + public: + /** + * Create a new trigger that is active when the given condition is true. + * + * @param isActive Whether the trigger is active. + */ + explicit Trigger(std::function isActive) + : m_isActive{std::move(isActive)} {} + + /** + * Create a new trigger that is never active (default constructor) - activity + * can be further determined by subclass code. + */ + Trigger() { + m_isActive = [] { return false; }; + } + + Trigger(const Trigger& other); + + /** + * Returns whether the trigger is active. Can be overridden by a subclass. + * + * @return Whether the trigger is active. + */ + virtual bool Get() const { return m_isActive(); } + + /** + * Binds a command to start when the trigger becomes active. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhenActive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes active. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhenActive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + + return *this; + } + + /** + * Binds a runnable to execute when the trigger becomes active. + * + * @param toRun the runnable to execute. + */ + Trigger WhenActive(std::function toRun); + + /** + * Binds a command to be started repeatedly while the trigger is active, and + * cancelled when it becomes inactive. Takes a raw pointer, and so is + * non-owning; users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveContinous(Command* command, bool interruptible = true); + + /** + * Binds a command to be started repeatedly while the trigger is active, and + * cancelled when it becomes inactive. Transfers command ownership to the + * button scheduler, so the user does not have to worry about lifespan - + * rvalue refs will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhileActiveContinous(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a runnable to execute repeatedly while the trigger is active. + * + * @param toRun the runnable to execute. + */ + Trigger WhileActiveContinous(std::function toRun); + + /** + * Binds a command to be started when the trigger becomes active, and + * cancelled when it becomes inactive. Takes a raw pointer, and so is + * non-owning; users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveOnce(Command* command, bool interruptible = true); + + /** + * Binds a command to be started when the trigger becomes active, and + * cancelled when it becomes inactive. Transfers command ownership to the + * button scheduler, so the user does not have to worry about lifespan - + * rvalue refs will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhileActiveOnce(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + command->Schedule(interruptible); + } else if (pressedLast && !pressed) { + command->Cancel(); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a command to start when the trigger becomes inactive. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger WhenInactive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes inactive. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be + * *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger WhenInactive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (pressedLast && !pressed) { + command->Schedule(interruptible); + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a runnable to execute when the trigger becomes inactive. + * + * @param toRun the runnable to execute. + */ + Trigger WhenInactive(std::function toRun); + + /** + * Binds a command to start when the trigger becomes active, and be cancelled + * when it again becomes active. Takes a raw pointer, and so is non-owning; + * users are responsible for the lifespan of the command. + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + Trigger ToggleWhenActive(Command* command, bool interruptible = true); + + /** + * Binds a command to start when the trigger becomes active, and be cancelled + * when it again becomes active. Transfers command ownership to the button + * scheduler, so the user does not have to worry about lifespan - rvalue refs + * will be *moved*, lvalue refs will be *copied.* + * + * @param command The command to bind. + * @param interruptible Whether the command should be interruptible. + * @return The trigger, for chained calls. + */ + template >::value>> + Trigger ToggleWhenActive(T&& command, bool interruptible = true) { + CommandScheduler::GetInstance().AddButton( + [pressedLast = Get(), *this, + command = std::make_unique>( + std::forward(command)), + interruptible]() mutable { + bool pressed = Get(); + + if (!pressedLast && pressed) { + if (command->IsScheduled()) { + command->Cancel(); + } else { + command->Schedule(interruptible); + } + } + + pressedLast = pressed; + }); + return *this; + } + + /** + * Binds a command to be cancelled when the trigger becomes active. Takes a + * raw pointer, and so is non-owning; users are responsible for the lifespan + * and scheduling of the command. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger CancelWhenActive(Command* command); + + /** + * Composes two triggers with logical AND. + * + * @return A trigger which is active when both component triggers are active. + */ + Trigger operator&&(Trigger rhs) { + return Trigger([*this, rhs] { return Get() && rhs.Get(); }); + } + + /** + * Composes two triggers with logical OR. + * + * @return A trigger which is active when either component trigger is active. + */ + Trigger operator||(Trigger rhs) { + return Trigger([*this, rhs] { return Get() || rhs.Get(); }); + } + + /** + * Composes a trigger with logical NOT. + * + * @return A trigger which is active when the component trigger is inactive, + * and vice-versa. + */ + Trigger operator!() { + return Trigger([*this] { return !Get(); }); + } + + private: + std::function m_isActive; +}; +} // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp new file mode 100644 index 0000000000..829f0d2ae2 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp @@ -0,0 +1,195 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/RunCommand.h" +#include "frc2/command/WaitUntilCommand.h" +#include "frc2/command/button/Trigger.h" +#include "gtest/gtest.h" + +using namespace frc2; +class ButtonTest : public CommandTestBase {}; + +TEST_F(ButtonTest, WhenPressedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + + WaitUntilCommand command([&finished] { return finished; }); + + Trigger([&pressed] { return pressed; }).WhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhenReleasedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = true; + Trigger([&pressed] { return pressed; }).WhenInactive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhileHeldTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + finished = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, WhenHeldTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + finished = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + + pressed = false; + Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command); + pressed = true; + scheduler.Run(); + pressed = false; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, ToggleWhenPressedTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = false; + WaitUntilCommand command([&finished] { return finished; }); + + pressed = false; + Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + pressed = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, AndTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed1 = false; + bool pressed2 = false; + WaitUntilCommand command([&finished] { return finished; }); + + (Trigger([&pressed1] { return pressed1; }) && + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command); + pressed1 = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed2 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, OrTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed1 = false; + bool pressed2 = false; + WaitUntilCommand command1([&finished] { return finished; }); + WaitUntilCommand command2([&finished] { return finished; }); + + (Trigger([&pressed1] { return pressed1; }) || + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command1); + pressed1 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + + pressed1 = false; + + (Trigger([&pressed1] { return pressed1; }) || + Trigger([&pressed2] { return pressed2; })) + .WhenActive(&command2); + pressed2 = true; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); +} + +TEST_F(ButtonTest, NegateTest) { + auto& scheduler = CommandScheduler::GetInstance(); + bool finished = false; + bool pressed = true; + WaitUntilCommand command([&finished] { return finished; }); + + (!Trigger([&pressed] { return pressed; })).WhenActive(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + pressed = false; + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} + +TEST_F(ButtonTest, RValueButtonTest) { + auto& scheduler = CommandScheduler::GetInstance(); + int counter = 0; + bool pressed = false; + + RunCommand command([&counter] { counter++; }, {}); + + Trigger([&pressed] { return pressed; }).WhenActive(std::move(command)); + scheduler.Run(); + EXPECT_EQ(counter, 0); + pressed = true; + scheduler.Run(); + EXPECT_EQ(counter, 1); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp new file mode 100644 index 0000000000..05f813aef4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/PerpetualCommand.h" +#include "frc2/command/RunCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class CommandDecoratorTest : public CommandTestBase {}; + +TEST_F(CommandDecoratorTest, WithTimeoutTest) { + CommandScheduler scheduler = GetScheduler(); + + auto command = RunCommand([] {}, {}).WithTimeout(.1); + + scheduler.Schedule(&command); + + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, InterruptOnTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = + RunCommand([] {}, {}).InterruptOn([&finished] { return finished; }); + + scheduler.Schedule(&command); + + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + finished = true; + + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, BeforeStartingTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = InstantCommand([] {}, {}).BeforeStarting( + [&finished] { finished = true; }); + + scheduler.Schedule(&command); + + EXPECT_TRUE(finished); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandDecoratorTest, WhenFinishedTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command = + InstantCommand([] {}, {}).WhenFinished([&finished] { finished = true; }); + + scheduler.Schedule(&command); + + EXPECT_FALSE(finished); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(finished); +} + +TEST_F(CommandDecoratorTest, PerpetuallyTest) { + CommandScheduler scheduler = GetScheduler(); + + auto command = InstantCommand([] {}, {}).Perpetually(); + + scheduler.Schedule(&command); + + scheduler.Run(); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp new file mode 100644 index 0000000000..260043064a --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SelectCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class CommandRequirementsTest : public CommandTestBase {}; + +TEST_F(CommandRequirementsTest, RequirementInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement; + + MockCommand command1({&requirement}); + MockCommand command2({&requirement}); + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()); + EXPECT_CALL(command1, End(true)); + EXPECT_CALL(command1, End(false)).Times(0); + + EXPECT_CALL(command2, Initialize()); + EXPECT_CALL(command2, Execute()); + EXPECT_CALL(command2, End(true)).Times(0); + EXPECT_CALL(command2, End(false)).Times(0); + + scheduler.Schedule(&command1); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + scheduler.Schedule(&command2); + EXPECT_FALSE(scheduler.IsScheduled(&command1)); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); + scheduler.Run(); +} + +TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement; + + MockCommand command1({&requirement}); + MockCommand command2({&requirement}); + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()).Times(2); + EXPECT_CALL(command1, End(true)).Times(0); + EXPECT_CALL(command1, End(false)).Times(0); + + EXPECT_CALL(command2, Initialize()).Times(0); + EXPECT_CALL(command2, Execute()).Times(0); + EXPECT_CALL(command2, End(true)).Times(0); + EXPECT_CALL(command2, End(false)).Times(0); + + scheduler.Schedule(false, &command1); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + scheduler.Schedule(&command2); + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + EXPECT_FALSE(scheduler.IsScheduled(&command2)); + scheduler.Run(); +} + +TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) { + TestSubsystem requirement1; + ErrorConfirmer confirmer("require"); + + MockCommand command1; + + requirement1.SetDefaultCommand(std::move(command1)); + + EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp new file mode 100644 index 0000000000..6572a98e3a --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" + +using namespace frc2; +class CommandScheduleTest : public CommandTestBase {}; + +TEST_F(CommandScheduleTest, InstantScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()); + EXPECT_CALL(command, End(false)); + + command.SetFinished(true); + scheduler.Schedule(&command); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, SingleIterationScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()).Times(2); + EXPECT_CALL(command, End(false)); + + scheduler.Schedule(&command); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Run(); + command.SetFinished(true); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, MultiScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command1; + MockCommand command2; + MockCommand command3; + + EXPECT_CALL(command1, Initialize()); + EXPECT_CALL(command1, Execute()).Times(2); + EXPECT_CALL(command1, End(false)); + + EXPECT_CALL(command2, Initialize()); + EXPECT_CALL(command2, Execute()).Times(3); + EXPECT_CALL(command2, End(false)); + + EXPECT_CALL(command3, Initialize()); + EXPECT_CALL(command3, Execute()).Times(4); + EXPECT_CALL(command3, End(false)); + + scheduler.Schedule(&command1); + scheduler.Schedule(&command2); + scheduler.Schedule(&command3); + EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3})); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3})); + command1.SetFinished(true); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3})); + EXPECT_FALSE(scheduler.IsScheduled(&command1)); + command2.SetFinished(true); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2})); + command3.SetFinished(true); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3})); +} + +TEST_F(CommandScheduleTest, SchedulerCancelTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_CALL(command, Initialize()); + EXPECT_CALL(command, Execute()); + EXPECT_CALL(command, End(false)).Times(0); + EXPECT_CALL(command, End(true)); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + scheduler.Cancel(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(CommandScheduleTest, NotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp new file mode 100644 index 0000000000..0429c625d4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" + +using namespace frc2; + +CommandTestBase::CommandTestBase() { + auto& scheduler = CommandScheduler::GetInstance(); + scheduler.CancelAll(); + scheduler.Enable(); + scheduler.ClearButtons(); +} + +CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); } + +void CommandTestBase::SetUp() { + HALSIM_SetDriverStationEnabled(true); + while (!HALSIM_GetDriverStationEnabled()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } +} + +void CommandTestBase::TearDown() { + CommandScheduler::GetInstance().ClearButtons(); +} + +void CommandTestBase::SetDSEnabled(bool enabled) { + HALSIM_SetDriverStationEnabled(enabled); + while (HALSIM_GetDriverStationEnabled() != static_cast(enabled)) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h new file mode 100644 index 0000000000..8b22844dca --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "ErrorConfirmer.h" +#include "frc2/command/CommandGroupBase.h" +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/SetUtilities.h" +#include "frc2/command/SubsystemBase.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "make_vector.h" +#include "simulation/DriverStationSim.h" + +namespace frc2 { +class CommandTestBase : public ::testing::Test { + public: + CommandTestBase(); + + class TestSubsystem : public SubsystemBase {}; + + protected: + class MockCommand : public Command { + public: + MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet()); + MOCK_METHOD0(IsFinished, bool()); + MOCK_CONST_METHOD0(RunsWhenDisabled, bool()); + MOCK_METHOD0(Initialize, void()); + MOCK_METHOD0(Execute, void()); + MOCK_METHOD1(End, void(bool interrupted)); + + MockCommand() { + m_requirements = {}; + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false)); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(true)); + } + + MockCommand(std::initializer_list requirements, + bool finished = false, bool runWhenDisabled = true) { + m_requirements.insert(requirements.begin(), requirements.end()); + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(finished)); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(runWhenDisabled)); + } + + MockCommand(MockCommand&& other) { + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(other.IsFinished())); + EXPECT_CALL(*this, RunsWhenDisabled()) + .WillRepeatedly(::testing::Return(other.RunsWhenDisabled())); + std::swap(m_requirements, other.m_requirements); + EXPECT_CALL(*this, GetRequirements()) + .WillRepeatedly(::testing::Return(m_requirements)); + } + + MockCommand(const MockCommand& other) : Command{} {} + + void SetFinished(bool finished) { + EXPECT_CALL(*this, IsFinished()) + .WillRepeatedly(::testing::Return(finished)); + } + + ~MockCommand() { + auto& scheduler = CommandScheduler::GetInstance(); + scheduler.Cancel(this); + } + + protected: + std::unique_ptr TransferOwnership() && { + return std::make_unique(std::move(*this)); + } + + private: + wpi::SmallSet m_requirements; + }; + + CommandScheduler GetScheduler(); + + void SetUp() override; + + void TearDown() override; + + void SetDSEnabled(bool enabled); +}; +} // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp new file mode 100644 index 0000000000..927721ab39 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SelectCommand.h" + +using namespace frc2; +class ConditionalCommandTest : public CommandTestBase {}; + +TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr mock = std::make_unique(); + MockCommand* mockptr = mock.get(); + + EXPECT_CALL(*mock, Initialize()); + EXPECT_CALL(*mock, Execute()).Times(2); + EXPECT_CALL(*mock, End(false)); + + ConditionalCommand conditional( + std::move(mock), std::make_unique(), [] { return true; }); + + scheduler.Schedule(&conditional); + scheduler.Run(); + mockptr->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&conditional)); +} + +TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ConditionalCommand conditional(std::move(command1), std::move(command2), + [] { return true; }); + scheduler.Schedule(&conditional); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&conditional)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp new file mode 100644 index 0000000000..b97cbb69f7 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class DefaultCommandTest : public CommandTestBase {}; + +TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + RunCommand command1([] {}, {&subsystem}); + + scheduler.SetDefaultCommand(&subsystem, std::move(command1)); + auto handle = scheduler.GetDefaultCommand(&subsystem); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(handle)); +} + +TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + RunCommand command1([] {}, {&subsystem}); + RunCommand command2([] {}, {&subsystem}); + + scheduler.SetDefaultCommand(&subsystem, std::move(command1)); + auto handle = scheduler.GetDefaultCommand(&subsystem); + scheduler.Run(); + scheduler.Schedule(&command2); + + EXPECT_TRUE(scheduler.IsScheduled(&command2)); + EXPECT_FALSE(scheduler.IsScheduled(handle)); + + scheduler.Cancel(&command2); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(handle)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp new file mode 100644 index 0000000000..2565a94271 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ErrorConfirmer.h" + +ErrorConfirmer* ErrorConfirmer::instance; + +int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode, + HAL_Bool isLVCode, const char* details, + const char* location, const char* callStack, + HAL_Bool printMsg) { + if (std::regex_search(details, std::regex(instance->m_msg))) { + instance->ConfirmError(); + } + return 1; +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h new file mode 100644 index 0000000000..011158ca45 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "gmock/gmock.h" +#include "simulation/DriverStationSim.h" + +class ErrorConfirmer { + public: + explicit ErrorConfirmer(const char* msg) : m_msg(msg) { + if (instance != nullptr) return; + HALSIM_SetSendError(HandleError); + EXPECT_CALL(*this, ConfirmError()); + instance = this; + } + + ~ErrorConfirmer() { + HALSIM_SetSendError(nullptr); + instance = nullptr; + } + + MOCK_METHOD0(ConfirmError, void()); + + const char* m_msg; + + static int32_t HandleError(HAL_Bool isError, int32_t errorCode, + HAL_Bool isLVCode, const char* details, + const char* location, const char* callStack, + HAL_Bool printMsg); + + private: + static ErrorConfirmer* instance; +}; diff --git a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp new file mode 100644 index 0000000000..140d4fbe1c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" + +using namespace frc2; +class FunctionalCommandTest : public CommandTestBase {}; + +TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + bool finished = false; + + FunctionalCommand command( + [&counter] { counter++; }, [&counter] { counter++; }, + [&counter](bool) { counter++; }, [&finished] { return finished; }); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_EQ(4, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp new file mode 100644 index 0000000000..e91f6779e7 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" + +using namespace frc2; +class InstantCommandTest : public CommandTestBase {}; + +TEST_F(InstantCommandTest, InstantCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + InstantCommand command([&counter] { counter++; }, {}); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_EQ(counter, 1); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp new file mode 100644 index 0000000000..9f2aefc7d1 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/NotifierCommand.h" + +using namespace frc2; +class NotifierCommandTest : public CommandTestBase {}; + +#ifdef __APPLE__ +TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) { +#else +TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) { +#endif + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + NotifierCommand command([&counter] { counter++; }, 0.01, {}); + + scheduler.Schedule(&command); + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + scheduler.Cancel(&command); + + EXPECT_NEAR(.01 * counter, .25, .025); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp new file mode 100644 index 0000000000..55c7b98514 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelCommandGroupTest : public CommandTestBase {}; + +TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + + ParallelCommandGroup group(tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + + ParallelCommandGroup group(tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)).Times(0); + EXPECT_CALL(*command2, End(true)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + scheduler.Run(); + scheduler.Cancel(&group); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelCommandGroup group((InstantCommand(), InstantCommand())); + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelCommandGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelCommandGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp new file mode 100644 index 0000000000..6e3246f8cb --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp @@ -0,0 +1,136 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelDeadlineGroupTest : public CommandTestBase {}; + +TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelDeadlineGroup group( + std::move(command1Holder), + tcb::make_vector>(std::move(command2Holder), + std::move(command3Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(2); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(2); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + command2->SetFinished(true); + scheduler.Run(); + command1->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem subsystem; + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelDeadlineGroup group( + std::move(command1Holder), + tcb::make_vector>(std::move(command2Holder), + std::move(command3Holder))); + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelDeadlineGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelDeadlineGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelDeadlineGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp new file mode 100644 index 0000000000..dad02a128c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ParallelRaceGroupTest : public CommandTestBase {}; + +TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelRaceGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(2); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(2); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(2); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + ParallelRaceGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(true)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(true)); + + scheduler.Schedule(&group); + + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelRaceGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + ParallelRaceGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + ParallelRaceGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp new file mode 100644 index 0000000000..b3ef86143e --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/PerpetualCommand.h" + +using namespace frc2; +class PerpetualCommandTest : public CommandTestBase {}; + +TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + bool check = false; + + PerpetualCommand command{InstantCommand([&check] { check = true; }, {})}; + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(check); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp new file mode 100644 index 0000000000..b940566d7e --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "CommandTestBase.h" +#include "frc2/command/PrintCommand.h" + +using namespace frc2; +class PrintCommandTest : public CommandTestBase {}; + +TEST_F(PrintCommandTest, PrintCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + PrintCommand command("Test!"); + + testing::internal::CaptureStdout(); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(), + std::regex("Test!"))); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp new file mode 100644 index 0000000000..09a569faba --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class ProxyScheduleCommandTest : public CommandTestBase {}; + +TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool scheduled = false; + + InstantCommand toSchedule([&scheduled] { scheduled = true; }, {}); + + ProxyScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduled); +} + +TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool finished = false; + + WaitUntilCommand toSchedule([&finished] { return finished; }); + + ProxyScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp new file mode 100644 index 0000000000..bac40d53d3 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp @@ -0,0 +1,156 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/SelectCommand.h" +#include "frc2/command/SequentialCommandGroup.h" + +using namespace frc2; +class RobotDisabledCommandTest : public CommandTestBase {}; + +TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + MockCommand command({}, false, false); + + EXPECT_CALL(command, End(true)); + + SetDSEnabled(true); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + SetDSEnabled(false); + + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} + +TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + MockCommand command1; + MockCommand command2; + + scheduler.Schedule(&command1); + + SetDSEnabled(false); + + scheduler.Run(); + + scheduler.Schedule(&command2); + + EXPECT_TRUE(scheduler.IsScheduled(&command1)); + EXPECT_TRUE(scheduler.IsScheduled(&command2)); +} + +TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()}; + SequentialCommandGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelCommandGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelRaceGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()}; + ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(), + MockCommand({}, false, false)}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(), + [] { return true; }}; + ConditionalCommand dontRunWhenDisabled{ + MockCommand(), MockCommand({}, false, false), [] { return true; }}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} + +TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) { + CommandScheduler scheduler = GetScheduler(); + + SelectCommand runWhenDisabled{[] { return 1; }, + std::pair(1, MockCommand()), + std::pair(1, MockCommand())}; + SelectCommand dontRunWhenDisabled{ + [] { return 1; }, std::pair(1, MockCommand()), + std::pair(1, MockCommand({}, false, false))}; + + SetDSEnabled(false); + + scheduler.Schedule(&runWhenDisabled); + scheduler.Schedule(&dontRunWhenDisabled); + + EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled)); + EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp new file mode 100644 index 0000000000..07eecb3f19 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class RunCommandTest : public CommandTestBase {}; + +TEST_F(RunCommandTest, RunCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + RunCommand command([&counter] { counter++; }, {}); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Run(); + scheduler.Run(); + + EXPECT_EQ(3, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp new file mode 100644 index 0000000000..249561b24c --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ScheduleCommand.h" + +using namespace frc2; +class ScheduleCommandTest : public CommandTestBase {}; + +TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) { + CommandScheduler& scheduler = CommandScheduler::GetInstance(); + + bool scheduled = false; + + InstantCommand toSchedule([&scheduled] { scheduled = true; }, {}); + + ScheduleCommand command(&toSchedule); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_TRUE(scheduled); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp new file mode 100644 index 0000000000..f0198c9187 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; +class SchedulerTest : public CommandTestBase {}; + +TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { + CommandScheduler scheduler = GetScheduler(); + + InstantCommand command; + + int counter = 0; + + scheduler.OnCommandInitialize([&counter](const Command&) { counter++; }); + scheduler.OnCommandExecute([&counter](const Command&) { counter++; }); + scheduler.OnCommandFinish([&counter](const Command&) { counter++; }); + + scheduler.Schedule(&command); + scheduler.Run(); + + EXPECT_EQ(counter, 3); +} + +TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + RunCommand command([] {}, {}); + + int counter = 0; + + scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; }); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Cancel(&command); + + EXPECT_EQ(counter, 1); +} + +TEST_F(SchedulerTest, UnregisterSubsystemTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem system; + + scheduler.RegisterSubsystem(&system); + + EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp new file mode 100644 index 0000000000..6be14efcd5 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SelectCommand.h" + +using namespace frc2; +class SelectCommandTest : public CommandTestBase {}; + +TEST_F(SelectCommandTest, SelectCommandTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr mock = std::make_unique(); + MockCommand* mockptr = mock.get(); + + EXPECT_CALL(*mock, Initialize()); + EXPECT_CALL(*mock, Execute()).Times(2); + EXPECT_CALL(*mock, End(false)); + + std::vector>> temp; + + temp.emplace_back(std::pair(1, std::move(mock))); + temp.emplace_back(std::pair(2, std::make_unique())); + temp.emplace_back(std::pair(3, std::make_unique())); + + SelectCommand select([] { return 1; }, std::move(temp)); + + scheduler.Schedule(&select); + scheduler.Run(); + mockptr->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&select)); +} + +TEST_F(SelectCommandTest, SelectCommandRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + SelectCommand select([] { return 1; }, std::pair(1, std::move(command1)), + std::pair(2, std::move(command2))); + + scheduler.Schedule(&select); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&select)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp new file mode 100644 index 0000000000..3a6f8d8ba4 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp @@ -0,0 +1,137 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class SequentialCommandGroupTest : public CommandTestBase {}; + +TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + SequentialCommandGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(1); + EXPECT_CALL(*command2, End(false)); + + EXPECT_CALL(*command3, Initialize()); + EXPECT_CALL(*command3, Execute()).Times(1); + EXPECT_CALL(*command3, End(false)); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + command2->SetFinished(true); + scheduler.Run(); + command3->SetFinished(true); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) { + CommandScheduler scheduler = GetScheduler(); + + std::unique_ptr command1Holder = std::make_unique(); + std::unique_ptr command2Holder = std::make_unique(); + std::unique_ptr command3Holder = std::make_unique(); + + MockCommand* command1 = command1Holder.get(); + MockCommand* command2 = command2Holder.get(); + MockCommand* command3 = command3Holder.get(); + + SequentialCommandGroup group{tcb::make_vector>( + std::move(command1Holder), std::move(command2Holder), + std::move(command3Holder))}; + + EXPECT_CALL(*command1, Initialize()); + EXPECT_CALL(*command1, Execute()).Times(1); + EXPECT_CALL(*command1, End(false)); + + EXPECT_CALL(*command2, Initialize()); + EXPECT_CALL(*command2, Execute()).Times(0); + EXPECT_CALL(*command2, End(false)).Times(0); + EXPECT_CALL(*command2, End(true)); + + EXPECT_CALL(*command3, Initialize()).Times(0); + EXPECT_CALL(*command3, Execute()).Times(0); + EXPECT_CALL(*command3, End(false)).Times(0); + EXPECT_CALL(*command3, End(true)).Times(0); + + scheduler.Schedule(&group); + + command1->SetFinished(true); + scheduler.Run(); + scheduler.Cancel(&group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) { + CommandScheduler scheduler = GetScheduler(); + + SequentialCommandGroup group{InstantCommand(), InstantCommand()}; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + SequentialCommandGroup group(command); + scheduler.Schedule(&group); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&group)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} + +TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) { + CommandScheduler scheduler = GetScheduler(); + + TestSubsystem requirement1; + TestSubsystem requirement2; + TestSubsystem requirement3; + TestSubsystem requirement4; + + InstantCommand command1([] {}, {&requirement1, &requirement2}); + InstantCommand command2([] {}, {&requirement3}); + InstantCommand command3([] {}, {&requirement3, &requirement4}); + + SequentialCommandGroup group(std::move(command1), std::move(command2)); + + scheduler.Schedule(&group); + scheduler.Schedule(&command3); + + EXPECT_TRUE(scheduler.IsScheduled(&command3)); + EXPECT_FALSE(scheduler.IsScheduled(&group)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp new file mode 100644 index 0000000000..3f2579259b --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/StartEndCommand.h" + +using namespace frc2; +class StartEndCommandTest : public CommandTestBase {}; + +TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; }, + {}); + + scheduler.Schedule(&command); + scheduler.Run(); + scheduler.Run(); + scheduler.Cancel(&command); + + EXPECT_EQ(2, counter); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp new file mode 100644 index 0000000000..40aa959723 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class WaitCommandTest : public CommandTestBase {}; + +TEST_F(WaitCommandTest, WaitCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + WaitCommand command(.1); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + std::this_thread::sleep_for(std::chrono::milliseconds(110)); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp new file mode 100644 index 0000000000..e9728db102 --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CommandTestBase.h" +#include "frc2/command/WaitUntilCommand.h" + +using namespace frc2; +class WaitUntilCommandTest : public CommandTestBase {}; + +TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + WaitUntilCommand command([&finished] { return finished; }); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + finished = true; + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h new file mode 100644 index 0000000000..a15f5a8e5d --- /dev/null +++ b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +namespace tcb { + +namespace detail { + +template +struct vec_type_helper { + using type = T; +}; + +template +struct vec_type_helper { + using type = typename std::common_type::type; +}; + +template +using vec_type_helper_t = typename vec_type_helper::type; + +template +struct all_constructible_and_convertible : std::true_type {}; + +template +struct all_constructible_and_convertible + : std::conditional::value && + std::is_convertible::value, + all_constructible_and_convertible, + std::false_type>::type {}; + +template ::value, + int>::type = 0> +std::vector make_vector_impl(Args&&... args) { + std::vector vec; + vec.reserve(sizeof...(Args)); + using arr_t = int[]; + (void)arr_t{0, (vec.emplace_back(std::forward(args)), 0)...}; + return vec; +} + +template ::value, + int>::type = 0> +std::vector make_vector_impl(Args&&... args) { + return std::vector{std::forward(args)...}; +} + +} // namespace detail + +template , + typename std::enable_if< + detail::all_constructible_and_convertible::value, + int>::type = 0> +std::vector make_vector(Args&&... args) { + return detail::make_vector_impl(std::forward(args)...); +} + +} // namespace tcb diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp index f6c3f99a07..d025f06088 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -32,7 +32,7 @@ class Robot : public frc::TimedRobot { private: frc::Joystick m_joystick{1}; frc::Encoder m_encoder{1, 2}; - frc::Spark m_motor{1}; + frc::PWMVictorSPX m_motor{1}; // Create a PID controller whose setpoint's change is subject to maximum // velocity and acceleration constraints. diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index 5dbab38455..9a15f316ce 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ class Robot : public frc::TimedRobot { private: frc::Joystick m_joystick{1}; frc::Encoder m_encoder{1, 2}; - frc::Spark m_motor{1}; + frc::PWMVictorSPX m_motor{1}; frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt}; frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..79f172ec59 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Spin up the shooter when the 'A' button is pressed + frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter); + + // Turn off the shooter when the 'B' button is pressed + frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter); + + // Shoot when the 'X' button is held + frc2::JoystickButton(&m_driverController, 3) + .WhenPressed(&m_shoot) + .WhenReleased(&m_stopFeeder); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_left1{kLeftMotor1Port}, + m_left2{kLeftMotor2Port}, + m_right1{kRightMotor1Port}, + m_right2{kRightMotor2Port}, + m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, + m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} { + // Set the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp new file mode 100644 index 0000000000..f539ce35f2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/ShooterSubsystem.h" + +#include + +#include "Constants.h" + +using namespace ShooterConstants; + +ShooterSubsystem::ShooterSubsystem() + : PIDSubsystem(frc2::PIDController(kP, kI, kD)), + m_shooterMotor(kShooterMotorPort), + m_feederMotor(kFeederMotorPort), + m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) { + m_controller.SetAbsoluteTolerance(kShooterToleranceRPS); + m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void ShooterSubsystem::UseOutput(double output) { + // Use a feedforward of the form kS + kV * velocity + m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS); +} + +void ShooterSubsystem::Disable() { + // Turn off motor when we disable, since useOutput(0) doesn't stop the motor + // due to our feedforward + frc2::PIDSubsystem::Disable(); + m_shooterMotor.Set(0); +} + +bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); } + +double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); } + +double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; } + +void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); } + +void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h new file mode 100644 index 0000000000..13ef6db1c3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace ShooterConstants { +const int kEncoderPorts[]{4, 5}; +const bool kEncoderReversed = false; +const int kEncoderCPR = 1024; +const double kEncoderDistancePerPulse = + // Distance units will be rotations + 1. / static_cast(kEncoderCPR); + +const int kShooterMotorPort = 4; +const int kFeederMotorPort = 5; + +const double kShooterFreeRPS = 5300; +const double kShooterTargetRPS = 4000; +const double kShooterToleranceRPS = 50; + +const double kP = 1; +const double kI = 0; +const double kD = 0; + +// On a real robot the feedforward constants should be empirically determined; +// these are reasonable guesses. +const double kSFractional = .05; +const double kVFractional = + // Should have value 1 at free speed... + 1. / kShooterFreeRPS; + +const double kFeederSpeed = .5; +} // namespace ShooterConstants + +namespace AutoConstants { +const double kAutoTimeoutSeconds = 12; +const double kAutoShootTimeSeconds = 7; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h new file mode 100644 index 0000000000..0eef0a9c29 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/ShooterSubsystem.h" + +namespace ac = AutoConstants; + +/** + * 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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + ShooterSubsystem m_shooter; + + // A simple autonomous routine that shoots the loaded frisbees + frc2::SequentialCommandGroup m_autonomousCommand = + frc2::SequentialCommandGroup{ + // Start the command by spinning up the shooter... + frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}), + // Wait until the shooter is at speed before feeding the frisbees + frc2::WaitUntilCommand([this] { return m_shooter.AtSetpoint(); }), + // Start running the feeder + frc2::InstantCommand([this] { m_shooter.RunFeeder(); }, {&m_shooter}), + // Shoot for the specified time + frc2::WaitCommand(ac::kAutoShootTimeSeconds)} + // Add a timeout (will end the command if, for instance, the shooter + // never gets up to + // speed) + .WithTimeout(ac::kAutoTimeoutSeconds) + // When the command ends, turn off the shooter and the feeder + .WhenFinished([this] { + m_shooter.Disable(); + m_shooter.StopFeeder(); + }); + + // Assorted commands to be bound to buttons + + frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); }, + {&m_shooter}}; + + frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); }, + {&m_shooter}}; + + // Shoots if the shooter wheen has reached the target speed + frc2::ConditionalCommand m_shoot{ + // Run the feeder + frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}}, + // Do nothing + frc2::InstantCommand(), + // Determine which of the above to do based on whether the shooter has + // reached the + // desired speed + [this] { return m_shooter.AtSetpoint(); }}; + + frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); }, + {&m_shooter}}; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1; + frc::PWMVictorSPX m_left2; + frc::PWMVictorSPX m_right1; + frc::PWMVictorSPX m_right2; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // The left-side drive encoder + frc::Encoder m_leftEncoder; + + // The right-side drive encoder + frc::Encoder m_rightEncoder; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h new file mode 100644 index 0000000000..664c4e097d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +class ShooterSubsystem : public frc2::PIDSubsystem { + public: + ShooterSubsystem(); + + void UseOutput(double output) override; + + double GetSetpoint() override; + + double GetMeasurement() override; + + void Disable() override; + + bool AtSetpoint(); + + void RunFeeder(); + + void StopFeeder(); + + private: + frc::PWMVictorSPX m_shooterMotor; + frc::PWMVictorSPX m_feederMotor; + frc::Encoder m_shooterEncoder; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c4bbe641cf --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/RobotContainer.cpp @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +#include "commands/CloseClaw.h" +#include "commands/OpenClaw.h" +#include "commands/Pickup.h" +#include "commands/Place.h" +#include "commands/PrepareToPickup.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/TankDrive.h" + +RobotContainer::RobotContainer() + : m_autonomousCommand(&m_claw, &m_wrist, &m_elevator, &m_drivetrain) { + frc::SmartDashboard::PutData(&m_drivetrain); + frc::SmartDashboard::PutData(&m_elevator); + frc::SmartDashboard::PutData(&m_wrist); + frc::SmartDashboard::PutData(&m_claw); + + m_claw.Log(); + m_wrist.Log(); + m_elevator.Log(); + m_drivetrain.Log(); + + m_drivetrain.SetDefaultCommand(TankDrive( + &m_drivetrain, + [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); }, + [this] { + return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); + })); + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + frc2::JoystickButton m_dUp{&m_joy, 5}; + frc2::JoystickButton m_dRight{&m_joy, 6}; + frc2::JoystickButton m_dDown{&m_joy, 7}; + frc2::JoystickButton m_dLeft{&m_joy, 8}; + frc2::JoystickButton m_l2{&m_joy, 9}; + frc2::JoystickButton m_r2{&m_joy, 10}; + frc2::JoystickButton m_l1{&m_joy, 11}; + frc2::JoystickButton m_r1{&m_joy, 12}; + + m_dUp.WhenPressed(SetElevatorSetpoint(0.2, &m_elevator)); + m_dDown.WhenPressed(SetElevatorSetpoint(-0.2, &m_elevator)); + m_dRight.WhenPressed(CloseClaw(&m_claw)); + m_dLeft.WhenPressed(OpenClaw(&m_claw)); + + m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator)); + m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator)); + m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator)); + m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain)); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // An example command will be run in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp new file mode 100644 index 0000000000..5db0cdb5f5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Autonomous.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/Autonomous.h" + +#include + +#include "commands/CloseClaw.h" +#include "commands/DriveStraight.h" +#include "commands/Pickup.h" +#include "commands/Place.h" +#include "commands/PrepareToPickup.h" +#include "commands/SetDistanceToBox.h" +#include "commands/SetWristSetpoint.h" + +Autonomous::Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator, + DriveTrain* drivetrain) { + SetName("Autonomous"); + AddCommands( + // clang-format off + PrepareToPickup(claw, wrist, elevator), + Pickup(claw, wrist, elevator), + SetDistanceToBox(0.10, drivetrain), + // DriveStraight(4, drivetrain) // Use encoders if ultrasonic is broken + Place(claw, wrist, elevator), + SetDistanceToBox(0.6, drivetrain), + // DriveStraight(-2, drivetrain) // Use encoders if ultrasonic is broken + frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), + CloseClaw(claw))); + // clang-format on +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp new file mode 100644 index 0000000000..97a9ccf0ca --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/CloseClaw.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/CloseClaw.h" + +#include "Robot.h" + +CloseClaw::CloseClaw(Claw* claw) : m_claw(claw) { + SetName("CloseClaw"); + AddRequirements({m_claw}); +} + +// Called just before this Command runs the first time +void CloseClaw::Initialize() { m_claw->Close(); } + +// Make this return true when this Command no longer needs to run execute() +bool CloseClaw::IsFinished() { return m_claw->IsGripping(); } + +// Called once after isFinished returns true +void CloseClaw::End(bool) { +// NOTE: Doesn't stop in simulation due to lower friction causing the can to +// fall out +// + there is no need to worry about stalling the motor or crushing the can. +#ifndef SIMULATION + m_claw->Stop(); +#endif +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp new file mode 100644 index 0000000000..70a6bd71b1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/DriveStraight.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/DriveStraight.h" + +#include + +#include "Robot.h" + +DriveStraight::DriveStraight(double distance, DriveTrain* drivetrain) + : frc2::CommandHelper( + frc2::PIDController(4, 0, 0), + [this]() { return m_drivetrain->GetDistance(); }, distance, + [this](double output) { m_drivetrain->Drive(output, output); }, + {drivetrain}), + m_drivetrain(drivetrain) { + m_controller.SetAbsoluteTolerance(0.01); +} + +// Called just before this Command runs the first time +void DriveStraight::Initialize() { + // Get everything in a safe starting state. + m_drivetrain->Reset(); + frc2::PIDCommand::Initialize(); +} + +bool DriveStraight::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp new file mode 100644 index 0000000000..6ea6e2cad5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/OpenClaw.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/OpenClaw.h" + +#include "Robot.h" + +OpenClaw::OpenClaw(Claw* claw) + : frc2::CommandHelper(1), m_claw(claw) { + SetName("OpenClaw"); + AddRequirements({m_claw}); +} + +// Called just before this Command runs the first time +void OpenClaw::Initialize() { + m_claw->Open(); + frc2::WaitCommand::Initialize(); +} + +// Called once after isFinished returns true +void OpenClaw::End(bool) { m_claw->Stop(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp new file mode 100644 index 0000000000..996414f237 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Pickup.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/Pickup.h" + +#include + +#include "commands/CloseClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +Pickup::Pickup(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("Pickup"); + AddCommands(CloseClaw(claw), + frc2::ParallelCommandGroup(SetWristSetpoint(-45, wrist), + SetElevatorSetpoint(0.25, elevator))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp new file mode 100644 index 0000000000..764ab0e452 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/Place.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/Place.h" + +#include "commands/OpenClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +Place::Place(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("Place"); + // clang-format off + AddCommands(SetElevatorSetpoint(0.25, elevator), + SetWristSetpoint(0, wrist), + OpenClaw(claw)); + // clang-format on +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp new file mode 100644 index 0000000000..f1399d0e86 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/PrepareToPickup.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/PrepareToPickup.h" + +#include + +#include "commands/OpenClaw.h" +#include "commands/SetElevatorSetpoint.h" +#include "commands/SetWristSetpoint.h" + +PrepareToPickup::PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator) { + SetName("PrepareToPickup"); + AddCommands(OpenClaw(claw), + frc2::ParallelCommandGroup(SetElevatorSetpoint(0, elevator), + SetWristSetpoint(0, wrist))); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp new file mode 100644 index 0000000000..cbe711e069 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetDistanceToBox.cpp @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/SetDistanceToBox.h" + +#include + +#include "Robot.h" + +SetDistanceToBox::SetDistanceToBox(double distance, DriveTrain* drivetrain) + : frc2::CommandHelper( + frc2::PIDController(-2, 0, 0), + [this]() { return m_drivetrain->GetDistanceToObstacle(); }, distance, + [this](double output) { m_drivetrain->Drive(output, output); }, + {drivetrain}), + m_drivetrain(drivetrain) { + m_controller.SetAbsoluteTolerance(0.01); +} + +// Called just before this Command runs the first time +void SetDistanceToBox::Initialize() { + // Get everything in a safe starting state. + m_drivetrain->Reset(); + frc2::PIDCommand::Initialize(); +} + +bool SetDistanceToBox::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp new file mode 100644 index 0000000000..161c1ee0f1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetElevatorSetpoint.cpp @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/SetElevatorSetpoint.h" + +#include + +#include "Robot.h" + +SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator* elevator) + : m_setpoint(setpoint), m_elevator(elevator) { + SetName("SetElevatorSetpoint"); + AddRequirements({m_elevator}); +} + +// Called just before this Command runs the first time +void SetElevatorSetpoint::Initialize() { + m_elevator->SetSetpoint(m_setpoint); + m_elevator->Enable(); +} + +// Make this return true when this Command no longer needs to run execute() +bool SetElevatorSetpoint::IsFinished() { + return m_elevator->GetController().AtSetpoint(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp new file mode 100644 index 0000000000..50c9d0a05a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/SetWristSetpoint.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/SetWristSetpoint.h" + +#include "Robot.h" + +SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist* wrist) + : m_setpoint(setpoint), m_wrist(wrist) { + SetName("SetWristSetpoint"); + AddRequirements({m_wrist}); +} + +// Called just before this Command runs the first time +void SetWristSetpoint::Initialize() { + m_wrist->SetSetpoint(m_setpoint); + m_wrist->Enable(); +} + +// Make this return true when this Command no longer needs to run execute() +bool SetWristSetpoint::IsFinished() { + return m_wrist->GetController().AtSetpoint(); +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp similarity index 52% rename from wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp rename to wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp index a8e94063dc..fed1cdb714 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/commands/TankDrive.cpp @@ -1,31 +1,26 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "commands/MyAutoCommand.h" +#include "commands/TankDrive.h" #include "Robot.h" -MyAutoCommand::MyAutoCommand() { - // Use Requires() here to declare subsystem dependencies - Requires(&Robot::m_subsystem); +TankDrive::TankDrive(DriveTrain* drivetrain, std::function left, + std::function right) + : m_drivetrain(drivetrain), m_left(left), m_right(right) { + SetName("TankDrive"); + AddRequirements({m_drivetrain}); } -// Called just before this Command runs the first time -void MyAutoCommand::Initialize() {} - // Called repeatedly when this Command is scheduled to run -void MyAutoCommand::Execute() {} +void TankDrive::Execute() { m_drivetrain->Drive(m_left(), m_right()); } // Make this return true when this Command no longer needs to run execute() -bool MyAutoCommand::IsFinished() { return false; } +bool TankDrive::IsFinished() { return false; } // Called once after isFinished returns true -void MyAutoCommand::End() {} - -// Called when another command which requires one or more of the same -// subsystems is scheduled to run -void MyAutoCommand::Interrupted() {} +void TankDrive::End(bool) { m_drivetrain->Drive(0, 0); } diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp similarity index 59% rename from wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp rename to wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp index 48afffbc8a..7f2a60f8ac 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Claw.cpp @@ -5,8 +5,20 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "OI.h" +#include "subsystems/Claw.h" -OI::OI() { - // Process operator interface input here. +Claw::Claw() { + // Let's show everything on the LiveWindow + SetName("Claw"); + AddChild("Motor", &m_motor); } + +void Claw::Open() { m_motor.Set(-1); } + +void Claw::Close() { m_motor.Set(1); } + +void Claw::Stop() { m_motor.Set(0); } + +bool Claw::IsGripping() { return m_contact.Get(); } + +void Claw::Log() {} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp new file mode 100644 index 0000000000..8eec077dac --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/DriveTrain.cpp @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveTrain.h" + +#include +#include + +DriveTrain::DriveTrain() { +// Encoders may measure differently in the real world and in +// simulation. In this example the robot moves 0.042 barleycorns +// per tick in the real world, but the simulated encoders +// simulate 360 tick encoders. This if statement allows for the +// real robot to handle this difference in devices. +#ifndef SIMULATION + m_leftEncoder.SetDistancePerPulse(0.042); + m_rightEncoder.SetDistancePerPulse(0.042); +#else + // Circumference in ft = 4in/12(in/ft)*PI + m_leftEncoder.SetDistancePerPulse(static_cast(4.0 / 12.0 * M_PI) / + 360.0); + m_rightEncoder.SetDistancePerPulse(static_cast(4.0 / 12.0 * M_PI) / + 360.0); +#endif + SetName("DriveTrain"); + // Let's show everything on the LiveWindow + AddChild("Front_Left Motor", &m_frontLeft); + AddChild("Rear Left Motor", &m_rearLeft); + AddChild("Front Right Motor", &m_frontRight); + AddChild("Rear Right Motor", &m_rearRight); + AddChild("Left Encoder", &m_leftEncoder); + AddChild("Right Encoder", &m_rightEncoder); + AddChild("Rangefinder", &m_rangefinder); + AddChild("Gyro", &m_gyro); +} + +void DriveTrain::Log() { + frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance()); + frc::SmartDashboard::PutNumber("Right Distance", + m_rightEncoder.GetDistance()); + frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate()); + frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate()); + frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle()); +} + +void DriveTrain::Drive(double left, double right) { + m_robotDrive.TankDrive(left, right); +} + +double DriveTrain::GetHeading() { return m_gyro.GetAngle(); } + +void DriveTrain::Reset() { + m_gyro.Reset(); + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveTrain::GetDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0; +} + +double DriveTrain::GetDistanceToObstacle() { + // Really meters in simulation since it's a rangefinder... + return m_rangefinder.GetAverageVoltage(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp new file mode 100644 index 0000000000..cfa62f8953 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Elevator.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/Elevator.h" + +#include +#include +#include + +Elevator::Elevator() + : frc2::PIDSubsystem(frc2::PIDController(kP_real, kI_real, 0)) { +#ifdef SIMULATION // Check for simulation and update PID values + GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0); +#endif + m_controller.SetAbsoluteTolerance(0.005); + + SetName("Elevator"); + // Let's show everything on the LiveWindow + AddChild("Motor", &m_motor); + AddChild("Pot", &m_pot); +} + +void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); } + +double Elevator::GetMeasurement() { return m_pot.Get(); } + +void Elevator::UseOutput(double d) { m_motor.Set(d); } + +double Elevator::GetSetpoint() { return m_setpoint; } + +void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp new file mode 100644 index 0000000000..953ca9564a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/cpp/subsystems/Wrist.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/Wrist.h" + +#include +#include + +Wrist::Wrist() : frc2::PIDSubsystem(frc2::PIDController(kP_real, 0, 0)) { +#ifdef SIMULATION // Check for simulation and update PID values + GetPIDController()->SetPID(kP_simulation, 0, 0, 0); +#endif + m_controller.SetAbsoluteTolerance(2.5); + + SetName("Wrist"); + // Let's show everything on the LiveWindow + AddChild("Motor", &m_motor); + AddChild("Pot", &m_pot); +} + +void Wrist::Log() { + // frc::SmartDashboard::PutData("Wrist Angle", &m_pot); +} + +double Wrist::GetSetpoint() { return m_setpoint; } + +void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; } + +double Wrist::GetMeasurement() { return m_pot.Get(); } + +void Wrist::UseOutput(double d) { m_motor.Set(d); } diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h new file mode 100644 index 0000000000..f201ae6544 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/RobotContainer.h @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "commands/Autonomous.h" +#include "subsystems/Claw.h" +#include "subsystems/DriveTrain.h" +#include "subsystems/Elevator.h" +#include "subsystems/Wrist.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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The robot's subsystems and commands are defined here... + frc::Joystick m_joy{0}; + + Claw m_claw; + Wrist m_wrist; + Elevator m_elevator; + DriveTrain m_drivetrain; + + Autonomous m_autonomousCommand; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h new file mode 100644 index 0000000000..c0d429c23c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Autonomous.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" +#include "subsystems/DriveTrain.h" +#include "subsystems/Elevator.h" +#include "subsystems/Wrist.h" + +/** + * The main autonomous command to pickup and deliver the soda to the box. + */ +class Autonomous + : public frc2::CommandHelper { + public: + Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator, + DriveTrain* drivetrain); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h new file mode 100644 index 0000000000..a5e6c0b0dc --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/CloseClaw.h @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" + +/** + * Opens the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +class CloseClaw : public frc2::CommandHelper { + public: + explicit CloseClaw(Claw* claw); + void Initialize() override; + bool IsFinished() override; + void End(bool interrupted) override; + + private: + Claw* m_claw; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h new file mode 100644 index 0000000000..1955169efd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/DriveStraight.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveTrain.h" + +/** + * Drive the given distance straight (negative values go backwards). + * Uses a local PID controller to run a simple PID loop that is only + * enabled while this command is running. The input is the averaged + * values of the left and right encoders. + */ +class DriveStraight + : public frc2::CommandHelper { + public: + explicit DriveStraight(double distance, DriveTrain* drivetrain); + void Initialize() override; + bool IsFinished() override; + + private: + DriveTrain* m_drivetrain; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h new file mode 100644 index 0000000000..486f86b715 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/OpenClaw.h @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" + +/** + * Opens the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +class OpenClaw : public frc2::CommandHelper { + public: + explicit OpenClaw(Claw* claw); + void Initialize() override; + void End(bool interrupted) override; + + private: + Claw* m_claw; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h new file mode 100644 index 0000000000..4d74588604 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Pickup.h @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" +#include "subsystems/Elevator.h" +#include "subsystems/Wrist.h" + +/** + * Pickup a soda can (if one is between the open claws) and + * get it in a safe state to drive around. + */ +class Pickup + : public frc2::CommandHelper { + public: + Pickup(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h new file mode 100644 index 0000000000..85945c3035 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/Place.h @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" +#include "subsystems/Elevator.h" +#include "subsystems/Wrist.h" + +/** + * Place a held soda can onto the platform. + */ +class Place : public frc2::CommandHelper { + public: + Place(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h new file mode 100644 index 0000000000..b2aa4d1c99 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/PrepareToPickup.h @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Claw.h" +#include "subsystems/Elevator.h" +#include "subsystems/Wrist.h" + +/** + * Make sure the robot is in a state to pickup soda cans. + */ +class PrepareToPickup : public frc2::CommandHelper { + public: + PrepareToPickup(Claw* claw, Wrist* wrist, Elevator* elevator); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h new file mode 100644 index 0000000000..b5b7b1338e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetDistanceToBox.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveTrain.h" + +/** + * Drive until the robot is the given distance away from the box. Uses a local + * PID controller to run a simple PID loop that is only enabled while this + * command is running. The input is the averaged values of the left and right + * encoders. + */ +class SetDistanceToBox + : public frc2::CommandHelper { + public: + explicit SetDistanceToBox(double distance, DriveTrain* drivetrain); + void Initialize() override; + bool IsFinished() override; + + private: + DriveTrain* m_drivetrain; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h new file mode 100644 index 0000000000..c3bcf6f795 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetElevatorSetpoint.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Elevator.h" + +/** + * Move the elevator to a given location. This command finishes when it is + * within + * the tolerance, but leaves the PID loop running to maintain the position. + * Other + * commands using the elevator should make sure they disable PID! + */ +class SetElevatorSetpoint + : public frc2::CommandHelper { + public: + explicit SetElevatorSetpoint(double setpoint, Elevator* elevator); + void Initialize() override; + bool IsFinished() override; + + private: + double m_setpoint; + Elevator* m_elevator; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h new file mode 100644 index 0000000000..96c01fabe0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/SetWristSetpoint.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/Wrist.h" + +/** + * Move the wrist to a given angle. This command finishes when it is within + * the tolerance, but leaves the PID loop running to maintain the position. + * Other commands using the wrist should make sure they disable PID! + */ +class SetWristSetpoint + : public frc2::CommandHelper { + public: + explicit SetWristSetpoint(double setpoint, Wrist* wrist); + void Initialize() override; + bool IsFinished() override; + + private: + double m_setpoint; + Wrist* m_wrist; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h new file mode 100644 index 0000000000..94b3392afa --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/commands/TankDrive.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveTrain.h" + +/** + * Have the robot drive tank style using the PS3 Joystick until interrupted. + */ +class TankDrive : public frc2::CommandHelper { + public: + TankDrive(DriveTrain* drivetrain, std::function left, + std::function right); + void Execute() override; + bool IsFinished() override; + void End(bool interrupted) override; + + private: + DriveTrain* m_drivetrain; + std::function m_left; + std::function m_right; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h new file mode 100644 index 0000000000..aa898fd2ea --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Claw.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +/** + * The claw subsystem is a simple system with a motor for opening and closing. + * If using stronger motors, you should probably use a sensor so that the + * motors don't stall. + */ +class Claw : public frc2::SubsystemBase { + public: + Claw(); + + /** + * Set the claw motor to move in the open direction. + */ + void Open(); + + /** + * Set the claw motor to move in the close direction. + */ + void Close(); + + /** + * Stops the claw motor from moving. + */ + void Stop(); + + /** + * Return true when the robot is grabbing an object hard enough + * to trigger the limit switch. + */ + bool IsGripping(); + + void Log(); + + private: + frc::PWMVictorSPX m_motor{7}; + frc::DigitalInput m_contact{5}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h new file mode 100644 index 0000000000..0556ecd522 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/DriveTrain.h @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace frc { +class Joystick; +} // namespace frc + +/** + * The DriveTrain subsystem incorporates the sensors and actuators attached to + * the robots chassis. These include four drive motors, a left and right encoder + * and a gyro. + */ +class DriveTrain : public frc2::SubsystemBase { + public: + DriveTrain(); + + /** + * The log method puts interesting information to the SmartDashboard. + */ + void Log(); + + /** + * Tank style driving for the DriveTrain. + * @param left Speed in range [-1,1] + * @param right Speed in range [-1,1] + */ + void Drive(double left, double right); + + /** + * @return The robots heading in degrees. + */ + double GetHeading(); + + /** + * Reset the robots sensors to the zero states. + */ + void Reset(); + + /** + * @return The distance driven (average of left and right encoders). + */ + double GetDistance(); + + /** + * @return The distance to the obstacle detected by the rangefinder. + */ + double GetDistanceToObstacle(); + + private: + frc::PWMVictorSPX m_frontLeft{1}; + frc::PWMVictorSPX m_rearLeft{2}; + frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft}; + + frc::PWMVictorSPX m_frontRight{3}; + frc::PWMVictorSPX m_rearRight{4}; + frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight}; + + frc::DifferentialDrive m_robotDrive{m_left, m_right}; + + frc::Encoder m_leftEncoder{1, 2}; + frc::Encoder m_rightEncoder{3, 4}; + frc::AnalogInput m_rangefinder{6}; + frc::AnalogGyro m_gyro{1}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h new file mode 100644 index 0000000000..bd63610910 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Elevator.h @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +/** + * The elevator subsystem uses PID to go to a given height. Unfortunately, in + * it's current + * state PID values for simulation are different than in the real world do to + * minor differences. + */ +class Elevator : public frc2::PIDSubsystem { + public: + Elevator(); + + /** + * The log method puts interesting information to the SmartDashboard. + */ + void Log(); + + /** + * Use the potentiometer as the PID sensor. This method is automatically + * called by the subsystem. + */ + double GetMeasurement() override; + + /** + * Use the motor as the PID output. This method is automatically called + * by + * the subsystem. + */ + void UseOutput(double d) override; + + double GetSetpoint() override; + + /** + * Sets the setpoint for the subsystem. + */ + void SetSetpoint(double setpoint); + + private: + frc::PWMVictorSPX m_motor{5}; + double m_setpoint = 0; + +// Conversion value of potentiometer varies between the real world and +// simulation +#ifndef SIMULATION + frc::AnalogPotentiometer m_pot{2, -2.0 / 5}; +#else + frc::AnalogPotentiometer m_pot{2}; // Defaults to meters +#endif + + static constexpr double kP_real = 4; + static constexpr double kI_real = 0.07; + static constexpr double kP_simulation = 18; + static constexpr double kI_simulation = 0.2; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h new file mode 100644 index 0000000000..7091f6779b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GearsBotNew/include/subsystems/Wrist.h @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +/** + * The wrist subsystem is like the elevator, but with a rotational joint instead + * of a linear joint. + */ +class Wrist : public frc2::PIDSubsystem { + public: + Wrist(); + + /** + * The log method puts interesting information to the SmartDashboard. + */ + void Log(); + + /** + * Use the potentiometer as the PID sensor. This method is automatically + * called by the subsystem. + */ + double GetMeasurement() override; + + /** + * Use the motor as the PID output. This method is automatically called + * by + * the subsystem. + */ + void UseOutput(double d) override; + + double GetSetpoint() override; + + /** + * Sets the setpoint for the subsystem. + */ + void SetSetpoint(double setpoint); + + private: + frc::PWMVictorSPX m_motor{6}; + double m_setpoint = 0; + +// Conversion value of potentiometer varies between the real world and +// simulation +#ifndef SIMULATION + frc::AnalogPotentiometer m_pot{3, -270.0 / 5}; +#else + frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees +#endif + + static constexpr double kP_real = 1; + static constexpr double kP_simulation = 0.05; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..c67148a310 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Stabilize robot to drive straight with gyro when left bumper is held + frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving); + + // Turn to 90 degrees when the 'X' button is pressed + frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90); + + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // no auto + return nullptr; +} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp new file mode 100644 index 0000000000..1e009f4497 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/TurnToAngle.h" + +#include + +using namespace DriveConstants; + +TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive) + : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD), + // Close loop on heading + [drive] { return drive->GetHeading(); }, + // Set reference to target + targetAngleDegrees, + // Pipe output to turn robot + [drive](double output) { drive->ArcadeDrive(0, output); }, + // Require the drive + {drive}) { + // Set the controller to be continuous (because it is an angle controller) + m_controller.EnableContinuousInput(-180, 180); + // Set the controller tolerance - the delta tolerance ensures the robot is + // stationary at the setpoint before it is considered as having reached the + // reference + m_controller.SetAbsoluteTolerance(kTurnToleranceDeg, + kTurnRateToleranceDegPerS); + + AddRequirements({drive}); +} + +bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..a5b7a61442 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_left1{kLeftMotor1Port}, + m_left2{kLeftMotor2Port}, + m_right1{kRightMotor1Port}, + m_right2{kRightMotor2Port}, + m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, + m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} { + // Set the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} + +double DriveSubsystem::GetHeading() { + return std::remainder(m_gyro.GetAngle(), 360); +} + +double DriveSubsystem::GetTurnRate() { + return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h new file mode 100644 index 0000000000..820e8ce8b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); + +const bool kGyroReversed = false; + +const double kStabilizationP = 1; +const double kStabilizationI = .5; +const double kStabilizationD = 0; + +const double kTurnP = 1; +const double kTurnI = 0; +const double kTurnD = 0; + +const double kTurnToleranceDeg = 5; +const double kTurnRateToleranceDegPerS = 10; // degrees per second +} // namespace DriveConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h new file mode 100644 index 0000000000..4e622ae5c4 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "Constants.h" +#include "commands/TurnToAngle.h" +#include "subsystems/DriveSubsystem.h" + +namespace ac = AutoConstants; +namespace dc = DriveConstants; + +/** + * 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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + + // Assorted commands to be bound to buttons + + // Turn to 90 degrees, with a 5 second timeout + frc2::ParallelRaceGroup m_turnTo90 = TurnToAngle{90, &m_drive}.WithTimeout(5); + + // Stabilize the robot while driving + frc2::PIDCommand m_stabilizeDriving{ + frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI, + dc::kStabilizationD}, + // Close the loop on the turn rate + [this] { return m_drive.GetTurnRate(); }, + // Setpoint is 0 + 0, + // Pipe the output to the turning controls + [this](double output) { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand), + output); + }, + // Require the robot drive + {&m_drive}}; + + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h new file mode 100644 index 0000000000..c7e875e87e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveSubsystem.h" + +/** + * A command that will turn the robot to the specified angle. + */ +class TurnToAngle : public frc2::CommandHelper { + public: + /** + * Turns to robot to the specified angle. + * + * @param targetAngleDegrees The angle to turn to + * @param drive The drive subsystem to use + */ + TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive); + + bool IsFinished() override; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..57ab4f559a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h @@ -0,0 +1,114 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + double GetHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1; + frc::PWMVictorSPX m_left2; + frc::PWMVictorSPX m_right1; + frc::PWMVictorSPX m_right2; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // The left-side drive encoder + frc::Encoder m_leftEncoder; + + // The right-side drive encoder + frc::Encoder m_rightEncoder; + + // The gyro sensor + frc::ADXRS450_Gyro m_gyro; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..f2a9cec655 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Add commands to the autonomous command chooser + m_chooser.AddOption("Simple Auto", &m_simpleAuto); + m_chooser.AddOption("Complex Auto", &m_complexAuto); + + // Put the chooser on the dashboard + frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser); + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.ArcadeDrive( + m_driverController.GetY(frc::GenericHID::kLeftHand), + m_driverController.GetX(frc::GenericHID::kRightHand)); + }, + {&m_drive})); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Grab the hatch when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_grabHatch); + // Release the hatch when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_releaseHatch); + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenPressed(&m_driveHalfSpeed) + .WhenReleased(&m_driveFullSpeed); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return m_chooser.GetSelected(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp new file mode 100644 index 0000000000..40bd60829b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/ComplexAuto.h" + +#include +#include +#include + +using namespace AutoConstants; + +ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { + AddCommands( + // Drive forward the specified distance + frc2::StartEndCommand([drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); }, + [drive] { drive->ArcadeDrive(0, 0); }, {drive}) + .BeforeStarting([drive] { drive->ResetEncoders(); }) + .InterruptOn([drive] { + return drive->GetAverageEncoderDistance() >= + kAutoDriveDistanceInches; + }), + // Release the hatch + frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}), + // Drive backward the specified distance + frc2::StartEndCommand( + [drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); }, + [drive] { drive->ArcadeDrive(0, 0); }, {drive}) + .BeforeStarting([drive] { drive->ResetEncoders(); }) + .InterruptOn([drive] { + return drive->GetAverageEncoderDistance() <= + kAutoBackupDistanceInches; + })); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_left1{kLeftMotor1Port}, + m_left2{kLeftMotor2Port}, + m_right1{kRightMotor1Port}, + m_right2{kRightMotor2Port}, + m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, + m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} { + // Set the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp new file mode 100644 index 0000000000..ea7b7961bd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/HatchSubsystem.h" + +using namespace HatchConstants; + +HatchSubsystem::HatchSubsystem() + : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} + +void HatchSubsystem::GrabHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); +} + +void HatchSubsystem::ReleaseHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h new file mode 100644 index 0000000000..b09572ed0d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace HatchConstants { +const int kHatchSolenoidModule = 0; +const int kHatchSolenoidPorts[]{0, 1}; +} // namespace HatchConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h new file mode 100644 index 0000000000..35edc412f8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "Constants.h" +#include "commands/ComplexAuto.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.h" + +namespace ac = AutoConstants; + +/** + * 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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + HatchSubsystem m_hatch; + + // The autonomous routines + frc2::ParallelRaceGroup m_simpleAuto = + frc2::StartEndCommand( + [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); }, + [this] { m_drive.ArcadeDrive(0, 0); }, {&m_drive}) + .BeforeStarting([this] { m_drive.ResetEncoders(); }) + .InterruptOn([this] { + return m_drive.GetAverageEncoderDistance() >= + ac::kAutoDriveDistanceInches; + }); + ComplexAuto m_complexAuto{&m_drive, &m_hatch}; + + // Assorted commands to be bound to buttons + + frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}}; + frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); }, + {&m_hatch}}; + frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); }, + {}}; + frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, + {}}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h new file mode 100644 index 0000000000..b767f3bbc5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Constants.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.h" + +/** + * A complex auto command that drives forward, releases a hatch, and then drives + * backward. + */ +class ComplexAuto + : public frc2::CommandHelper { + public: + /** + * Creates a new ComplexAuto. + * + * @param drive The drive subsystem this command will run on + * @param hatch The hatch subsystem this command will run on + */ + ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1; + frc::PWMVictorSPX m_left2; + frc::PWMVictorSPX m_right1; + frc::PWMVictorSPX m_right2; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // The left-side drive encoder + frc::Encoder m_leftEncoder; + + // The right-side drive encoder + frc::Encoder m_rightEncoder; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h new file mode 100644 index 0000000000..a41ae1b0e0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "Constants.h" + +class HatchSubsystem : public frc2::SubsystemBase { + public: + HatchSubsystem(); + + // Subsystem methods go here. + + /** + * Grabs the hatch. + */ + void GrabHatch(); + + /** + * Releases the hatch. + */ + void ReleaseHatch(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + frc::DoubleSolenoid m_hatchSolenoid; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..7c02618904 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include + +#include + +#include "commands/DefaultDrive.h" +#include "commands/GrabHatch.h" +#include "commands/HalveDriveSpeed.h" +#include "commands/ReleaseHatch.h" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Add commands to the autonomous command chooser + m_chooser.AddOption("Simple Auto", &m_simpleAuto); + m_chooser.AddOption("Complex Auto", &m_complexAuto); + + // Put the chooser on the dashboard + frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser); + + // Configure the button bindings + ConfigureButtonBindings(); + + // Set up default drive command + m_drive.SetDefaultCommand(DefaultDrive( + &m_drive, + [this] { return m_driverController.GetY(frc::GenericHID::kLeftHand); }, + [this] { return m_driverController.GetX(frc::GenericHID::kRightHand); })); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // NOTE: Using `new` here will leak these commands if they are ever no longer + // needed. This is usually a non-issue as button-bindings tend to be permanent + // - however, if you wish to avoid this, the commands should be + // stack-allocated and declared as members of RobotContainer. + + // Grab the hatch when the 'A' button is pressed. + frc2::JoystickButton(&m_driverController, 1) + .WhenPressed(new GrabHatch(&m_hatch)); + // Release the hatch when the 'B' button is pressed. + frc2::JoystickButton(&m_driverController, 2) + .WhenPressed(new ReleaseHatch(&m_hatch)); + // While holding the shoulder button, drive at half speed + frc2::JoystickButton(&m_driverController, 6) + .WhenHeld(new HalveDriveSpeed(&m_drive)); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Runs the chosen command in autonomous + return m_chooser.GetSelected(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp new file mode 100644 index 0000000000..cb41de6744 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/ComplexAuto.h" + +using namespace AutoConstants; + +ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { + AddCommands( + // Drive forward the specified distance + DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive), + // Release the hatch + ReleaseHatch(hatch), + // Drive backward the specified distance + DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive)); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp new file mode 100644 index 0000000000..3bdee6ef4f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/DefaultDrive.h" + +DefaultDrive::DefaultDrive(DriveSubsystem* subsystem, + std::function forward, + std::function rotation) + : m_drive{subsystem}, m_forward{forward}, m_rotation{rotation} { + AddRequirements({subsystem}); +} + +void DefaultDrive::Execute() { + m_drive->ArcadeDrive(m_forward(), m_rotation()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp new file mode 100644 index 0000000000..6c7ef407e5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/DriveDistance.h" + +#include + +DriveDistance::DriveDistance(double inches, double speed, + DriveSubsystem* subsystem) + : m_drive(subsystem), m_distance(inches), m_speed(speed) { + AddRequirements({subsystem}); +} + +void DriveDistance::Initialize() { + m_drive->ResetEncoders(); + m_drive->ArcadeDrive(m_speed, 0); +} + +void DriveDistance::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); } + +bool DriveDistance::IsFinished() { + return std::abs(m_drive->GetAverageEncoderDistance()) >= m_distance; +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp similarity index 58% rename from wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h rename to wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp index 0b7713ee42..f665761269 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp @@ -1,13 +1,16 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ -#pragma once +#include "commands/GrabHatch.h" -class OI { - public: - OI(); -}; +GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) { + AddRequirements({subsystem}); +} + +void GrabHatch::Initialize() { m_hatch->GrabHatch(); } + +bool GrabHatch::IsFinished() { return true; } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp new file mode 100644 index 0000000000..839bb87f86 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp @@ -0,0 +1,15 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/HalveDriveSpeed.h" + +HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem) + : m_drive(subsystem) {} + +void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); } + +void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp new file mode 100644 index 0000000000..e8fbd61c8f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "commands/ReleaseHatch.h" + +ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) { + AddRequirements({subsystem}); +} + +void ReleaseHatch::Initialize() { m_hatch->ReleaseHatch(); } + +bool ReleaseHatch::IsFinished() { return true; } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000000..64be1b8535 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/DriveSubsystem.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_left1{kLeftMotor1Port}, + m_left2{kLeftMotor2Port}, + m_right1{kRightMotor1Port}, + m_right2{kRightMotor2Port}, + m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, + m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} { + // Set the distance per pulse for the encoders + m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} + +void DriveSubsystem::ArcadeDrive(double fwd, double rot) { + m_drive.ArcadeDrive(fwd, rot); +} + +void DriveSubsystem::ResetEncoders() { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); +} + +double DriveSubsystem::GetAverageEncoderDistance() { + return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.; +} + +frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; } + +frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } + +void DriveSubsystem::SetMaxOutput(double maxOutput) { + m_drive.SetMaxOutput(maxOutput); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp new file mode 100644 index 0000000000..ea7b7961bd --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "subsystems/HatchSubsystem.h" + +using namespace HatchConstants; + +HatchSubsystem::HatchSubsystem() + : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {} + +void HatchSubsystem::GrabHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); +} + +void HatchSubsystem::ReleaseHatch() { + m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); +} diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h new file mode 100644 index 0000000000..b09572ed0d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace DriveConstants { +const int kLeftMotor1Port = 0; +const int kLeftMotor2Port = 1; +const int kRightMotor1Port = 2; +const int kRightMotor2Port = 3; + +const int kLeftEncoderPorts[]{0, 1}; +const int kRightEncoderPorts[]{2, 3}; +const bool kLeftEncoderReversed = false; +const bool kRightEncoderReversed = true; + +const int kEncoderCPR = 1024; +const double kWheelDiameterInches = 6; +const double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * 3.142) / static_cast(kEncoderCPR); +} // namespace DriveConstants + +namespace HatchConstants { +const int kHatchSolenoidModule = 0; +const int kHatchSolenoidPorts[]{0, 1}; +} // namespace HatchConstants + +namespace AutoConstants { +const double kAutoDriveDistanceInches = 60; +const double kAutoBackupDistanceInches = 20; +const double kAutoDriveSpeed = .5; +} // namespace AutoConstants + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h new file mode 100644 index 0000000000..3e663f9bdb --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +#include "Constants.h" +#include "commands/ComplexAuto.h" +#include "commands/DefaultDrive.h" +#include "commands/DriveDistance.h" +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The robot's subsystems and commands are defined here... + + // The robot's subsystems + DriveSubsystem m_drive; + HatchSubsystem m_hatch; + + // The autonomous routines + DriveDistance m_simpleAuto{AutoConstants::kAutoDriveDistanceInches, + AutoConstants::kAutoDriveSpeed, &m_drive}; + ComplexAuto m_complexAuto{&m_drive, &m_hatch}; + + // The chooser for the autonomous routines + frc::SendableChooser m_chooser; + + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h new file mode 100644 index 0000000000..88a246009a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Constants.h" +#include "commands/DriveDistance.h" +#include "commands/ReleaseHatch.h" + +/** + * A complex auto command that drives forward, releases a hatch, and then drives + * backward. + */ +class ComplexAuto + : public frc2::CommandHelper { + public: + /** + * Creates a new ComplexAuto. + * + * @param drive The drive subsystem this command will run on + * @param hatch The hatch subsystem this command will run on + */ + ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h new file mode 100644 index 0000000000..d42d133af0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveSubsystem.h" + +/** + * A command to drive the robot with joystick input passed in through lambdas. + * Written explicitly for pedagogical purposes - actual code should inline a + * command this simple with RunCommand. + * + * @see RunCommand + */ +class DefaultDrive + : public frc2::CommandHelper { + public: + /** + * Creates a new DefaultDrive. + * + * @param subsystem The drive subsystem this command wil run on. + * @param forward The control input for driving forwards/backwards + * @param rotation The control input for turning + */ + DefaultDrive(DriveSubsystem* subsystem, std::function forward, + std::function rotation); + + void Execute() override; + + private: + DriveSubsystem* m_drive; + std::function m_forward; + std::function m_rotation; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h new file mode 100644 index 0000000000..6f350a9049 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveSubsystem.h" + +class DriveDistance + : public frc2::CommandHelper { + public: + /** + * Creates a new DriveDistance. + * + * @param inches The number of inches the robot will drive + * @param speed The speed at which the robot will drive + * @param drive The drive subsystem on which this command will run + */ + DriveDistance(double inches, double speed, DriveSubsystem* subsystem); + + void Initialize() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + DriveSubsystem* m_drive; + double m_distance; + double m_speed; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h new file mode 100644 index 0000000000..0ab0c13dff --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/HatchSubsystem.h" + +/** + * A simple command that grabs a hatch with the HatchSubsystem. Written + * explicitly for pedagogical purposes. Actual code should inline a command + * this simple with InstantCommand. + * + * @see InstantCommand + */ +class GrabHatch : public frc2::CommandHelper { + public: + explicit GrabHatch(HatchSubsystem* subsystem); + + void Initialize() override; + + bool IsFinished() override; + + private: + HatchSubsystem* m_hatch; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h new file mode 100644 index 0000000000..0b5d7c7980 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/DriveSubsystem.h" + +class HalveDriveSpeed + : public frc2::CommandHelper { + public: + explicit HalveDriveSpeed(DriveSubsystem* subsystem); + + void Initialize() override; + + void End(bool interrupted) override; + + private: + DriveSubsystem* m_drive; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h new file mode 100644 index 0000000000..b98866f5ef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "subsystems/HatchSubsystem.h" + +/** + * A simple command that releases a hatch with the HatchSubsystem. Written + * explicitly for pedagogical purposes. Actual code should inline a command + * this simple with InstantCommand. + * + * @see InstantCommand + */ +class ReleaseHatch + : public frc2::CommandHelper { + public: + explicit ReleaseHatch(HatchSubsystem* subsystem); + + void Initialize() override; + + bool IsFinished() override; + + private: + HatchSubsystem* m_hatch; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000000..df36b81f0a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include + +#include "Constants.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + void ArcadeDrive(double fwd, double rot); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + double GetAverageEncoderDistance(); + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + frc::Encoder& GetLeftEncoder(); + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + frc::Encoder& GetRightEncoder(); + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive + * more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + void SetMaxOutput(double maxOutput); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + // The motor controllers + frc::PWMVictorSPX m_left1; + frc::PWMVictorSPX m_left2; + frc::PWMVictorSPX m_right1; + frc::PWMVictorSPX m_right2; + + // The motors on the left side of the drive + frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2}; + + // The motors on the right side of the drive + frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2}; + + // The robot's drive + frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors}; + + // The left-side drive encoder + frc::Encoder m_leftEncoder; + + // The right-side drive encoder + frc::Encoder m_rightEncoder; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h new file mode 100644 index 0000000000..a41ae1b0e0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "Constants.h" + +class HatchSubsystem : public frc2::SubsystemBase { + public: + HatchSubsystem(); + + // Subsystem methods go here. + + /** + * Grabs the hatch. + */ + void GrabHatch(); + + /** + * Releases the hatch. + */ + void ReleaseHatch(); + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + frc::DoubleSolenoid m_hatchSolenoid; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..7d51dc78b8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +#include +#include + +#include + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Set names of commands + m_instantCommand1.SetName("Instant Command 1"); + m_instantCommand2.SetName("Instant Command 2"); + m_waitCommand.SetName("Wait 5 Seconds Command"); + + // Set the scheduler to log Shuffleboard events for command initialize, + // interrupt, finish + frc2::CommandScheduler::GetInstance().OnCommandInitialize( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Initialized", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + frc2::CommandScheduler::GetInstance().OnCommandInterrupt( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Interrupted", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + frc2::CommandScheduler::GetInstance().OnCommandFinish( + [](const frc2::Command& command) { + frc::Shuffleboard::AddEventMarker( + "Command Finished", command.GetName(), + frc::ShuffleboardEventImportance::kNormal); + }); + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here + + // Run instant command 1 when the 'A' button is pressed + frc2::JoystickButton(&m_driverController, 0).WhenPressed(&m_instantCommand1); + // Run instant command 2 when the 'X' button is pressed + frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_instantCommand2); + // Run instant command 3 when the 'Y' button is held; release early to + // interrupt + frc2::JoystickButton(&m_driverController, 4).WhenHeld(&m_waitCommand); +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // no auto + return nullptr; +} diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h new file mode 100644 index 0000000000..0a5f832d33 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This should not be used for any other + * purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h new file mode 100644 index 0000000000..fd93be14e3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "Constants.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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The robot's subsystems and commands are defined here... + + // The driver's controller + frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + + // Three commands that do nothing; for demonstration purposes. + frc2::InstantCommand m_instantCommand1; + frc2::InstantCommand m_instantCommand2; + frc2::WaitCommand m_waitCommand{5}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp new file mode 100644 index 0000000000..6c39023fef --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" + +#include + +#include + +void Robot::RobotInit() {} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want to run during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } + +/** + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. + */ +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +/** + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. + */ +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand != nullptr) { + 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 != nullptr) { + m_autonomousCommand->Cancel(); + m_autonomousCommand = nullptr; + } +} + +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} + +/** + * This function is called periodically during test mode. + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..b06845e823 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +RobotContainer::RobotContainer() { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // Run the select command in autonomous + return &m_exampleSelectCommand; +} diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h new file mode 100644 index 0000000000..0a5f832d33 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This should not be used for any other + * purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace OIConstants { +const int kDriverControllerPort = 1; +} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h new file mode 100644 index 0000000000..872d72a57a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "RobotContainer.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 TestPeriodic() override; + + private: + // Have it null by default so that if testing teleop it + // doesn't have undefined behavior and potentially crash. + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h new file mode 100644 index 0000000000..c93e3203be --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +/** + * 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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The enum used as keys for selecting the command to run. + enum CommandSelector { ONE, TWO, THREE }; + + // An example selector method for the selectcommand. Returns the selector + // that will select which command to run. Can base this choice on logical + // conditions evaluated at runtime. + CommandSelector Select() { return ONE; } + + // The robot's subsystems and commands are defined here... + + // An example selectcommand. Will select from the three commands based on the + // value returned by the selector method at runtime. Note that selectcommand + // takes a generic type, so the selector does not have to be an enum; it could + // be any desired type (string, integer, boolean, double...) + frc2::SelectCommand m_exampleSelectCommand{ + [this] { return Select(); }, + // Maps selector values to commands + std::pair{ONE, frc2::PrintCommand{"Command one was selected!"}}, + std::pair{TWO, frc2::PrintCommand{"Command two was selected!"}}, + std::pair{THREE, frc2::PrintCommand{"Command three was selected!"}}}; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index b3564c7ea0..588a9d07a1 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -240,6 +240,16 @@ "foldername": "GearsBot", "gradlebase": "cpp" }, + { + "name": "GearsBotNew", + "description": "A fully functional example CommandBased program for WPIs GearsBot robot, using the new command-based framework. This code can run on your computer if it supports simulation.", + "tags": [ + "CommandBased Robot", + "Complete List" + ], + "foldername": "GearsBotNew", + "gradlebase": "cpp" + }, { "name": "PacGoat", "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.", @@ -267,5 +277,66 @@ ], "foldername": "ShuffleBoard", "gradlebase": "cpp" + }, + { + "name": "'Traditional' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'traditional' style, i.e. commands are given their own classes.", + "tags": [ + "Complete robot", + "Command-based" + ], + "foldername": "HatchbotTraditional", + "gradlebase": "cpp" + }, + { + "name": "'Inlined' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.", + "tags": [ + "Complete robot", + "Command-based", + "Lambdas" + ], + "foldername": "HatchbotInlined", + "gradlebase": "cpp" + }, + { + "name": "Select Command Example", + "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.", + "tags": [ + "Command-based" + ], + "foldername": "SelectCommand", + "gradlebase": "cpp" + }, + { + "name": "Scheduler Event Logging", + "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite", + "tags": [ + "Command-based", + "Shuffleboard" + ], + "foldername": "SchedulerEventLogging", + "gradlebase": "cpp" + }, + { + "name": "Frisbeebot", + "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework", + "tags": [ + "Command-based", + "PID" + ], + "foldername": "Frisbeebot", + "mainclass": "Main" + }, + { + "name": "Gyro Drive Commands", + "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.", + "tags": [ + "Command-based", + "PID", + "Gyro" + ], + "foldername": "GyroDriveCommands", + "mainclass": "Main" } ] diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp index df2a9ccaaf..6c39023fef 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,27 +7,21 @@ #include "Robot.h" -#include +#include + #include -ExampleSubsystem Robot::m_subsystem; -OI Robot::m_oi; - -void Robot::RobotInit() { - m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto); - m_chooser.AddOption("My Auto", &m_myAuto); - frc::SmartDashboard::PutData("Auto Modes", &m_chooser); -} +void Robot::RobotInit() {} /** * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, + * this for items like diagnostics that you want to run during disabled, * autonomous, teleoperated and test. * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ -void Robot::RobotPeriodic() {} +void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } /** * This function is called once each time the robot enters Disabled mode. You @@ -36,36 +30,21 @@ void Robot::RobotPeriodic() {} */ void Robot::DisabledInit() {} -void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); } +void Robot::DisabledPeriodic() {} /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable chooser - * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard, - * remove all of the chooser code and uncomment the GetString code to get the - * auto name from the text box below the Gyro. - * - * You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons to - * the if-else structure below with additional strings & commands. + * This autonomous runs the autonomous command selected by your {@link + * RobotContainer} class. */ void Robot::AutonomousInit() { - // std::string autoSelected = frc::SmartDashboard::GetString( - // "Auto Selector", "Default"); - // if (autoSelected == "My Auto") { - // m_autonomousCommand = &m_myAuto; - // } else { - // m_autonomousCommand = &m_defaultAuto; - // } - - m_autonomousCommand = m_chooser.GetSelected(); + m_autonomousCommand = m_container.GetAutonomousCommand(); if (m_autonomousCommand != nullptr) { - m_autonomousCommand->Start(); + m_autonomousCommand->Schedule(); } } -void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); } +void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { // This makes sure that the autonomous stops running when @@ -78,8 +57,14 @@ void Robot::TeleopInit() { } } -void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); } +/** + * This function is called periodically during operator control. + */ +void Robot::TeleopPeriodic() {} +/** + * This function is called periodically during test mode. + */ void Robot::TestPeriodic() {} #ifndef RUNNING_FRC_TESTS diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp new file mode 100644 index 0000000000..8210645c8b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "RobotContainer.h" + +RobotContainer::RobotContainer() : m_autonomousCommand(&m_subsystem) { + // Initialize all of your commands and subsystems here + + // Configure the button bindings + ConfigureButtonBindings(); +} + +void RobotContainer::ConfigureButtonBindings() { + // Configure your button bindings here +} + +frc2::Command* RobotContainer::GetAutonomousCommand() { + // An example command will be run in autonomous + return &m_autonomousCommand; +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp index fa83682d74..0e709aa6c3 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,25 +7,5 @@ #include "commands/ExampleCommand.h" -#include "Robot.h" - -ExampleCommand::ExampleCommand() { - // Use Requires() here to declare subsystem dependencies - Requires(&Robot::m_subsystem); -} - -// Called just before this Command runs the first time -void ExampleCommand::Initialize() {} - -// Called repeatedly when this Command is scheduled to run -void ExampleCommand::Execute() {} - -// Make this return true when this Command no longer needs to run execute() -bool ExampleCommand::IsFinished() { return false; } - -// Called once after isFinished returns true -void ExampleCommand::End() {} - -// Called when another command which requires one or more of the same -// subsystems is scheduled to run -void ExampleCommand::Interrupted() {} +ExampleCommand::ExampleCommand(ExampleSubsystem* subsystem) + : m_subsystem{subsystem} {} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp index cdde203c22..2e720c9a7c 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,14 +7,10 @@ #include "subsystems/ExampleSubsystem.h" -#include "RobotMap.h" - -ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {} - -void ExampleSubsystem::InitDefaultCommand() { - // Set the default command for a subsystem here. - // SetDefaultCommand(new MySpecialCommand()); +ExampleSubsystem::ExampleSubsystem() { + // Implementation of subsystem constructor goes here. } -// Put methods for controlling this subsystem -// here. Call these from Commands. +void ExampleSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. +} diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h new file mode 100644 index 0000000000..5ac2de033c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h @@ -0,0 +1,18 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This should not be used for any other + * purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h index 7dd5093369..872d72a57a 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,20 +7,14 @@ #pragma once -#include -#include -#include +#include -#include "OI.h" -#include "commands/ExampleCommand.h" -#include "commands/MyAutoCommand.h" -#include "subsystems/ExampleSubsystem.h" +#include + +#include "RobotContainer.h" class Robot : public frc::TimedRobot { public: - static ExampleSubsystem m_subsystem; - static OI m_oi; - void RobotInit() override; void RobotPeriodic() override; void DisabledInit() override; @@ -34,8 +28,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc::Command* m_autonomousCommand = nullptr; - ExampleCommand m_defaultAuto; - MyAutoCommand m_myAuto; - frc::SendableChooser m_chooser; + frc2::Command* m_autonomousCommand = nullptr; + + RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h new file mode 100644 index 0000000000..46609acae7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "commands/ExampleCommand.h" +#include "subsystems/ExampleSubsystem.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 RobotContainer { + public: + RobotContainer(); + + frc2::Command* GetAutonomousCommand(); + + private: + // The robot's subsystems and commands are defined here... + ExampleSubsystem m_subsystem; + ExampleCommand m_autonomousCommand; + + void ConfigureButtonBindings(); +}; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h deleted file mode 100644 index dd78a21843..0000000000 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h +++ /dev/null @@ -1,25 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -/** - * The RobotMap is a mapping from the ports sensors and actuators are wired into - * to a variable name. This provides flexibility changing wiring, makes checking - * the wiring easier and significantly reduces the number of magic numbers - * floating around. - */ - -// For example to map the left and right motors, you could define the -// following variables to use with your drivetrain subsystem. -// constexpr int kLeftMotor = 1; -// constexpr int kRightMotor = 2; - -// If you are using multiple modules, make sure to define both the port -// number and the module. For example you with a rangefinder: -// constexpr int kRangeFinderPort = 1; -// constexpr int kRangeFinderModule = 1; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h index 1d11728dad..c36477f930 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,14 +7,28 @@ #pragma once -#include +#include +#include -class ExampleCommand : public frc::Command { +#include "subsystems/ExampleSubsystem.h" + +/** + * An example command that uses an example subsystem. + * + *

Note that this extends CommandHelper, rather extending CommandBase + * directly; this is crucially important, or else the decorator functions in + * Command will *not* work! + */ +class ExampleCommand + : public frc2::CommandHelper { public: - ExampleCommand(); - void Initialize() override; - void Execute() override; - bool IsFinished() override; - void End() override; - void Interrupted() override; + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + explicit ExampleCommand(ExampleSubsystem* subsystem); + + private: + ExampleSubsystem* m_subsystem; }; diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h index 66bc329cf4..763eafdac0 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,14 +7,20 @@ #pragma once -#include +#include -class ExampleSubsystem : public frc::Subsystem { +class ExampleSubsystem : public frc2::SubsystemBase { public: ExampleSubsystem(); - void InitDefaultCommand() override; + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. private: - // It's desirable that everything possible under private except - // for methods that implement subsystem capabilities + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. };