Add arm code to 2023
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: If97b4dd4c8fb16371e21ac6e9667a2d52b1b53a6
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 06f036e..667a78d 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -27,11 +27,11 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(dynamics, Path::Reversed(%s()), 0.005, kArmConstants));"
+ " Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(dynamics, %s(), 0.005, kArmConstants));"
+ " Trajectory(dynamics, %s(), 0.005));"
% (path_function_name(str(name))))
start_index = None
@@ -202,11 +202,11 @@
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
"const frc971::control_loops::arm::Dynamics *dynamics, "
- "::std::vector<TrajectoryAndParams> * /*trajectories*/,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
- "const ::Eigen::Matrix<double, 2, 2> & /*alpha_unitizer*/,")
+ "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
cc_file.append(" "
- "double /*vmax*/) {")
+ "double vmax) {")
cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
index = 0
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 6d8f489..cfc68ac 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -17,16 +17,21 @@
circular_index=-1)
cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
-points = []
+# TODO(Max): Add real paths for arm.
+points = [(neutral, "NeutralPos"), (neutral_to_cone_1, "NeutralToConePos1"),
+ (neutral_to_cone_2, "NeutralToConePos2"), (cone_pos, "ConePos"),
+ (neutral_to_cone_perch_pos_1, "NeutralToConePerchPos1"),
+ (neutral_to_cone_perch_pos_2, "NeutralToConePerchPos2"),
+ (cone_perch_pos, "ConePerchPos")]
front_points = []
back_points = []
unnamed_segments = []
-named_segments = []
-
-segments = [
+named_segments = [
ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
"NeutralToCone"),
ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
neutral_to_cone_perch_pos_2, cone_perch_pos,
- "NeutralToPerchedCone"),
+ "NeutralToConePerch"),
]
+
+segments = unnamed_segments + named_segments