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.
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 63fdcf0..1422dde 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -49,6 +49,9 @@
float max_angular_acceleration_intake;
float max_angular_acceleration_shoulder;
float max_angular_acceleration_wrist;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ float voltage_rollers;
};
message Status {
@@ -86,6 +89,8 @@
float voltage_intake;
float voltage_shoulder;
float voltage_wrist;
+
+ float voltage_rollers;
};
queue Goal goal;