Add intake rollers.
Change-Id: Ib6f78956950d61fb048e7370a609a3f2f9f3c876
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index a2b7a9e..096ed58 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -14,14 +14,14 @@
namespace superstructure {
namespace {
+constexpr double kZeroingVoltage = 4.0;
+
constexpr double kIntakeEncoderIndexDifference =
constants::Values::kIntakeEncoderIndexDifference;
constexpr double kWristEncoderIndexDifference =
constants::Values::kWristEncoderIndexDifference;
constexpr double kShoulderEncoderIndexDifference =
constants::Values::kShoulderEncoderIndexDifference;
-
-constexpr double kZeroingVoltage = 4.0;
} // namespace
// ///// CollisionAvoidance /////
@@ -454,6 +454,13 @@
output->voltage_intake = intake_.intake_voltage();
output->voltage_shoulder = arm_.shoulder_voltage();
output->voltage_wrist = arm_.wrist_voltage();
+
+ // Logic to run our rollers on the intake.
+ output->voltage_rollers = 0.0;
+ if (unsafe_goal) {
+ output->voltage_rollers =
+ ::std::max(-8.0, ::std::min(8.0, unsafe_goal->voltage_rollers));
+ }
}
// Save debug/internal state.