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;
+}
diff --git a/bot3/output/motor_writer.cc b/bot3/output/motor_writer.cc
index 809a125..18b263e 100644
--- a/bot3/output/motor_writer.cc
+++ b/bot3/output/motor_writer.cc
@@ -11,7 +11,7 @@
#include "aos/common/controls/output_check.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/queues/rollers.q.h"
+#include "bot3/control_loops/rollers/rollers.q.h"
using ::aos::util::SimpleLogInterval;
@@ -54,8 +54,7 @@
}
{
- // This isn't actually a queue group...
- static auto &rollers = ::bot3::rollers;
+ static auto &rollers = ::bot3::control_loops::rollers.output;
rollers.FetchLatest();
if (rollers.IsNewerThanMS(kOutputMaxAgeMS)) {
LOG_STRUCT(DEBUG, "will output", *rollers);
@@ -65,6 +64,7 @@
SetPWMOutput(1, rollers->back_intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(6, -rollers->back_intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(4, rollers->low_goal_voltage / 12.0, kTalonBounds);
+
SetSolenoid(2, rollers->front_extended);
SetSolenoid(5, !rollers->front_extended);
SetSolenoid(3, rollers->back_extended);
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
index e3243af..5e4ff3a 100644
--- a/bot3/output/output.gyp
+++ b/bot3/output/output.gyp
@@ -11,6 +11,7 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
'<(AOS)/common/controls/controls.gyp:control_loop',
'<(DEPTH)/bot3/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
diff --git a/bot3/prime/prime.gyp b/bot3/prime/prime.gyp
index 5e5a00b..acde62f 100644
--- a/bot3/prime/prime.gyp
+++ b/bot3/prime/prime.gyp
@@ -9,6 +9,7 @@
'../control_loops/drivetrain/drivetrain.gyp:drivetrain',
'../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
+ '../control_loops/rollers/rollers.gyp:rollers',
'../autonomous/autonomous.gyp:auto',
#'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
diff --git a/bot3/prime/start_list.txt b/bot3/prime/start_list.txt
index fcaf76d..8606b0b 100644
--- a/bot3/prime/start_list.txt
+++ b/bot3/prime/start_list.txt
@@ -5,3 +5,4 @@
auto
sensor_receiver
joystick_proxy
+rollers
diff --git a/bot3/queues/queues.gyp b/bot3/queues/queues.gyp
index 65ffa45..a7f03c2 100644
--- a/bot3/queues/queues.gyp
+++ b/bot3/queues/queues.gyp
@@ -1,7 +1,6 @@
{
'variables': {
'queue_files': [
- 'rollers.q',
'to_log.q',
]
},
diff --git a/bot3/queues/rollers.q b/bot3/queues/rollers.q
deleted file mode 100644
index 1b706d0..0000000
--- a/bot3/queues/rollers.q
+++ /dev/null
@@ -1,15 +0,0 @@
-package bot3;
-
-message Rollers {
- // 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;
-};
-queue Rollers rollers;