Adjust the button behavior and arm locations.
Change-Id: Ide2e4d3a8b22602501547d721cca1c7a7e4f3e8f
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 6d7b628..aa91137 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -386,8 +386,8 @@
]
-tall_box_x = 0.401
-tall_box_y = 0.14
+tall_box_x = 0.411
+tall_box_y = 0.125
short_box_x = 0.431
short_box_y = 0.082
@@ -401,17 +401,19 @@
# TODO(austin): Drive the front/back off the same numbers a bit better.
front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(
+ 0.700, 2.125, circular_index=-1.000000)
front_middle2_box = to_theta_with_circular_index(
- 0.732, 2.268, circular_index=-1)
+ 0.700, 2.268, circular_index=-1)
front_middle1_box = to_theta_with_circular_index(
- 0.878, 1.885, circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.926, 1.542, circular_index=-1)
+ 0.800, 1.915, circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
back_middle2_box = to_theta_with_circular_index(
- -0.732, 2.268, circular_index=0)
+ -0.700, 2.268, circular_index=0)
back_middle1_box = to_theta_with_circular_index(
- -0.878, 1.885, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.926, 1.542, circular_index=0)
+ -0.800, 1.915, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.572, circular_index=0)
front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
@@ -420,15 +422,23 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+front_switch_auto = to_theta_with_circular_index(
+ 0.750, 2.20, circular_index=-1.000000)
+
+starting = numpy.array(
+ [numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array(
+ [numpy.pi / 2.0, -numpy.pi / 2.0])
+
self_hang = numpy.array(
[numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
partner_hang = numpy.array(
[numpy.pi / 2.0 - (-0.25), numpy.pi / 2.0])
above_hang = numpy.array(
- [numpy.pi / 2.0 - 0.008739, numpy.pi / 2.0 - (-0.101927)])
+ [numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
below_hang = numpy.array(
- [numpy.pi / 2.0 - 0.329954, numpy.pi / 2.0 - (-0.534816)])
+ [numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -452,6 +462,7 @@
(tall_box_grab, "TallBoxGrab"),
(short_box_grab, "ShortBoxGrab"),
(front_high_box, "FrontHighBox"),
+ (front_middle3_box, "FrontMiddle3Box"),
(front_middle2_box, "FrontMiddle2Box"),
(front_middle1_box, "FrontMiddle1Box"),
(front_low_box, "FrontLowBox"),
@@ -467,6 +478,9 @@
(below_hang, "BelowHang"),
(self_hang, "SelfHang"),
(partner_hang, "PartnerHang"),
+ (front_switch_auto, "FrontSwitchAuto"),
+ (starting, "Starting"),
+ (vertical_starting, "VerticalStarting"),
] # yapf: disable
# We need to define critical points so we can create paths connecting them.
@@ -494,9 +508,20 @@
AngleSegment(neutral, back_switch),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+ AngleSegment(starting, vertical_starting),
+ AngleSegment(vertical_starting, neutral),
+
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
+
+ XYSegment(front_middle3_box, up),
+ XYSegment(front_middle3_box, front_high_box),
+ XYSegment(front_middle3_box, front_middle2_box),
+ XYSegment(front_middle3_box, front_middle1_box),
+
+ XYSegment(neutral, front_switch_auto),
+
XYSegment(up, front_middle1_box),
XYSegment(up, front_low_box),
XYSegment(front_high_box, front_middle2_box),
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 4609827..3f48cc9 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -71,7 +71,7 @@
private:
bool AtState(uint32_t state) const { return current_node_ == state; }
- bool NearEnd(double threshold = 0.02) const {
+ bool NearEnd(double threshold = 0.03) 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;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 37fb4f9..e22219e 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -37,15 +37,15 @@
const double left_intake_goal = ::std::min(
arm_.max_intake_override(),
(unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.left_intake_angle));
+ const double right_intake_goal = ::std::min(
+ arm_.max_intake_override(),
+ (unsafe_goal == nullptr ? 0.0 : unsafe_goal->intake.right_intake_angle));
+
intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
&(position->left_intake),
output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
- 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,