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;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 9493037..d3dd2df 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -38,10 +38,11 @@
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);
void Reset();
@@ -71,6 +72,10 @@
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
+ uint32_t current_node() const { return current_node_; }
+
+ double path_distance_to_go() { return follower_.path_distance_to_go(); }
+
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
bool NearEnd(double threshold = 0.03) const {
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 2098cd0..26f75be 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -96,18 +96,28 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
+
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ if (unsafe_goal) {
+ if (unsafe_goal->open_threshold != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ open_claw = false;
+ }
+ }
+ }
arm_.Iterate(
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
- unsafe_goal != nullptr ? unsafe_goal->open_claw : false,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
&(position->arm), position->claw_beambreak_triggered,
position->box_back_beambreak_triggered, intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm),
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false);
+ output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
if (output) {
if (unsafe_goal) {
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index e2cad73..fc1d935 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -140,7 +140,11 @@
double voltage_winch;
+ double open_threshold;
+
bool disable_box_correct;
+
+ bool trajectory_override;
};
message Status {