Added superstructure and intake classes.
Created the superstucture, intake, and intake controller class,
had to alter some of the intake python and superstructure queue to fit.
Change-Id: Ieabcf288f6dd50c282a3bcb61ec13062f735872b
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..157a46e
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,67 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+namespace {
+// The maximum voltage the intake roller will be allowed to use.
+constexpr double kMaxIntakeRollerVoltage = 12.0;
+} // namespace
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ superstructure_queue),
+ intake_left_(constants::GetValues().left_intake.zeroing),
+ intake_right_(constants::GetValues().right_intake.zeroing) {}
+
+void Superstructure::RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position *position,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status *status) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ intake_left_.Reset();
+ intake_right_.Reset();
+ }
+
+ intake_left_.Iterate(unsafe_goal != nullptr
+ ? &(unsafe_goal->intake.left_intake_angle)
+ : nullptr,
+ &(position->intake.left),
+ output != nullptr ? &(output->intake.left) : nullptr,
+ &(status->left_intake));
+
+ intake_right_.Iterate(unsafe_goal != nullptr
+ ? &(unsafe_goal->intake.right_intake_angle)
+ : nullptr,
+ &(position->intake.right),
+ output != nullptr ? &(output->intake.right) : nullptr,
+ &(status->right_intake));
+
+ status->estopped =
+ status->left_intake.estopped || status->right_intake.estopped;
+
+ status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed;
+
+ if (output && unsafe_goal) {
+ output->intake.left.voltage_rollers = ::std::max(
+ -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+ kMaxIntakeRollerVoltage));
+ output->intake.right.voltage_rollers = ::std::max(
+ -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+ kMaxIntakeRollerVoltage));
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018