Made hanging work.
Change-Id: I02f7601c530387a1d9c03c0995544a8541466231
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index d90ec68..49ea43f 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -48,7 +48,7 @@
// incorporating a small safety margin makes writing test cases much easier
// since you can directly compare statuses against the constants in the
// CollisionAvoidance class.
- constexpr double kSafetyMargin = 0.02; // radians
+ constexpr double kSafetyMargin = 0.03; // radians
// Avoid colliding the shooter with the frame.
// If the shoulder is below a certain angle or we want to move it below
@@ -624,7 +624,8 @@
kill_shoulder_ = true;
}
}
- arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage, max_voltage);
+ arm_.set_max_voltage(kill_shoulder_ ? 0.0 : max_voltage,
+ kill_shoulder_ ? kShooterHangingVoltage : max_voltage);
intake_.set_max_voltage(max_voltage);
if (IsRunning() && !kill_shoulder_) {
@@ -671,7 +672,7 @@
output->unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
- ::y2016::sensors::ball_detector.FetchAnother();
+ ::y2016::sensors::ball_detector.FetchLatest();
bool ball_detected = false;
if (::y2016::sensors::ball_detector.get()) {
ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;