Adjust the button behavior and arm locations.
Change-Id: Ide2e4d3a8b22602501547d721cca1c7a7e4f3e8f
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 4609827..3f48cc9 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -71,7 +71,7 @@
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
- bool NearEnd(double threshold = 0.02) const {
+ bool NearEnd(double threshold = 0.03) const {
return ::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= threshold &&
::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= threshold &&
follower_.path_distance_to_go() < 1e-3;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 37fb4f9..e22219e 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -37,15 +37,15 @@
const double left_intake_goal = ::std::min(
arm_.max_intake_override(),
(unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
+ const double right_intake_goal = ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
+
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
- const double right_intake_goal = ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
-
intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
&(position->right_intake),
output != nullptr ? &(output->right_intake) : nullptr,