Startup with more power to free the brake.
Change-Id: I2f85ef12f26dbcd7d5b9a151079bffc51cf8e696
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index f69bab3..4912359 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -336,11 +336,13 @@
}
}
- follower_.Update(
- arm_ekf_.X_hat(), disable, kDt(), kVMax(),
- close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
- LOG(INFO, "Max voltage: %f\n",
- close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ const double max_operating_voltage =
+ close_enough_for_full_power_
+ ? kOperatingVoltage()
+ : (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ max_operating_voltage);
+ LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
status->goal_theta0 = follower_.theta(0);
status->goal_theta1 = follower_.theta(1);
status->goal_omega0 = follower_.omega(0);