Sped box grab up significantly and handle disable
Add some more trajectories and cut tolerances to make box grabbing go
faster. Also, go to the nearest node when disabled.
Change-Id: Ic641db69a8c18eb61771ebc352e05ceef0d90084
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index fbdcfbb..d5dbe12 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -19,9 +19,8 @@
" trajectories->emplace_back(Path::Reversed(%s()), 0.005);" %
(path_function_name(str(name))))
else:
- cc_file.append(
- " trajectories->emplace_back(%s(), 0.005);" %
- (path_function_name(str(name))))
+ cc_file.append(" trajectories->emplace_back(%s(), 0.005);" %
+ (path_function_name(str(name))))
start_index = None
end_index = None
@@ -38,13 +37,11 @@
(index_function_name(start_index),
index_function_name(end_index)))
cc_file.append(
- " trajectories->back().path().length()});"
- )
+ " (trajectories->back().path().length() + 0.2)});")
# TODO(austin): Allow different vmaxes for different paths.
cc_file.append(
- " trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);"
- )
+ " trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);")
cc_file.append("")
@@ -115,16 +112,17 @@
(-point[3], -point[4], -point[5]))
cc_file.append(" }));")
cc_file.append("}")
-
+
# Matrix of nodes
h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
- cc_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
+ cc_file.append(
+ "::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
cc_file.append(" ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
- for index, point in enumerate(graph_generate.points):
+ for point in graph_generate.points:
cc_file.append(
- " points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());" % (
- numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+ " points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());"
+ % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
cc_file.append(" return points;")
cc_file.append("}")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index fdc5507..607613c 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -387,7 +387,7 @@
tall_box_x = 0.401
-tall_box_y = 0.13
+tall_box_y = 0.14
short_box_x = 0.431
short_box_y = 0.082
@@ -459,6 +459,10 @@
XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
ready_above_box, "ReadyToNeutral"),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ tall_box_grab, "TallToNeutral"),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ short_box_grab, "ShortToNeutral"),
SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
front_high_box, "NeutralToFrontHigh"),
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 45d0e45..c283f19 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -26,7 +26,8 @@
.finished()),
search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
// Go to the start of the first trajectory.
- follower_(ReadyAboveBoxPoint()) {
+ follower_(ReadyAboveBoxPoint()),
+ points_(PointList()) {
int i = 0;
for (const auto &trajectory : trajectories_) {
LOG(INFO, "trajectory length for edge node %d: %f\n", i,
@@ -115,20 +116,34 @@
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();
+ case State::DISABLED: {
+ follower_.SwitchTrajectory(nullptr);
+ close_enough_for_full_power_ = false;
+ const ::Eigen::Matrix<double, 2, 1> current_theta =
+ (::Eigen::Matrix<double, 2, 1>() << arm_ekf_.X_hat(0),
+ arm_ekf_.X_hat(2))
+ .finished();
+ uint32_t best_index = 0;
+ double best_distance = (points_[0] - current_theta).norm();
+ uint32_t current_index = 0;
+ for (const ::Eigen::Matrix<double, 2, 1> &point : points_) {
+ const double new_distance = (point - current_theta).norm();
+ if (new_distance < best_distance) {
+ best_distance = new_distance;
+ best_index = current_index;
+ }
+ ++current_index;
+ }
+ follower_.set_theta(points_[best_index]);
+ current_node_ = best_index;
+
+ if (!outputs_disabled) {
state_ = State::GOTO_PATH;
} else {
break;
}
+ }
case State::GOTO_PATH:
if (outputs_disabled) {
@@ -162,6 +177,7 @@
close_enough_for_full_power_ = false;
}
+ // TODO(austin): Do we need to debounce box_back_beambreak_triggered ?
if (claw_closed_) {
if ((filtered_goal == ReadyAboveBoxIndex()) ||
(filtered_goal == TallBoxGrabIndex()) ||
@@ -194,9 +210,6 @@
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) {
@@ -211,9 +224,6 @@
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) {
@@ -227,7 +237,7 @@
break;
case GrabState::CLAW_CLOSE:
if (::aos::monotonic_clock::now() >
- claw_close_start_time_ + ::std::chrono::milliseconds(500)) {
+ claw_close_start_time_ + ::std::chrono::milliseconds(300)) {
grab_state_ = GrabState::OPEN_INTAKE;
}
break;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d296703..8e1860f 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -19,17 +19,18 @@
Arm();
// If true, tune down all the constants for testing.
- static constexpr bool kGrannyMode() { return true; }
+ static constexpr bool kGrannyMode() { return false; }
// The operating voltage.
static constexpr double kOperatingVoltage() {
return kGrannyMode() ? 4.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
- static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 25.0; }
- static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 25.0; }
- static constexpr double kVMax() { return kGrannyMode() ? 4.0 : 11.5; }
- static constexpr double kPathlessVMax() { return 4.0; }
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 15.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 15.0; }
+
+ static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
+ static constexpr double kPathlessVMax() { return 5.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
@@ -68,7 +69,7 @@
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
- bool NearEnd(double threshold = 0.01) const {
+ bool NearEnd(double threshold = 0.02) 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;
@@ -100,6 +101,8 @@
EKF arm_ekf_;
TrajectoryFollower follower_;
+ const ::std::vector<::Eigen::Matrix<double, 2, 1>> points_;
+
// Start at the 0th index.
uint32_t current_node_ = 0;
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index 018a218..48140dd 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -477,6 +477,7 @@
void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 6, 1> &X,
bool disabled, double dt, double plan_vmax,
double voltage_limit) {
+ // TODO(austin): Separate voltage limit for shoulder for no path.
last_goal_ = goal_;
if (!has_path()) {
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 599ddde..2c36efa 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -94,7 +94,7 @@
State state() const { return state_; }
bool clear_of_box() const {
- return controller_.output_position() < -0.2;
+ return controller_.output_position() < -0.1;
}
private: