Add rollers control loop.
The rollers are a separate control loop because Austin wanted
Comran to get practice writing them.
This change contains modifications that Comran made to the rollers
control loop. There was a lot of cleaning up and fixing by me
as well.
Change-Id: I3d012fb8a9652c0b85ed27f5d23fe7d63bb977ce
diff --git a/bot3/control_loops/rollers/rollers.cc b/bot3/control_loops/rollers/rollers.cc
new file mode 100644
index 0000000..8777549
--- /dev/null
+++ b/bot3/control_loops/rollers/rollers.cc
@@ -0,0 +1,74 @@
+#include "bot3/control_loops/rollers/rollers.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+void RollersLoop::RunIteration(const Rollers::Goal *goal,
+ const Rollers::Position * /*position*/,
+ Rollers::Output *output,
+ Rollers::Status * /*status*/) {
+ constexpr double kBot3IntakeForwardVoltage = 12.0;
+ constexpr double kBot3IntakeBackwardVoltage = -12.0;
+ constexpr double kBot3LowGoalForwardVoltage = 6.0;
+ constexpr double kBot3LowGoalBackwardVoltage = -6.0;
+
+ const int intake = goal->intake;
+ const int low_spit = goal->low_spit;
+ const bool human_player = goal->human_player;
+
+ if (!output) {
+ return;
+ }
+
+ output->Zero();
+
+ switch (low_spit) {
+ case 1:
+ // Spit towards front
+ output->low_goal_voltage = kBot3LowGoalBackwardVoltage;
+ output->front_intake_voltage = kBot3IntakeBackwardVoltage;
+ output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ break;
+ case -1:
+ // Spit towards back
+ output->low_goal_voltage = kBot3LowGoalForwardVoltage;
+ output->back_intake_voltage = -kBot3IntakeBackwardVoltage;
+ output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ break;
+ default:
+ // Stationary
+ break;
+ }
+
+ switch (intake) {
+ case 1:
+ // Front intake.
+ output->front_extended = true;
+ output->back_extended = false;
+ output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->back_intake_voltage = 0.0;
+ break;
+ case -1:
+ // Back intake.
+ output->back_extended = true;
+ output->front_extended = false;
+ output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->front_intake_voltage = 0.0;
+ break;
+ default:
+ // Stationary
+ break;
+ }
+
+ if (human_player) {
+ // Intake for human player.
+ output->front_extended = false;
+ output->back_extended = false;
+ output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/rollers/rollers.gyp b/bot3/control_loops/rollers/rollers.gyp
new file mode 100644
index 0000000..e865ed8
--- /dev/null
+++ b/bot3/control_loops/rollers/rollers.gyp
@@ -0,0 +1,45 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'rollers_loop',
+ 'type': 'static_library',
+ 'sources': ['rollers.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/rollers',
+ },
+ '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': 'rollers_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'rollers.cc',
+ ],
+ 'dependencies': [
+ 'rollers_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ 'rollers_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ },
+ {
+ 'target_name': 'rollers',
+ 'type': 'executable',
+ 'sources': [
+ 'rollers_main.cc',
+ ],
+ 'dependencies': [
+ 'rollers_lib',
+ '<(AOS)/linux_code/linux_code.gyp:init'
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/rollers/rollers.h b/bot3/control_loops/rollers/rollers.h
new file mode 100644
index 0000000..f67b372
--- /dev/null
+++ b/bot3/control_loops/rollers/rollers.h
@@ -0,0 +1,33 @@
+#ifndef BOT3_CONTROL_LOOPS_ROLLERS_H_
+#define BOT3_CONTROL_LOOPS_ROLLERS_H_
+
+#include "aos/common/controls/control_loop.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class RollersLoop
+ : public aos::controls::ControlLoop<control_loops::Rollers,
+ false, false, true> {
+ public:
+ // Constructs a control loops which can take a rollers or defaults to the
+ // rollers at ::bot3::control_loops::rollers.
+ explicit RollersLoop(
+ control_loops::Rollers *my_rollers = &control_loops::rollers)
+ : aos::controls::ControlLoop<control_loops::Rollers, false, false, true>(
+ my_rollers) {}
+
+ protected:
+ // Executes one cycle of the control loop.
+ virtual void RunIteration(
+ const control_loops::Rollers::Goal *goal,
+ const control_loops::Rollers::Position *position,
+ control_loops::Rollers::Output *output,
+ control_loops::Rollers::Status *status);
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // BOT3_CONTROL_LOOPS_ROLLERS_H_
diff --git a/bot3/control_loops/rollers/rollers.q b/bot3/control_loops/rollers/rollers.q
new file mode 100644
index 0000000..5abb488
--- /dev/null
+++ b/bot3/control_loops/rollers/rollers.q
@@ -0,0 +1,40 @@
+package bot3.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+queue_group Rollers {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // -1 = back intake, 1 = front intake, all else = stationary.
+ int16_t intake;
+ // -1 = backwards, 1 = forwards, all else = stationary.
+ int16_t low_spit;
+ // Whether we want the human player load function.
+ bool human_player;
+ };
+
+ message Position {};
+
+ message Output {
+ // Positive voltage = intaking, Negative = spitting.
+ double front_intake_voltage;
+ double back_intake_voltage;
+ // Voltage for the low goal rollers.
+ // Positive voltage = ball towards back, Negative = ball towards front.
+ double low_goal_voltage;
+
+ // Whether the front and back intake pistons are extended.
+ bool front_extended;
+ bool back_extended;
+ };
+
+ message Status {};
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group Rollers rollers;
diff --git a/bot3/control_loops/rollers/rollers_main.cc b/bot3/control_loops/rollers/rollers_main.cc
new file mode 100644
index 0000000..75d2cc6
--- /dev/null
+++ b/bot3/control_loops/rollers/rollers_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/rollers/rollers.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ bot3::control_loops::RollersLoop rollers;
+ rollers.Run();
+ ::aos::Cleanup();
+ return 0;
+}