Add double jointed arm trajectory with roll
This is strongly inspired by the 2018 arm code and drivetrain splines,
but is different enough to not be worth making the same.
Change-Id: I9d840b65596454a2e42a1b4a082ea1549e5b4ae2
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index c125fac..0ef2cf9 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -78,3 +78,50 @@
"//frc971/control_loops/double_jointed_arm:dynamics",
],
)
+
+cc_library(
+ name = "trajectory",
+ srcs = ["trajectory.cc"],
+ hdrs = ["trajectory.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/control_loops:binomial",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:fixed_quadrature",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = ["trajectory_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":arm_constants",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":arm_constants",
+ ":trajectory",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops:binomial",
+ "//frc971/control_loops:fixed_quadrature",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//y2023/control_loops/superstructure/roll:roll_plants",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)