Add intake control loop.
Change-Id: I504e4b11c0d8424249e2544da517748a657ddef8
diff --git a/bot3/control_loops/intake/intake.cc b/bot3/control_loops/intake/intake.cc
new file mode 100644
index 0000000..935730b
--- /dev/null
+++ b/bot3/control_loops/intake/intake.cc
@@ -0,0 +1,36 @@
+#include "bot3/control_loops/intake/intake.h"
+
+#include "bot3/control_loops/intake/intake.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+Intake::Intake(control_loops::IntakeQueue *intake)
+ : aos::controls::ControlLoop<control_loops::IntakeQueue>(intake) {}
+
+void Intake::RunIteration(
+ const control_loops::IntakeQueue::Goal *goal,
+ const control_loops::IntakeQueue::Position * /*position*/,
+ control_loops::IntakeQueue::Output *output,
+ control_loops::IntakeQueue::Status * /*status*/) {
+
+ if (output != nullptr) {
+ output->Zero();
+
+ const int16_t intake_movement = goal->movement;
+
+ if (intake_movement > 0) {
+ // Suck.
+ output->intake = kIntakeVoltageFullPower;
+ } else if (intake_movement < 0) {
+ // Spit.
+ output->intake = -kIntakeVoltageFullPower;
+ } else {
+ // Stationary.
+ output->intake = 0.0;
+ }
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/intake/intake.gyp b/bot3/control_loops/intake/intake.gyp
new file mode 100644
index 0000000..41f1b7f
--- /dev/null
+++ b/bot3/control_loops/intake/intake.gyp
@@ -0,0 +1,60 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'intake_queue',
+ 'type': 'static_library',
+ 'sources': ['intake.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/intake',
+ },
+ 'dependencies': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/controls/controls.gyp:control_loop_queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'intake_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'intake.cc',
+ ],
+ 'dependencies': [
+ 'intake_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ 'intake_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ },
+ {
+ 'target_name': 'intake_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'intake_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'intake_lib',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
+ '<(AOS)/common/common.gyp:time',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ ],
+ },
+ {
+ 'target_name': 'intake',
+ 'type': 'executable',
+ 'sources': [
+ 'intake_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'intake_lib',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/intake/intake.h b/bot3/control_loops/intake/intake.h
new file mode 100644
index 0000000..43cd9fc
--- /dev/null
+++ b/bot3/control_loops/intake/intake.h
@@ -0,0 +1,28 @@
+#ifndef BOT3_CONTROL_LOOPS_INTAKE_H_
+#define BOT3_CONTROL_LOOPS_INTAKE_H_
+
+#include "aos/common/controls/control_loop.h"
+
+#include "bot3/control_loops/intake/intake.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+constexpr double kIntakeVoltageFullPower = 12.0;
+
+class Intake : public aos::controls::ControlLoop<control_loops::IntakeQueue> {
+ public:
+ explicit Intake(
+ control_loops::IntakeQueue *intake_queue = &control_loops::intake_queue);
+
+ protected:
+ void RunIteration(const control_loops::IntakeQueue::Goal *goal,
+ const control_loops::IntakeQueue::Position *position,
+ control_loops::IntakeQueue::Output *output,
+ control_loops::IntakeQueue::Status *status) override;
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_INTAKE_H_
diff --git a/bot3/control_loops/intake/intake.q b/bot3/control_loops/intake/intake.q
new file mode 100644
index 0000000..3047f6b
--- /dev/null
+++ b/bot3/control_loops/intake/intake.q
@@ -0,0 +1,27 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+queue_group IntakeQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Positive = suck, negative = spit, zero = stationary.
+ int16_t movement;
+ };
+
+ message Position {};
+
+ message Output {
+ double intake;
+ };
+
+ message Status {};
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IntakeQueue intake_queue;
diff --git a/bot3/control_loops/intake/intake_lib_test.cc b/bot3/control_loops/intake/intake_lib_test.cc
new file mode 100644
index 0000000..6fe6672
--- /dev/null
+++ b/bot3/control_loops/intake/intake_lib_test.cc
@@ -0,0 +1,123 @@
+#include "bot3/control_loops/intake/intake.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "bot3/control_loops/intake/intake.q.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the elevator and sends out queue messages with the
+// position.
+class IntakeSimulation {
+ public:
+ // Constructs a simulation.
+ IntakeSimulation()
+ : queue_(".bot3.control_loops.intake_queue", 0x627ceeeb,
+ ".bot3.control_loops.intake_queue.goal",
+ ".bot3.control_loops.intake_queue.position",
+ ".bot3.control_loops.intake_queue.output",
+ ".bot3.control_loops.intake_queue.status") {
+ }
+
+ // Simulates for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(queue_.output.FetchLatest());
+ }
+
+ private:
+ IntakeQueue queue_;
+};
+
+class IntakeTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ IntakeTest()
+ : queue_(".bot3.control_loops.intake_queue", 0x627ceeeb,
+ ".bot3.control_loops.intake_queue.goal",
+ ".bot3.control_loops.intake_queue.position",
+ ".bot3.control_loops.intake_queue.output",
+ ".bot3.control_loops.intake_queue.status"),
+ intake_(&queue_),
+ plant_() {
+ set_team_id(971);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ queue_.position.MakeMessage().Send();
+
+ intake_.Iterate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to shared memory that
+ // is no longer valid.
+ IntakeQueue queue_;
+
+ // Create a control loop.
+ Intake intake_;
+ IntakeSimulation plant_;
+};
+
+// Tests that the loop does nothing when the goal is zero.
+TEST_F(IntakeTest, DoesNothing) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .movement(0)
+ .Send());
+
+ // Run for a bit.
+ RunForTime(1.0);
+
+ ASSERT_TRUE(queue_.output.FetchLatest());
+ EXPECT_EQ(queue_.output->intake, 0.0);
+}
+
+// Tests that the intake sucks with a positive goal.
+TEST_F(IntakeTest, SuckingGoal) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .movement(1)
+ .Send());
+
+ // Run for a bit.
+ RunForTime(1.0);
+
+ ASSERT_TRUE(queue_.output.FetchLatest());
+ EXPECT_GT(queue_.output->intake, 0.0);
+}
+
+// Tests that the intake spits with a negative goal.
+TEST_F(IntakeTest, SpittingGoal) {
+ ASSERT_TRUE(queue_.goal.MakeWithBuilder()
+ .movement(-1)
+ .Send());
+
+ // Run for a bit.
+ RunForTime(1.0);
+
+ ASSERT_TRUE(queue_.output.FetchLatest());
+ EXPECT_LT(queue_.output->intake, 0.0);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/intake/intake_main.cc b/bot3/control_loops/intake/intake_main.cc
new file mode 100644
index 0000000..ccc5fab
--- /dev/null
+++ b/bot3/control_loops/intake/intake_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/intake/intake.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::bot3::control_loops::Intake intake;
+ intake.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
index 1283b33..f0a0e27 100644
--- a/bot3/prime/prime.gyp
+++ b/bot3/prime/prime.gyp
@@ -11,7 +11,9 @@
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_bot3',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test_bot3',
'../control_loops/drivetrain/drivetrain.gyp:replay_drivetrain_bot3',
- '../autonomous/autonomous.gyp:auto_bot3',
+ '../control_loops/intake/intake.gyp:intake',
+ '../control_loops/intake/intake.gyp:intake_lib_test',
+ #'../autonomous/autonomous.gyp:auto_bot3',
'../bot3.gyp:joystick_reader_bot3',
'<(DEPTH)/frc971/zeroing/zeroing.gyp:zeroing_test',
#'../http_status/http_status.gyp:http_status',