Added tweak up and tweak down buttons.
Next up, add a bunch of extra nodes.
Change-Id: I4c2df145298567e61e8c36760be88f59a391588b
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 5559f7c..abe5ed3 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -117,6 +117,26 @@
% (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
h_file.append("}")
+ front_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.front_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> FrontPoints() {" %
+ len(front_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(front_points), ", ".join(front_points)))
+ h_file.append("}")
+
+ back_points = [
+ index_function_name(point[1]) + "()" for point in graph_generate.back_points
+ ]
+ h_file.append("")
+ h_file.append("constexpr ::std::array<uint32_t, %d> BackPoints() {" %
+ len(back_points))
+ h_file.append(" return ::std::array<uint32_t, %d>{{%s}};" %
+ (len(back_points), ", ".join(back_points)))
+ h_file.append("}")
+
# Add the Make*Path functions.
h_file.append("")
cc_file.append("")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 0807c79..3b21e5d 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -556,19 +556,26 @@
front_switch_c1 = numpy.array([1.903841, -0.622351])
front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+front_points = [
+ (front_high_box, "FrontHighBox"),
+ (front_middle2_box, "FrontMiddle2Box"),
+ (front_middle3_box, "FrontMiddle3Box"),
+ (front_middle1_box, "FrontMiddle1Box"),
+ (front_low_box, "FrontLowBox"),
+ (front_switch, "FrontSwitch"),
+] # yapf: disable
+
+back_points = [
+ (back_high_box, "BackHighBox"),
+ (back_middle2_box, "BackMiddle2Box"),
+ (back_middle1_box, "BackMiddle1Box"),
+ (back_low_box, "BackLowBox"),
+] # yapf: disable
+
points = [(ready_above_box, "ReadyAboveBox"),
(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"),
- (back_high_box, "BackHighBox"),
- (back_middle2_box, "BackMiddle2Box"),
- (back_middle1_box, "BackMiddle1Box"),
- (back_low_box, "BackLowBox"),
- (front_switch, "FrontSwitch"),
(back_switch, "BackSwitch"),
(neutral, "Neutral"),
(up, "Up"),
@@ -580,7 +587,7 @@
(starting, "Starting"),
(duck, "Duck"),
(vertical_starting, "VerticalStarting"),
-] # yapf: disable
+] + front_points + back_points # yapf: disable
duck_c1 = numpy.array([1.337111, -1.721008])
duck_c2 = numpy.array([1.283701, -1.795519])
@@ -588,6 +595,9 @@
ready_to_up_c1 = numpy.array([1.792962, 0.198329])
ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
@@ -611,6 +621,7 @@
]
unnamed_segments = [
+ SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2, front_switch_auto),
SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
@@ -623,15 +634,11 @@
XYSegment(ready_above_box, front_middle1_box),
XYSegment(ready_above_box, front_middle2_box),
XYSegment(ready_above_box, front_middle3_box),
- XYSegment(ready_above_box, front_high_box),
- #XYSegment(ready_above_box, up),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, front_high_box),
AngleSegment(starting, vertical_starting),
AngleSegment(vertical_starting, neutral),
- # TODO(austin): Duck -> neutral with a theta spline.
- #AngleSegment(duck, vertical_starting),
-
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
@@ -641,8 +648,6 @@
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),