Added intake rollers and test
Change-Id: Ie47b1e3d85b1693e2269bfd883bea03d0b887b92
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index efa6ccb..faf1c9a 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -38,11 +38,10 @@
output != nullptr ? &(output->wrist_voltage) : nullptr,
&(status->wrist));
- intake_.Iterate(
- unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
+ &(position->intake_joint),
+ output != nullptr ? &(output->intake_joint_voltage) : nullptr,
+ &(status->intake));
stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
&(position->stilts),
@@ -55,6 +54,16 @@
status->estopped = status->elevator.estopped || status->wrist.estopped ||
status->intake.estopped || status->stilts.estopped;
+ if (output) {
+ if (status->intake.position > kMinIntakeAngleForRollers) {
+ output->intake_roller_voltage =
+ (unsafe_goal != nullptr) ? unsafe_goal->roller_voltage : 0.0;
+
+ } else {
+ output->intake_roller_voltage = 0.0;
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);