diff --git a/wpilibc/shared/include/Commands/Command.h b/wpilibc/shared/include/Commands/Command.h index 532f09657c..19d1e7a0a1 100644 --- a/wpilibc/shared/include/Commands/Command.h +++ b/wpilibc/shared/include/Commands/Command.h @@ -81,6 +81,7 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { bool IsTimedOut() const; bool AssertUnlocked(const std::string& message); void SetParent(CommandGroup* parent); + void ClearRequirements(); virtual void Initialize(); virtual void Execute(); @@ -112,6 +113,8 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { virtual void _End(); virtual void _Cancel(); + friend class ConditionalCommand; + private: void LockChanges(); /*synchronized*/ void Removed(); diff --git a/wpilibc/shared/include/Commands/ConditionalCommand.h b/wpilibc/shared/include/Commands/ConditionalCommand.h new file mode 100644 index 0000000000..e7fcb3c836 --- /dev/null +++ b/wpilibc/shared/include/Commands/ConditionalCommand.h @@ -0,0 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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/Command.h" +#include "Commands/InstantCommand.h" + +namespace frc { + +/** + * A {@link ConditionalCommand} is a {@link Command} that starts one of two + * commands. + * + *

+ * A {@link ConditionalCommand} uses m_condition to determine whether it should + * run m_onTrue or m_onFalse. + *

+ * + *

+ * A {@link ConditionalCommand} adds the proper {@link Command} to the {@link + * Scheduler} during {@link ConditionalCommand#initialize()} and then {@link + * ConditionalCommand#isFinished()} will return true once that {@link Command} + * has finished executing. + *

+ * + *

+ * If no {@link Command} is specified for m_onFalse, the occurrence of that + * condition will be a no-op. + *

+ * + * @see Command + * @see Scheduler + */ +class ConditionalCommand : public Command { + public: + explicit ConditionalCommand(Command* onTrue, + Command* onFalse = new InstantCommand()); + ConditionalCommand(const std::string& name, Command* onTrue, + Command* onFalse = new InstantCommand()); + virtual ~ConditionalCommand() = default; + + protected: + /** + * The Condition to test to determine which Command to run. + * + * @return true if m_onTrue should be run or false if m_onFalse should be run. + */ + virtual bool Condition() = 0; + + void _Initialize() override; + void _Cancel() override; + bool IsFinished() override; + void Interrupted() override; + + private: + /** + * The Command to execute if {@link ConditionalCommand#Condition()} returns + * true + */ + Command* m_onTrue; + + /** + * The Command to execute if {@link ConditionalCommand#Condition()} returns + * false + */ + Command* m_onFalse; + + /** + * Stores command chosen by condition + */ + Command* m_chosenCommand = nullptr; +}; + +} // namespace frc diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp index 2bf20abc79..3d7d2acd91 100644 --- a/wpilibc/shared/src/Commands/Command.cpp +++ b/wpilibc/shared/src/Commands/Command.cpp @@ -303,6 +303,13 @@ void Command::SetParent(CommandGroup* parent) { } } +/** + * Clears list of subsystem requirements. This is only used by + * {@link ConditionalCommand} so cancelling the chosen command works properly in + * {@link CommandGroup}. + */ +void Command::ClearRequirements() { m_requirements.clear(); } + /** * This is used internally to mark that the command has been started. * diff --git a/wpilibc/shared/src/Commands/ConditionalCommand.cpp b/wpilibc/shared/src/Commands/ConditionalCommand.cpp new file mode 100644 index 0000000000..6d36086a50 --- /dev/null +++ b/wpilibc/shared/src/Commands/ConditionalCommand.cpp @@ -0,0 +1,84 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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/ConditionalCommand.h" + +#include "Commands/Scheduler.h" + +using namespace frc; + +/** + * Creates a new ConditionalCommand with given onTrue and onFalse Commands. + * + * @param onTrue The Command to execute if {@link + * ConditionalCommand#Condition()} returns true + * @param onFalse The Command to execute if {@link + * ConditionalCommand#Condition()} returns false + */ +ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) { + m_onTrue = onTrue; + m_onFalse = onFalse; + + for (auto requirement : m_onTrue->GetRequirements()) Requires(requirement); + for (auto requirement : m_onFalse->GetRequirements()) Requires(requirement); +} + +/** + * Creates a new ConditionalCommand with given onTrue and onFalse Commands. + * + * @param name the name for this command group + * @param onTrue The Command to execute if {@link + * ConditionalCommand#Condition()} returns true + * @param onFalse The Command to execute if {@link + * ConditionalCommand#Condition()} returns false + */ +ConditionalCommand::ConditionalCommand(const std::string& name, Command* onTrue, + Command* onFalse) + : Command(name) { + m_onTrue = onTrue; + m_onFalse = onFalse; + + for (auto requirement : m_onTrue->GetRequirements()) Requires(requirement); + for (auto requirement : m_onFalse->GetRequirements()) Requires(requirement); +} + +void ConditionalCommand::_Initialize() { + if (Condition()) { + m_chosenCommand = m_onTrue; + } else { + m_chosenCommand = m_onFalse; + } + + /* + * This is a hack to make cancelling the chosen command inside a CommandGroup + * work properly + */ + m_chosenCommand->ClearRequirements(); + + m_chosenCommand->Start(); +} + +void ConditionalCommand::_Cancel() { + if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) { + m_chosenCommand->Cancel(); + } + + Command::_Cancel(); +} + +bool ConditionalCommand::IsFinished() { + return m_chosenCommand != nullptr && m_chosenCommand->IsRunning() && + m_chosenCommand->IsFinished(); +} + +void ConditionalCommand::Interrupted() { + if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) { + m_chosenCommand->Cancel(); + } + + Command::Interrupted(); +} diff --git a/wpilibcIntegrationTests/include/command/MockConditionalCommand.h b/wpilibcIntegrationTests/include/command/MockConditionalCommand.h new file mode 100644 index 0000000000..b00911e8ff --- /dev/null +++ b/wpilibcIntegrationTests/include/command/MockConditionalCommand.h @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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 "command/MockCommand.h" + +namespace frc { + +class MockConditionalCommand : public ConditionalCommand { + public: + MockConditionalCommand(MockCommand* onTrue, MockCommand* onFalse); + void SetCondition(bool condition); + + protected: + bool Condition() override; + + private: + bool m_condition = false; +}; + +} // namespace frc diff --git a/wpilibcIntegrationTests/src/command/ConditionalCommandTest.cpp b/wpilibcIntegrationTests/src/command/ConditionalCommandTest.cpp new file mode 100644 index 0000000000..271131d483 --- /dev/null +++ b/wpilibcIntegrationTests/src/command/ConditionalCommandTest.cpp @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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 + +#include "Commands/ConditionalCommand.h" +#include "Commands/Scheduler.h" +#include "command/MockCommand.h" +#include "command/MockConditionalCommand.h" +#include "gtest/gtest.h" + +using namespace frc; + +class ConditionalCommandTest : public testing::Test { + public: + MockConditionalCommand* m_command; + MockCommand* m_onTrue; + MockCommand* m_onFalse; + + protected: + void SetUp() override { + RobotState::SetImplementation(DriverStation::GetInstance()); + Scheduler::GetInstance()->SetEnabled(true); + + m_onTrue = new MockCommand(); + m_onFalse = new MockCommand(); + m_command = new MockConditionalCommand(m_onTrue, m_onFalse); + } + + void TearDown() override { delete m_command; } + + /** + * Tears Down the Scheduler at the end of each test. + * + * Must be called at the end of each test inside each test in order to prevent + * them being deallocated when they leave the scope of the test (causing a + * segfault). This cannot be done within the virtual void Teardown() method + * because it is called outside of the scope of the test. + */ + void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); } + + void AssertCommandState(MockCommand& command, int32_t initialize, + int32_t execute, int32_t isFinished, int32_t end, + int32_t interrupted) { + EXPECT_EQ(initialize, command.GetInitializeCount()); + EXPECT_EQ(execute, command.GetExecuteCount()); + EXPECT_EQ(isFinished, command.GetIsFinishedCount()); + EXPECT_EQ(end, command.GetEndCount()); + EXPECT_EQ(interrupted, command.GetInterruptedCount()); + } +}; + +TEST_F(ConditionalCommandTest, OnTrueTest) { + m_command->SetCondition(true); + + Scheduler::GetInstance()->AddCommand(m_command); + AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); // init command and select m_onTrue + AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); // init m_onTrue + AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); + AssertCommandState(*m_onTrue, 1, 1, 2, 0, 0); + Scheduler::GetInstance()->Run(); + AssertCommandState(*m_onTrue, 1, 2, 4, 0, 0); + + EXPECT_TRUE(m_onTrue->GetInitializeCount() > 0) + << "Did not initialize the true command\n"; + EXPECT_TRUE(m_onFalse->GetInitializeCount() == 0) + << "Initialized the false command\n"; + + TeardownScheduler(); +} + +TEST_F(ConditionalCommandTest, OnFalseTest) { + m_command->SetCondition(false); + + Scheduler::GetInstance()->AddCommand(m_command); + AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); // init command and select m_onTrue + AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); // init m_onTrue + AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0); + Scheduler::GetInstance()->Run(); + AssertCommandState(*m_onFalse, 1, 1, 2, 0, 0); + Scheduler::GetInstance()->Run(); + AssertCommandState(*m_onFalse, 1, 2, 4, 0, 0); + + EXPECT_TRUE(m_onFalse->GetInitializeCount() > 0) + << "Did not initialize the false command"; + EXPECT_TRUE(m_onTrue->GetInitializeCount() == 0) + << "Initialized the true command"; + + TeardownScheduler(); +} diff --git a/wpilibcIntegrationTests/src/command/MockConditionalCommand.cpp b/wpilibcIntegrationTests/src/command/MockConditionalCommand.cpp new file mode 100644 index 0000000000..b23b815908 --- /dev/null +++ b/wpilibcIntegrationTests/src/command/MockConditionalCommand.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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 "command/MockConditionalCommand.h" + +using namespace frc; + +MockConditionalCommand::MockConditionalCommand(MockCommand* onTrue, + MockCommand* onFalse) + : ConditionalCommand(onTrue, onFalse) {} + +void MockConditionalCommand::SetCondition(bool condition) { + m_condition = condition; +} + +bool MockConditionalCommand::Condition() { return m_condition; } diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Command.java index 9c3b276e99..8c99e06852 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Command.java +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Command.java @@ -386,6 +386,15 @@ public abstract class Command implements NamedSendable { } } + /** + * Clears list of subsystem requirements. This is only used by + * {@link ConditionalCommand} so cancelling the chosen command works properly + * in {@link CommandGroup}. + */ + protected void clearRequirements() { + m_requirements = new Set(); + } + /** * Starts up the command. Gets the command ready to start.

Note that the command will * eventually start, however it will not necessarily do so immediately, and may in fact be diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java new file mode 100644 index 0000000000..f5c443363e --- /dev/null +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java @@ -0,0 +1,164 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.command; + +import java.util.Enumeration; + +/** + * A {@link ConditionalCommand} is a {@link Command} that starts one of two commands. + * + *

+ * A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or + * m_onFalse. + *

+ * + *

+ * A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during + * {@link ConditionalCommand#initialize()} and then {@link ConditionalCommand#isFinished()} will + * return true once that {@link Command} has finished executing. + *

+ * + *

+ * If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a + * no-op. + *

+ * + * @see Command + * @see Scheduler + */ +public abstract class ConditionalCommand extends Command { + /** + * The Command to execute if {@link ConditionalCommand#condition()} returns true. + */ + private Command m_onTrue; + + /** + * The Command to execute if {@link ConditionalCommand#condition()} returns false. + */ + private Command m_onFalse; + + /** + * Stores command chosen by condition. + */ + private Command m_chosenCommand = null; + + /** + * Creates a new ConditionalCommand with given onTrue and onFalse Commands. + * + *

Users of this constructor should also override condition(). + * + * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true + */ + public ConditionalCommand(Command onTrue) { + this(onTrue, new InstantCommand()); + } + + /** + * Creates a new ConditionalCommand with given onTrue and onFalse Commands. + * + *

Users of this constructor should also override condition(). + * + * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true + * @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false + */ + public ConditionalCommand(Command onTrue, Command onFalse) { + m_onTrue = onTrue; + m_onFalse = onFalse; + + for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) { + requires((Subsystem) e.nextElement()); + } + + for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) { + requires((Subsystem) e.nextElement()); + } + } + + /** + * Creates a new ConditionalCommand with given name and onTrue and onFalse Commands. + * + *

Users of this constructor should also override condition(). + * + * @param name the name for this command group + * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true + */ + public ConditionalCommand(String name, Command onTrue) { + this(name, onTrue, new InstantCommand()); + } + + /** + * Creates a new ConditionalCommand with given name and onTrue and onFalse Commands. + * + *

Users of this constructor should also override condition(). + * + * @param name the name for this command group + * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true + * @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false + */ + public ConditionalCommand(String name, Command onTrue, Command onFalse) { + super(name); + m_onTrue = onTrue; + m_onFalse = onFalse; + + for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) { + requires((Subsystem) e.nextElement()); + } + + for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) { + requires((Subsystem) e.nextElement()); + } + } + + /** + * The Condition to test to determine which Command to run. + * + * @return true if m_onTrue should be run or false if m_onFalse should be run. + */ + protected abstract boolean condition(); + + /** + * Calls {@link ConditionalCommand#condition()} and runs the proper command. + */ + @Override + protected void _initialize() { + if (condition()) { + m_chosenCommand = m_onTrue; + } else { + m_chosenCommand = m_onFalse; + } + + // This is a hack to make cancelling the chosen command inside a CommandGroup work properly + m_chosenCommand.clearRequirements(); + + m_chosenCommand.start(); + } + + @Override + protected void _cancel() { + if (m_chosenCommand != null && m_chosenCommand.isRunning()) { + m_chosenCommand.cancel(); + } + + super._cancel(); + } + + @Override + protected boolean isFinished() { + return m_chosenCommand != null && m_chosenCommand.isRunning() + && m_chosenCommand.isFinished(); + } + + @Override + protected void interrupted() { + if (m_chosenCommand != null && m_chosenCommand.isRunning()) { + m_chosenCommand.cancel(); + } + + super.interrupted(); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java new file mode 100644 index 0000000000..d34c2f45ea --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.command; + +import static org.junit.Assert.assertTrue; + +import org.junit.Before; +import org.junit.Test; + +public class ConditionalCommandTest extends AbstractCommandTest { + MockConditionalCommand m_command; + MockCommand m_onTrue; + MockCommand m_onFalse; + Boolean m_condition; + + @Before + public void initCommands() { + m_onTrue = new MockCommand(); + m_onFalse = new MockCommand(); + m_command = new MockConditionalCommand(m_onTrue, m_onFalse); + } + + @Test + public void testOnTrue() { + m_command.setCondition(true); + + Scheduler.getInstance().add(m_command); + assertCommandState(m_onTrue, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); // init command and select m_onTrue + assertCommandState(m_onTrue, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); // init m_onTrue + assertCommandState(m_onTrue, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); + assertCommandState(m_onTrue, 1, 1, 2, 0, 0); + Scheduler.getInstance().run(); + assertCommandState(m_onTrue, 1, 2, 4, 0, 0); + + assertTrue("Did not initialize the true command", m_onTrue.getInitializeCount() > 0); + assertTrue("Initialized the false command", m_onFalse.getInitializeCount() == 0); + } + + @Test + public void testOnFalse() { + m_command.setCondition(false); + + Scheduler.getInstance().add(m_command); + assertCommandState(m_onFalse, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); // init command and select m_onFalse + assertCommandState(m_onFalse, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); // init m_onFalse + assertCommandState(m_onFalse, 0, 0, 0, 0, 0); + Scheduler.getInstance().run(); + assertCommandState(m_onFalse, 1, 1, 2, 0, 0); + Scheduler.getInstance().run(); + assertCommandState(m_onFalse, 1, 2, 4, 0, 0); + + assertTrue("Did not initialize the false command", m_onFalse.getInitializeCount() > 0); + assertTrue("Initialized the true command", m_onTrue.getInitializeCount() == 0); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java new file mode 100644 index 0000000000..61d70a3056 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.command; + +public class MockConditionalCommand extends ConditionalCommand { + private boolean m_condition = false; + + public MockConditionalCommand(MockCommand onTrue, MockCommand onFalse) { + super(onTrue, onFalse); + } + + @Override + protected boolean condition() { + return m_condition; + } + + public void setCondition(boolean condition) { + this.m_condition = condition; + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/command/CommandTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/command/CommandTestSuite.java index 9870df1742..c66f2fe6c9 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/command/CommandTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/command/CommandTestSuite.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; @RunWith(Suite.class) @SuiteClasses({ButtonTest.class, CommandParallelGroupTest.class, CommandScheduleTest.class, CommandSequentialGroupTest.class, CommandSupersedeTest.class, CommandTimeoutTest.class, - DefaultCommandTest.class}) + ConditionalCommandTest.class, DefaultCommandTest.class}) public class CommandTestSuite extends AbstractTestSuite { }