commit 8f0e07f8f36721484cf83badd46ca6cb65f3a354
author Maxwell Henderson <mxwhenderson@gmail.com> Wed Feb 08 21:10:58 2023 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Feb 18 14:16:05 2023 -0800
tree 7c91f24d8b6fed3d7cc078c681404f3083aca04d
parent bc78ea9b807d9aeb3d061349c4165dfdc948bcff

Move 2018 arm code to frc971 and make dynamics adjustable

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I16e7bbb0d293aca461abc931f560fb9066b19aaf

y2018/control_loops/superstructure/arm/arm.cc

diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 1c29157..ab5deea 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -5,8 +5,9 @@
 
 #include "aos/logging/logging.h"
 #include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
 #include "y2018/control_loops/superstructure/arm/generated_graph.h"
 
 namespace y2018 {
@@ -29,9 +30,12 @@
       alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
                        0.0, 0.0, 1.0 / kAlpha1Max())
                           .finished()),
-      search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
+      dynamics_(kArmConstants),
+      search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+                                    kVMax())),
+      arm_ekf_(&dynamics_),
       // Go to the start of the first trajectory.
-      follower_(ReadyAboveBoxPoint()),
+      follower_(&dynamics_, ReadyAboveBoxPoint()),
       points_(PointList()) {
   int i = 0;
   for (const auto &trajectory : trajectories_) {