Add throw and override modes for the Arm
This lets us throw boxes in teleop, and also get unstuck from under or
over the scale
Change-Id: Ia71824d8ae5801795d7ea21ce1becdfc1d2b55ee
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d79a293..812f091 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -42,10 +42,10 @@
bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, double *proximal_output,
+ const bool intake_clear_of_box, bool suicide,
+ bool trajectory_override, double *proximal_output,
double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status,
- bool suicide) {
+ bool *claw_closed, control_loops::ArmStatus *status) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -160,6 +160,11 @@
case State::GOTO_PATH:
if (outputs_disabled) {
state_ = State::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = State::GOTO_PATH;
} else if (close_enough_for_full_power_) {
state_ = State::RUNNING;
grab_state_ = GrabState::NORMAL;
@@ -175,6 +180,11 @@
state_ = State::ESTOP;
} else if (outputs_disabled) {
state_ = State::DISABLED;
+ } else if (trajectory_override) {
+ follower_.SwitchTrajectory(nullptr);
+ current_node_ = filtered_goal;
+ follower_.set_theta(points_[current_node_]);
+ state_ = State::GOTO_PATH;
} else if (suicide) {
state_ = State::PREP_CLIMB;
climb_count_ = 50;