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