Implement the C++ drivetrain trajectory optimizer
This implements the curvature, forwards, and backwards passes, and adds
a test which makes sure the feed forwards gets us close enough to the
end. Also adds a plotting tool (trajectory_plot) to simulate everything
and tune.
Change-Id: I9f8d6088893cc0b7263b3ff0d79667c027604700
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7f87cca..432dd0e 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -303,3 +303,53 @@
"@com_github_gflags_gflags//:gflags",
],
)
+
+cc_library(
+ name = "trajectory",
+ srcs = ["trajectory.cc"],
+ hdrs = ["trajectory.h"],
+ deps = [
+ ":distance_spline",
+ ":drivetrain_config",
+ "//aos/logging:matrix_logging",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:runge_kutta",
+ "//frc971/control_loops:state_feedback_loop",
+ "//third_party/eigen",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":distance_spline",
+ ":trajectory",
+ "//aos/logging:implementations",
+ "//aos/logging:matrix_logging",
+ "//aos/network:team_number",
+ "//third_party/eigen",
+ "//third_party/matplotlib-cpp",
+ "//y2016/control_loops/drivetrain:drivetrain_base",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = [
+ "trajectory_test.cc",
+ ],
+ deps = [
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ "//y2016:constants",
+ "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+ ],
+)