Moved bot3 to y2015_bot3
Change-Id: Idebb248ddc3ff658f2404f367dfa4a6dc421e0c1
diff --git a/y2015_bot3/control_loops/intake/BUILD b/y2015_bot3/control_loops/intake/BUILD
new file mode 100644
index 0000000..fe21b7f
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/BUILD
@@ -0,0 +1,53 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'intake_queue',
+ srcs = [
+ 'intake.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ ],
+)
+
+cc_library(
+ name = 'intake_lib',
+ srcs = [
+ 'intake.cc',
+ ],
+ hdrs = [
+ 'intake.h',
+ ],
+ deps = [
+ ':intake_queue',
+ '//aos/common/controls:control_loop',
+ ],
+)
+
+cc_test(
+ name = 'intake_lib_test',
+ srcs = [
+ 'intake_lib_test.cc',
+ ],
+ deps = [
+ '//third_party/googletest',
+ ':intake_lib',
+ '//frc971/control_loops:state_feedback_loop',
+ '//aos/common/controls:control_loop_test',
+ '//aos/common:time',
+ '//frc971/control_loops:team_number_test_environment',
+ ],
+)
+
+cc_binary(
+ name = 'intake',
+ srcs = [
+ 'intake_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':intake_lib',
+ ],
+)
diff --git a/y2015_bot3/control_loops/intake/intake.cc b/y2015_bot3/control_loops/intake/intake.cc
new file mode 100644
index 0000000..51bab21
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/intake.cc
@@ -0,0 +1,30 @@
+#include "y2015_bot3/control_loops/intake/intake.h"
+
+#include "y2015_bot3/control_loops/intake/intake.q.h"
+
+namespace y2015_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();
+
+ if (goal != nullptr) {
+ output->intake = goal->movement;
+ output->claw_closed = goal->claw_closed;
+ } else {
+ output->intake = 0.0;
+ output->claw_closed = false;
+ }
+ }
+}
+
+} // namespace control_loops
+} // namespace y2015_bot3
diff --git a/y2015_bot3/control_loops/intake/intake.gyp b/y2015_bot3/control_loops/intake/intake.gyp
new file mode 100644
index 0000000..41f1b7f
--- /dev/null
+++ b/y2015_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/y2015_bot3/control_loops/intake/intake.h b/y2015_bot3/control_loops/intake/intake.h
new file mode 100644
index 0000000..120b0a3
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/intake.h
@@ -0,0 +1,28 @@
+#ifndef Y2015_BOT3_CONTROL_LOOPS_INTAKE_H_
+#define Y2015_BOT3_CONTROL_LOOPS_INTAKE_H_
+
+#include "aos/common/controls/control_loop.h"
+
+#include "y2015_bot3/control_loops/intake/intake.q.h"
+
+namespace y2015_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 y2015_bot3
+
+#endif // Y2015_BOT3_CONTROL_LOOPS_INTAKE_H_
diff --git a/y2015_bot3/control_loops/intake/intake.q b/y2015_bot3/control_loops/intake/intake.q
new file mode 100644
index 0000000..f778231
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/intake.q
@@ -0,0 +1,32 @@
+package y2015_bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+queue_group IntakeQueue {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Units: volts
+ double movement;
+
+ bool claw_closed;
+ };
+
+ message Position {};
+
+ message Output {
+ // Positive or negative, depending on whether we're sucking or spitting.
+ double intake;
+
+ bool claw_closed;
+ };
+
+ message Status {};
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IntakeQueue intake_queue;
diff --git a/y2015_bot3/control_loops/intake/intake_lib_test.cc b/y2015_bot3/control_loops/intake/intake_lib_test.cc
new file mode 100644
index 0000000..6d598fe
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/intake_lib_test.cc
@@ -0,0 +1,121 @@
+#include "y2015_bot3/control_loops/intake/intake.h"
+
+#include <math.h>
+#include <unistd.h>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2015_bot3/control_loops/intake/intake.q.h"
+
+using ::aos::time::Time;
+
+namespace y2015_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_(".y2015_bot3.control_loops.intake_queue", 0x627ceeeb,
+ ".y2015_bot3.control_loops.intake_queue.goal",
+ ".y2015_bot3.control_loops.intake_queue.position",
+ ".y2015_bot3.control_loops.intake_queue.output",
+ ".y2015_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_(".y2015_bot3.control_loops.intake_queue", 0x627ceeeb,
+ ".y2015_bot3.control_loops.intake_queue.goal",
+ ".y2015_bot3.control_loops.intake_queue.position",
+ ".y2015_bot3.control_loops.intake_queue.output",
+ ".y2015_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 y2015_bot3
diff --git a/y2015_bot3/control_loops/intake/intake_main.cc b/y2015_bot3/control_loops/intake/intake_main.cc
new file mode 100644
index 0000000..095460d
--- /dev/null
+++ b/y2015_bot3/control_loops/intake/intake_main.cc
@@ -0,0 +1,11 @@
+#include "y2015_bot3/control_loops/intake/intake.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2015_bot3::control_loops::Intake intake;
+ intake.Run();
+ ::aos::Cleanup();
+ return 0;
+}