Taught superstructure to turn rollers off.
Change-Id: Iebf041d095ee1fab1a361524c4cc5268765cab7a
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index f633d7b..6a10b61 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -78,6 +78,7 @@
'//aos/common/controls:control_loop',
'//aos/common/util:trapezoid_profile',
'//aos/common:math',
+ '//y2016/queues:ball_detector',
'//frc971/control_loops:state_feedback_loop',
'//frc971/control_loops:simple_capped_state_feedback_loop',
'//frc971/zeroing',
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index e5f0fff..7871732 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -7,6 +7,7 @@
#include "y2016/control_loops/superstructure/integral_intake_plant.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
+#include "y2016/queues/ball_detector.q.h"
#include "y2016/constants.h"
@@ -653,12 +654,23 @@
output->voltage_top_rollers = 0.0;
output->voltage_bottom_rollers = 0.0;
if (unsafe_goal) {
- output->voltage_top_rollers = ::std::max(
- -kMaxIntakeTopVoltage,
- ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
- output->voltage_bottom_rollers = ::std::max(
- -kMaxIntakeBottomVoltage,
- ::std::min(unsafe_goal->voltage_bottom_rollers, kMaxIntakeBottomVoltage));
+ ::y2016::sensors::ball_detector.FetchAnother();
+ bool ball_detected = false;
+ if (::y2016::sensors::ball_detector.get()) {
+ ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
+ }
+ if (unsafe_goal->force_intake || !ball_detected) {
+ output->voltage_top_rollers = ::std::max(
+ -kMaxIntakeTopVoltage,
+ ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
+ output->voltage_bottom_rollers =
+ ::std::max(-kMaxIntakeBottomVoltage,
+ ::std::min(unsafe_goal->voltage_bottom_rollers,
+ kMaxIntakeBottomVoltage));
+ } else {
+ output->voltage_top_rollers = 0.0;
+ output->voltage_bottom_rollers = 0.0;
+ }
output->traverse_unlatched = unsafe_goal->traverse_unlatched;
output->traverse_down = unsafe_goal->traverse_down;
}
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index f550512..2919398 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -65,6 +65,8 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+ bool force_intake;
+
// If true, release the latch which holds the traverse mechanism in the
// middle.
bool traverse_unlatched;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 6706485..b542d04 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -367,6 +367,7 @@
new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
new_superstructure_goal->traverse_down = traverse_down_;
+ new_superstructure_goal->force_intake = true;
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");