Added arm pickup FSM
We can now automatically pick up a box when it is in the intake.
Change-Id: Ib93ce376717a79fc3a8747681bb7e907a4b92851
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index d236efe..45d0e45 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -37,11 +37,29 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const uint32_t *unsafe_goal,
+void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
- double *proximal_output, double *distal_output,
- bool *release_arm_brake, control_loops::ArmStatus *status) {
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered,
+ const bool intake_clear_of_box, double *proximal_output,
+ double *distal_output, bool *release_arm_brake,
+ bool *claw_closed, control_loops::ArmStatus *status) {
::Eigen::Matrix<double, 2, 1> Y;
+ const bool outputs_disabled =
+ ((proximal_output == nullptr) || (distal_output == nullptr) ||
+ (release_arm_brake == nullptr) || (claw_closed == nullptr));
+
+ uint32_t filtered_goal = 0;
+ if (unsafe_goal != nullptr) {
+ filtered_goal = *unsafe_goal;
+ }
+
+ if (open_claw) {
+ claw_closed_ = false;
+ }
+ if (outputs_disabled) {
+ claw_closed_ = true;
+ }
Y << position->proximal.encoder + proximal_offset_,
position->distal.encoder + distal_offset_;
@@ -74,15 +92,13 @@
state_ = State::ZEROING;
proximal_zeroing_estimator_.Reset();
distal_zeroing_estimator_.Reset();
- // TODO(austin): Go to the nearest node. For now, we are always going to
- // go to node 0.
break;
case State::ZEROING:
// Zero by not moving.
if (proximal_zeroing_estimator_.zeroed() &&
distal_zeroing_estimator_.zeroed()) {
- state_ = State::RUNNING;
+ state_ = State::DISABLED;
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
@@ -99,6 +115,30 @@
break;
}
+ case State::DISABLED:
+ if (!outputs_disabled) {
+ follower_.SwitchTrajectory(nullptr);
+ close_enough_for_full_power_ = false;
+ // TODO(austin): Nearest point at the end of Initialize.
+ // So, get a vector of all the points, loop through them, and find the
+ // closest one.
+ follower_.set_theta(ReadyAboveBoxPoint());
+ current_node_ = ReadyAboveBoxIndex();
+
+ state_ = State::GOTO_PATH;
+ } else {
+ break;
+ }
+
+ case State::GOTO_PATH:
+ if (outputs_disabled) {
+ state_ = State::DISABLED;
+ } else if (close_enough_for_full_power_) {
+ state_ = State::RUNNING;
+ grab_state_ = GrabState::NORMAL;
+ }
+ break;
+
case State::RUNNING:
// ESTOP if we hit the hard limits.
// TODO(austin): Pick some sane limits.
@@ -106,56 +146,8 @@
distal_zeroing_estimator_.error()) {
LOG(ERROR, "Zeroing error ESTOP\n");
state_ = State::ESTOP;
- } else if (unsafe_goal != nullptr) {
- if (!follower_.has_path()) {
- // TODO(austin): Nearest point at the end of Initialize.
- // So, get a vector of all the points, loop through them, and find the
- // closest one.
-
- // If we don't have a path and are far from the goal, don't update the
- // path.
- // TODO(austin): Once we get close to our first setpoint, crank the
- // power back up. Otherwise we'll be weak at startup.
- LOG(INFO, "No path.\n");
- if (!close_enough_for_full_power_) {
- break;
- }
- }
- if (current_node_ != *unsafe_goal) {
- LOG(INFO, "Goal is different\n");
- if (*unsafe_goal >= search_graph_.num_vertexes()) {
- LOG(ERROR, "goal out of range ESTOP\n");
- state_ = State::ESTOP;
- break;
- }
-
- if (follower_.path_distance_to_go() > 1e-3) {
- LOG(INFO, " Distance to go %f\n", follower_.path_distance_to_go());
- // Still on the old path segment. Can't change yet.
- break;
- }
-
- search_graph_.SetGoal(*unsafe_goal);
-
- size_t min_edge = 0;
- double min_cost = -1.0;
- for (const SearchGraph::HalfEdge &edge :
- search_graph_.Neighbors(current_node_)) {
- const double cost = search_graph_.GetCostToGoal(edge.dest);
- if (min_cost == -1 || cost < min_cost) {
- min_edge = edge.edge_id;
- min_cost = cost;
- }
- }
- // Ok, now we know which edge we are on. Figure out the path and
- // trajectory.
- const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
- LOG(INFO, "Switching from node %d to %d along edge %d\n",
- static_cast<int>(current_node_), static_cast<int>(next_edge.end),
- static_cast<int>(min_edge));
- follower_.SwitchTrajectory(&trajectories_[min_edge]);
- current_node_ = next_edge.end;
- }
+ } else if (outputs_disabled) {
+ state_ = State::DISABLED;
}
break;
@@ -164,14 +156,162 @@
break;
}
- const bool disable =
- ((proximal_output == nullptr) || (distal_output == nullptr) ||
- (release_arm_brake == nullptr)) ||
- state_ != State::RUNNING;
+ const bool disable = outputs_disabled ||
+ (state_ != State::RUNNING && state_ != State::GOTO_PATH);
if (disable) {
close_enough_for_full_power_ = false;
}
+ if (claw_closed_) {
+ if ((filtered_goal == ReadyAboveBoxIndex()) ||
+ (filtered_goal == TallBoxGrabIndex()) ||
+ (filtered_goal == ShortBoxGrabIndex())) {
+ filtered_goal = NeutralIndex();
+ }
+ }
+
+ // TODO(austin): Do we need to debounce box_back_beambreak_triggered ?
+ switch (grab_state_) {
+ case GrabState::NORMAL:
+ if (grab_box && !claw_closed_) {
+ grab_state_ = GrabState::WAIT_FOR_BOX;
+ } else {
+ break;
+ }
+ case GrabState::WAIT_FOR_BOX:
+ if (!grab_box) {
+ grab_state_ = GrabState::NORMAL;
+ } else {
+ if (AtState(ReadyAboveBoxIndex()) && NearEnd()) {
+ // We are being asked to grab the box, and the claw is near the box.
+ if (box_back_beambreak_triggered) {
+ // And we now see the box! Try for a tall box.
+ grab_state_ = GrabState::TALL_BOX;
+ }
+ }
+ }
+ break;
+ case GrabState::TALL_BOX:
+ if (!grab_box) {
+ grab_state_ = GrabState::NORMAL;
+ } else if (!box_back_beambreak_triggered) {
+ // Lost the box, go back to wait for it.
+ grab_state_ = GrabState::WAIT_FOR_BOX;
+ } else if (AtState(TallBoxGrabIndex()) && NearEnd()) {
+ // We are being asked to grab the box, and the claw is near the box.
+ if (claw_beambreak_triggered) {
+ grab_state_ = GrabState::CLAW_CLOSE;
+ // Snap time for the delay here.
+ claw_close_start_time_ = ::aos::monotonic_clock::now();
+ } else {
+ grab_state_ = GrabState::SHORT_BOX;
+ }
+ }
+ break;
+ case GrabState::SHORT_BOX:
+ if (!grab_box) {
+ grab_state_ = GrabState::NORMAL;
+ } else if (!box_back_beambreak_triggered) {
+ // Lost the box, go back to wait for it.
+ grab_state_ = GrabState::WAIT_FOR_BOX;
+ } else if (AtState(ShortBoxGrabIndex()) && NearEnd()) {
+ // We are being asked to grab the box, and the claw is near the box.
+ if (claw_beambreak_triggered) {
+ grab_state_ = GrabState::CLAW_CLOSE;
+ // Snap time for the delay here.
+ claw_close_start_time_ = ::aos::monotonic_clock::now();
+ } else {
+ grab_state_ = GrabState::WAIT_FOR_BOX;
+ }
+ }
+ break;
+ case GrabState::CLAW_CLOSE:
+ if (::aos::monotonic_clock::now() >
+ claw_close_start_time_ + ::std::chrono::milliseconds(500)) {
+ grab_state_ = GrabState::OPEN_INTAKE;
+ }
+ break;
+ case GrabState::OPEN_INTAKE:
+ if (intake_clear_of_box) {
+ grab_state_ = GrabState::NORMAL;
+ }
+ break;
+ }
+
+ // Now, based out our current state, go to the right state.
+ switch (grab_state_) {
+ case GrabState::NORMAL:
+ // Don't let the intake close fully with the claw closed.
+ // TODO(austin): If we want to transfer the box from the claw to the
+ // intake, we'll need to change this.
+ if (claw_closed_) {
+ max_intake_override_ = -0.5;
+ } else {
+ max_intake_override_ = 1000.0;
+ }
+ break;
+ case GrabState::WAIT_FOR_BOX:
+ filtered_goal = ReadyAboveBoxIndex();
+ claw_closed_ = false;
+ max_intake_override_ = 1000.0;
+ break;
+ case GrabState::TALL_BOX:
+ filtered_goal = TallBoxGrabIndex();
+ claw_closed_ = false;
+ max_intake_override_ = 1000.0;
+ break;
+ case GrabState::SHORT_BOX:
+ filtered_goal = ShortBoxGrabIndex();
+ claw_closed_ = false;
+ max_intake_override_ = 1000.0;
+ break;
+ case GrabState::CLAW_CLOSE:
+ // Don't move.
+ filtered_goal = current_node_;
+ claw_closed_ = true;
+ max_intake_override_ = 1000.0;
+ break;
+ case GrabState::OPEN_INTAKE:
+ // Don't move.
+ filtered_goal = current_node_;
+ claw_closed_ = true;
+ max_intake_override_ = -0.5;
+ break;
+ }
+
+ if (state_ == State::RUNNING && unsafe_goal != nullptr) {
+ if (current_node_ != filtered_goal) {
+ LOG(INFO, "Goal is different\n");
+ if (filtered_goal >= search_graph_.num_vertexes()) {
+ LOG(ERROR, "goal node out of range ESTOP\n");
+ state_ = State::ESTOP;
+ } else if (follower_.path_distance_to_go() > 1e-3) {
+ // Still on the old path segment. Can't change yet.
+ } else {
+ search_graph_.SetGoal(filtered_goal);
+
+ size_t min_edge = 0;
+ double min_cost = ::std::numeric_limits<double>::infinity();
+ for (const SearchGraph::HalfEdge &edge :
+ search_graph_.Neighbors(current_node_)) {
+ const double cost = search_graph_.GetCostToGoal(edge.dest);
+ if (cost < min_cost) {
+ min_edge = edge.edge_id;
+ min_cost = cost;
+ }
+ }
+ // Ok, now we know which edge we are on. Figure out the path and
+ // trajectory.
+ const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
+ LOG(INFO, "Switching from node %d to %d along edge %d\n",
+ static_cast<int>(current_node_), static_cast<int>(next_edge.end),
+ static_cast<int>(min_edge));
+ follower_.SwitchTrajectory(&trajectories_[min_edge]);
+ current_node_ = next_edge.end;
+ }
+ }
+ }
+
follower_.Update(
arm_ekf_.X_hat(), disable, kDt(), kVMax(),
close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
@@ -195,6 +335,7 @@
*distal_output = ::std::max(
-kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
*release_arm_brake = true;
+ *claw_closed = claw_closed_;
}
status->proximal_estimator_state =
@@ -209,6 +350,7 @@
distal_zeroing_estimator_.zeroed());
status->estopped = (state_ == State::ESTOP);
status->state = static_cast<int32_t>(state_);
+ status->grab_state = static_cast<int32_t>(grab_state_);
status->failed_solutions = follower_.failed_solutions();
arm_ekf_.Predict(follower_.U(), kDt());
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 480715d..d296703 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -23,7 +23,7 @@
// The operating voltage.
static constexpr double kOperatingVoltage() {
- return kGrannyMode() ? 6.0 : 12.0;
+ return kGrannyMode() ? 4.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 25.0; }
@@ -31,26 +31,54 @@
static constexpr double kVMax() { return kGrannyMode() ? 4.0 : 11.5; }
static constexpr double kPathlessVMax() { return 4.0; }
- void Iterate(const uint32_t *unsafe_goal,
+ void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
- double *proximal_output, double *distal_output,
- bool *release_arm_brake, control_loops::ArmStatus *status);
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered,
+ const bool intake_clear_of_box, double *proximal_output,
+ double *distal_output, bool *release_arm_brake,
+ bool *claw_closed, control_loops::ArmStatus *status);
void Reset();
enum class State : int32_t {
UNINITIALIZED,
ZEROING,
+ DISABLED,
+ GOTO_PATH,
RUNNING,
ESTOP,
};
+ enum class GrabState : int32_t {
+ NORMAL,
+ WAIT_FOR_BOX,
+ TALL_BOX,
+ SHORT_BOX,
+ CLAW_CLOSE,
+ OPEN_INTAKE,
+ };
+
State state() const { return state_; }
+ GrabState grab_state() const { return grab_state_; }
+
+ // Returns the maximum position for the intake. This is used to override the
+ // intake position to release the box when the state machine is lifting.
+ double max_intake_override() const { return max_intake_override_; }
private:
+ bool AtState(uint32_t state) const { return current_node_ == state; }
+ bool NearEnd(double threshold = 0.01) 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;
+ }
+
State state_ = State::UNINITIALIZED;
- ::aos::monotonic_clock::time_point last_time_ =
+ GrabState grab_state_ = GrabState::NORMAL;
+
+ ::aos::monotonic_clock::time_point claw_close_start_time_ =
::aos::monotonic_clock::min_time;
::frc971::zeroing::PotAndAbsEncoderZeroingEstimator proximal_zeroing_estimator_;
@@ -59,6 +87,9 @@
double proximal_offset_ = 0.0;
double distal_offset_ = 0.0;
+ bool claw_closed_ = true;
+ double max_intake_override_ = 1000.0;
+
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
::std::vector<Trajectory> trajectories_;
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index e2337f2..018a218 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -364,7 +364,7 @@
saturation_fraction_along_path_ = 0.0;
omega_.setZero();
if (trajectory_ != nullptr) {
- theta_ = trajectory_->ThetaT(goal_(0));
+ set_theta(trajectory_->ThetaT(goal_(0)));
}
}
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index 38c2b7a..a3a4246 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -329,6 +329,8 @@
bool has_path() const { return trajectory_ != nullptr; }
+ void set_theta(const ::Eigen::Matrix<double, 2, 1> &theta) { theta_ = theta; }
+
// Returns the goal distance along the path.
const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
double goal(int i) const { return goal_(i); }
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index b154334..599ddde 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -29,6 +29,8 @@
// Returns the control loop calculated voltage.
double voltage() const;
+ double output_position() const { return loop_->X_hat(0); }
+
// Executes the control loop for a cycle.
void Update(bool disabled, const double *unsafe_goal);
@@ -91,6 +93,10 @@
State state() const { return state_; }
+ bool clear_of_box() const {
+ return controller_.output_position() < -0.2;
+ }
+
private:
IntakeController controller_;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 2cddf83..3b6224c 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -34,27 +34,35 @@
arm_.Reset();
}
- intake_left_.Iterate(unsafe_goal != nullptr
- ? &(unsafe_goal->intake.left_intake_angle)
- : nullptr,
+ const double left_intake_goal = ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
+ intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
- intake_right_.Iterate(unsafe_goal != nullptr
- ? &(unsafe_goal->intake.right_intake_angle)
- : nullptr,
+ 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,
&(status->right_intake));
+ const bool intake_clear_of_box =
+ intake_left_.clear_of_box() && intake_right_.clear_of_box();
arm_.Iterate(
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- &(position->arm),
+ unsafe_goal != nullptr ? unsafe_goal->grab_box : false,
+ unsafe_goal != nullptr ? unsafe_goal->open_claw : false, &(position->arm),
+ position->claw_beambreak_triggered,
+ position->box_back_beambreak_triggered, intake_clear_of_box,
output != nullptr ? &(output->voltage_proximal) : nullptr,
output != nullptr ? &(output->voltage_distal) : nullptr,
output != nullptr ? &(output->release_arm_brake) : nullptr,
- &(status->arm));
+ output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
status->estopped = status->left_intake.estopped ||
status->right_intake.estopped || status->arm.estopped;
@@ -63,7 +71,7 @@
status->arm.zeroed;
if (output && unsafe_goal) {
- const double roller_voltage = ::std::max(
+ double roller_voltage = ::std::max(
-kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 15;
@@ -108,6 +116,9 @@
break;
}
+ if (position->box_back_beambreak_triggered && roller_voltage > 0.0) {
+ roller_voltage = 0;
+ }
switch (rotation_state_) {
case RotationState::NOT_ROTATING:
output->left_intake.voltage_rollers = roller_voltage;
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index d44b6b2..4020094 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -95,6 +95,8 @@
// The current state machine state.
uint32_t state;
+ uint32_t grab_state;
+
// The number of times the LQR solver failed.
uint32_t failed_solutions;
};
@@ -126,6 +128,8 @@
// Used to identiy a position in the planned set of positions on the arm.
uint32_t arm_goal_position;
+ // If true, start the grab box sequence.
+ bool grab_box;
bool open_claw;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 1430a45..639a683 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -203,7 +203,7 @@
constants::Values::kIntakeRange().upper) /
2.0);
- InitializeArmPosition(arm::ReadyAboveBoxPoint());
+ InitializeArmPosition(arm::UpPoint());
}
void InitializeIntakePosition(double start_pos) {
@@ -348,6 +348,8 @@
goal->intake.left_intake_angle = 0.1;
goal->intake.right_intake_angle = 0.2;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -369,6 +371,8 @@
goal->intake.left_intake_angle = 0.1;
goal->intake.right_intake_angle = 0.2;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -388,6 +392,8 @@
goal->intake.left_intake_angle = 5.0 * M_PI;
goal->intake.right_intake_angle = 5.0 * M_PI;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -415,6 +421,8 @@
goal->intake.left_intake_angle = -5.0 * M_PI;
goal->intake.right_intake_angle = -5.0 * M_PI;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -442,6 +450,8 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
@@ -463,6 +473,8 @@
auto goal = superstructure_queue_.goal.MakeMessage();
goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
@@ -480,6 +492,8 @@
constants::Values::kIntakeRange().upper - 0.1;
goal->intake.right_intake_angle =
constants::Values::kIntakeRange().upper - 0.1;
+ goal->arm_goal_position = arm::UpIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
RunForTime(chrono::seconds(10));
@@ -509,14 +523,15 @@
}
// Tests that we don't freak out without a goal.
-TEST_F(SuperstructureTest, ArmNoGoalTest) {
+TEST_F(SuperstructureTest, ArmSimpleGoal) {
RunForTime(chrono::seconds(5));
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = 0;
+ goal->intake.left_intake_angle = -0.8;
+ goal->intake.right_intake_angle = -0.8;
+ goal->arm_goal_position = arm::FrontHighBoxIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -530,6 +545,7 @@
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::FrontHighBoxIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
RunForTime(chrono::seconds(10));
@@ -541,6 +557,7 @@
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::ReadyAboveBoxIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
@@ -556,6 +573,7 @@
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::BackLowBoxIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
RunForTime(chrono::seconds(10));
@@ -567,6 +585,7 @@
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::ReadyAboveBoxIndex();
+ goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}