Adding a simple codelab. Hopefully this can be useful in training.
Change-Id: I3bab489d66969e78fd527c7f8f4361228bb20be9
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
new file mode 100644
index 0000000..2823200
--- /dev/null
+++ b/frc971/codelab/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//visibility:public'])
+
+load('//aos/build:queues.bzl', 'queue_library')
+
+cc_binary(
+ name = 'basic_test',
+ srcs = ['basic_test.cc'],
+ deps = [
+ ':basic_queue',
+ ':basic',
+ '//aos/testing:googletest',
+ '//aos/common:queues',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:team_number_test_environment',
+ ],
+ testonly = 1,
+)
+
+cc_library(
+ name = 'basic',
+ srcs = ['basic.cc'],
+ hdrs = ['basic.h'],
+ deps = [
+ ':basic_queue',
+ '//aos/common/controls:control_loop',
+ ],
+)
+
+queue_library(
+ name = 'basic_queue',
+ srcs = [
+ 'basic.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:queues',
+ ],
+)
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
new file mode 100644
index 0000000..536384c
--- /dev/null
+++ b/frc971/codelab/basic.cc
@@ -0,0 +1,22 @@
+#include "frc971/codelab/basic.h"
+
+namespace frc971 {
+namespace codelab {
+
+Basic::Basic(BasicQueue *my_basic_queue)
+ : aos::controls::ControlLoop<BasicQueue>(my_basic_queue) {}
+
+void Basic::RunIteration(const BasicQueue::Goal *goal,
+ const BasicQueue::Position *position,
+ BasicQueue::Output *output,
+ BasicQueue::Status *status) {
+ // TODO(you): Set the intake_voltage to 12 Volts when
+ // intake is requested (via intake in goal). Make sure not to set
+ // the motor to anything but 0 V when the limit_sensor is pressed.
+
+ // Ignore: These are to avoid clang warnings.
+ (void)goal, (void)position, (void)output, (void)status;
+}
+
+} // namespace codelab
+} // namespace frc971
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
new file mode 100644
index 0000000..b67fdbd
--- /dev/null
+++ b/frc971/codelab/basic.h
@@ -0,0 +1,58 @@
+#ifndef FRC971_CODELAB_BASIC_H_
+#define FRC971_CODELAB_BASIC_H_
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+
+#include "frc971/codelab/basic.q.h"
+
+namespace frc971 {
+namespace codelab {
+
+// This codelab helps build basic knowledge of how to use 971 control loop
+// primatives.
+//
+// The meat of the task is to make the tests pass.
+//
+// Run the tests with:
+// $ bazel run //frc971/codelab:basic_test -- --gtest_color=yes
+//
+// Control loops all follow the same convention:
+// There are 4 queues (goal, position, status, output).
+//
+// 2 queues are input queues: goal, position.
+// 2 queues are output queues: output, status.
+//
+// ::aos::controls::ControlLoop is a helper class that takes
+// a queue_group type from a .h file, and organizes to call
+// RunIteration() at a consistent interval. It will fetch from
+// goal and position messages from the goal and position queue,
+// and publish an output and status result to the output and status
+// queues.
+//
+// The basic.q file will construct boilerplate c++ code for the
+// Goal, Position, Status, Message
+// types, and construct static variables for fetching these named queues.
+// Inquisitive souls can check out:
+// $ bazel build //frc971/codelab:basic_queue
+// $ vim bazel-genfiles/frc971/codelab/basic.q.{cc,h} -o
+// from the 971-Robot-Code directory.
+//
+// Order of approaching this should be:
+// - Read the BUILD file and learn about what code is being generated.
+// - Read basic.q, and familiarize yourself on the inputs and types involved.
+class Basic : public ::aos::controls::ControlLoop<BasicQueue> {
+ public:
+ explicit Basic(BasicQueue *my_basic_queue = &basic_queue);
+
+ protected:
+ void RunIteration(const BasicQueue::Goal *goal,
+ const BasicQueue::Position *position,
+ BasicQueue::Output *output,
+ BasicQueue::Status *status) override;
+};
+
+} // namespace codelab
+} // namespace frc971
+
+#endif // FRC971_CODELAB_BASIC_H_
diff --git a/frc971/codelab/basic.q b/frc971/codelab/basic.q
new file mode 100644
index 0000000..8183271
--- /dev/null
+++ b/frc971/codelab/basic.q
@@ -0,0 +1,44 @@
+package frc971.codelab;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+// The theme of this basic test is a simple intake system.
+//
+// The system will have a motor driven by the voltage returned
+// by output, and then eventually this motor, when run enough,
+// will trigger the limit_sensor. The hypothetical motor should shut
+// off in that hypothetical situation to avoid hypothetical burnout.
+queue_group BasicQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The control loop needs to intake now.
+ bool intake;
+ };
+
+ message Position {
+ // This is a potential incoming sensor value letting us know
+ // if we need to be intaking.
+ bool limit_sensor;
+ };
+
+ message Status {
+ // Lets consumers of basic_queue.status know if
+ // the requested intake is finished.
+ bool intake_complete;
+ };
+
+ message Output {
+ // This would be set up to drive a hypothetical motor that would
+ // hope to intake something.
+ double intake_voltage;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group BasicQueue basic_queue;
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
new file mode 100644
index 0000000..0acf9e7
--- /dev/null
+++ b/frc971/codelab/basic_test.cc
@@ -0,0 +1,170 @@
+#include "frc971/codelab/basic.h"
+
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/queue.h"
+#include "frc971/codelab/basic.q.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace codelab {
+namespace testing {
+
+// Class which simulates stuff and sends out queue messages with the
+// position.
+class BasicSimulation {
+ public:
+ BasicSimulation()
+ : basic_queue_(".frc971.codelab.basic_queue", 0x78d8e372,
+ ".frc971.codelab.basic_queue.goal",
+ ".frc971.codelab.basic_queue.position",
+ ".frc971.codelab.basic_queue.output",
+ ".frc971.codelab.basic_queue.status") {}
+
+ // Sends a queue message with the position data.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<BasicQueue::Position> position =
+ basic_queue_.position.MakeMessage();
+
+ position->limit_sensor = limit_sensor_;
+
+ position.Send();
+ }
+
+ void VerifyResults(double voltage, bool status) {
+ basic_queue_.output.FetchLatest();
+ basic_queue_.status.FetchLatest();
+
+ ASSERT_TRUE(basic_queue_.output.get() != nullptr);
+ ASSERT_TRUE(basic_queue_.status.get() != nullptr);
+
+ EXPECT_EQ(basic_queue_.output->intake_voltage, voltage);
+ EXPECT_EQ(basic_queue_.status->intake_complete, status);
+ }
+
+ void set_limit_sensor(bool value) { limit_sensor_ = value; }
+
+ // Simulates basic control loop for a single timestep.
+ void Simulate() { EXPECT_TRUE(basic_queue_.output.FetchLatest()); }
+
+ private:
+ BasicQueue basic_queue_;
+ bool limit_sensor_ = false;
+};
+
+class BasicControlLoopTest : public ::aos::testing::ControlLoopTest {
+ public:
+ BasicControlLoopTest()
+ : basic_queue_(".frc971.codelab.basic_queue", 0x78d8e372,
+ ".frc971.codelab.basic_queue.goal",
+ ".frc971.codelab.basic_queue.position",
+ ".frc971.codelab.basic_queue.output",
+ ".frc971.codelab.basic_queue.status"),
+ basic_loop_(&basic_queue_) {
+ set_team_id(control_loops::testing::kTeamNumber);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ basic_simulation_.SendPositionMessage();
+ basic_loop_.Iterate();
+ basic_simulation_.Simulate();
+
+ TickTime();
+ }
+
+ BasicQueue basic_queue_;
+ Basic basic_loop_;
+ BasicSimulation basic_simulation_;
+};
+
+// Tests that when the motor has finished intaking it stops.
+TEST_F(BasicControlLoopTest, IntakeLimitTransitionsToTrue) {
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ basic_simulation_.set_limit_sensor(false);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(12.0, false);
+
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ basic_simulation_.set_limit_sensor(true);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(0.0, true);
+}
+
+// Tests that the intake goes on if the sensor is not pressed
+// and intake is requested.
+TEST_F(BasicControlLoopTest, IntakeLimitNotSet) {
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ basic_simulation_.set_limit_sensor(false);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(12.0, false);
+}
+
+// Tests that the intake is off if no intake is requested,
+// even if the limit sensor is off.
+TEST_F(BasicControlLoopTest, NoIntakeLimitNotSet) {
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+ basic_simulation_.set_limit_sensor(false);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the intake is off even if the limit sensor
+// is pressed and intake is requested.
+TEST_F(BasicControlLoopTest, IntakeLimitSet) {
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ basic_simulation_.set_limit_sensor(true);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(0.0, true);
+}
+
+// Tests that the intake is off if no intake is requested,
+TEST_F(BasicControlLoopTest, NoIntakeLimitSet) {
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+ basic_simulation_.set_limit_sensor(true);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the control loop handles things properly if no goal is set.
+TEST_F(BasicControlLoopTest, NoGoalSet) {
+ basic_simulation_.set_limit_sensor(true);
+
+ RunIteration();
+
+ basic_simulation_.VerifyResults(0.0, false);
+}
+
+// Tests that the control loop handles things properly if disabled.
+TEST_F(BasicControlLoopTest, Disabled) {
+ basic_simulation_.set_limit_sensor(true);
+
+ ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+
+ RunIteration(false);
+
+ basic_simulation_.VerifyResults(0.0, false);
+}
+
+} // namespace testing
+} // namespace codelab
+} // namespace frc971