diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 567810d..a3cb808 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -94,6 +94,7 @@
         "//frc971/control_loops:control_loop_test",
         "//frc971/control_loops:position_sensor_sim",
         "//frc971/control_loops:team_number_test_environment",
+        "//y2018/control_loops/superstructure/arm:arm_constants",
         "//y2018/control_loops/superstructure/intake:intake_plants",
     ],
 )
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index f90cc92..9f969ff 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -1,128 +1,4 @@
 cc_library(
-    name = "trajectory",
-    srcs = [
-        "trajectory.cc",
-    ],
-    hdrs = [
-        "trajectory.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//visibility:public"],
-    deps = [
-        ":dynamics",
-        "//aos/logging",
-        "//frc971/control_loops:dlqr",
-        "//frc971/control_loops:jacobian",
-        "@org_tuxfamily_eigen//:eigen",
-    ],
-)
-
-cc_test(
-    name = "trajectory_test",
-    srcs = [
-        "trajectory_test.cc",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    deps = [
-        ":demo_path",
-        ":dynamics",
-        ":ekf",
-        ":trajectory",
-        "//aos/testing:googletest",
-        "@org_tuxfamily_eigen//:eigen",
-    ],
-)
-
-cc_library(
-    name = "dynamics",
-    srcs = [
-        "dynamics.cc",
-    ],
-    hdrs = [
-        "dynamics.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//visibility:public"],
-    deps = [
-        "//frc971/control_loops:runge_kutta",
-        "@com_github_gflags_gflags//:gflags",
-        "@org_tuxfamily_eigen//:eigen",
-    ],
-)
-
-cc_library(
-    name = "demo_path",
-    srcs = [
-        "demo_path.cc",
-    ],
-    hdrs = ["demo_path.h"],
-    target_compatible_with = ["@platforms//os:linux"],
-    deps = [":trajectory"],
-)
-
-cc_test(
-    name = "dynamics_test",
-    srcs = [
-        "dynamics_test.cc",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    deps = [
-        ":dynamics",
-        "//aos/testing:googletest",
-    ],
-)
-
-cc_binary(
-    name = "trajectory_plot",
-    srcs = [
-        "trajectory_plot.cc",
-    ],
-    target_compatible_with = ["@platforms//cpu:x86_64"],
-    deps = [
-        ":ekf",
-        ":generated_graph",
-        ":trajectory",
-        "//frc971/analysis:in_process_plotter",
-        "@com_github_gflags_gflags//:gflags",
-        "@org_tuxfamily_eigen//:eigen",
-    ],
-)
-
-cc_library(
-    name = "ekf",
-    srcs = [
-        "ekf.cc",
-    ],
-    hdrs = [
-        "ekf.h",
-    ],
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//visibility:public"],
-    deps = [
-        ":dynamics",
-        "//frc971/control_loops:jacobian",
-        "@org_tuxfamily_eigen//:eigen",
-    ],
-)
-
-cc_library(
-    name = "graph",
-    srcs = ["graph.cc"],
-    hdrs = ["graph.h"],
-    target_compatible_with = ["@platforms//os:linux"],
-)
-
-cc_test(
-    name = "graph_test",
-    srcs = ["graph_test.cc"],
-    target_compatible_with = ["@platforms//os:linux"],
-    deps = [
-        ":graph",
-        "//aos/testing:googletest",
-    ],
-)
-
-cc_library(
     name = "arm",
     srcs = [
         "arm.cc",
@@ -133,15 +9,16 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
-        ":demo_path",
-        ":ekf",
         ":generated_graph",
-        ":graph",
-        ":trajectory",
+        "//frc971/control_loops/double_jointed_arm:demo_path",
+        "//frc971/control_loops/double_jointed_arm:ekf",
+        "//frc971/control_loops/double_jointed_arm:graph",
+        "//frc971/control_loops/double_jointed_arm:trajectory",
         "//frc971/zeroing",
         "//y2018:constants",
         "//y2018/control_loops/superstructure:superstructure_position_fbs",
         "//y2018/control_loops/superstructure:superstructure_status_fbs",
+        "//y2018/control_loops/superstructure/arm:arm_constants",
     ],
 )
 
@@ -170,7 +47,35 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
-        ":graph",
-        ":trajectory",
+        ":arm_constants",
+        "//frc971/control_loops/double_jointed_arm:graph",
+        "//frc971/control_loops/double_jointed_arm:trajectory",
+    ],
+)
+
+cc_library(
+    name = "arm_constants",
+    hdrs = ["arm_constants.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops/double_jointed_arm:dynamics",
+    ],
+)
+
+cc_binary(
+    name = "trajectory_plot",
+    srcs = [
+        "trajectory_plot.cc",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":arm_constants",
+        ":generated_graph",
+        "//frc971/analysis:in_process_plotter",
+        "//frc971/control_loops/double_jointed_arm:ekf",
+        "//frc971/control_loops/double_jointed_arm:trajectory",
+        "@com_github_gflags_gflags//:gflags",
+        "@org_tuxfamily_eigen//:eigen",
     ],
 )
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_) {
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 7ceb001..51d6c4d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -4,14 +4,17 @@
 #include "aos/time/time.h"
 #include "frc971/zeroing/zeroing.h"
 #include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
 #include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
 #include "y2018/control_loops/superstructure/superstructure_position_generated.h"
 #include "y2018/control_loops/superstructure/superstructure_status_generated.h"
 
+using frc971::control_loops::arm::TrajectoryFollower;
+using frc971::control_loops::arm::EKF;
+
 namespace y2018 {
 namespace control_loops {
 namespace superstructure {
@@ -114,6 +117,8 @@
 
   double vmax_ = kVMax();
 
+  frc971::control_loops::arm::Dynamics dynamics_;
+
   ::std::vector<TrajectoryAndParams> trajectories_;
   SearchGraph search_graph_;
 
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
new file mode 100644
index 0000000..9ac7020
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+constexpr double kStallCurrent = 89.0;
+
+constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
+    .l1 = 46.25 * 0.0254,
+    .l2 = 41.80 * 0.0254,
+    .m1 = 9.34 / 2.2,
+    .m2 = 9.77 / 2.2,
+
+    // Moment of inertia of the joints in kg m^2
+    .j1 = 2957.05 * 0.0002932545454545454,
+    .j2 = 2824.70 * 0.0002932545454545454,
+
+    // Radius of the center of mass of the joints in meters.
+    .r1 = 21.64 * 0.0254,
+    .r2 = 26.70 * 0.0254,
+
+    // Gear ratios for the two joints.
+    .g1 = 140.0,
+    .g2 = 90.0,
+
+    // MiniCIM motor constants.
+    .efficiency_tweak = kEfficiencyTweak,
+    .stall_torque = kStallTorque,
+    .free_speed = kFreeSpeed,
+    .stall_current = kStallCurrent,
+    .resistance = 12.0 / kStallCurrent,
+    .Kv = kFreeSpeed / 12.0,
+    .Kt = kStallTorque / kStallCurrent,
+
+    // Number of motors on the distal joint.
+    .num_distal_motors = 2.0,
+};
+
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/y2018/control_loops/superstructure/arm/demo_path.cc
deleted file mode 100644
index 03856c9..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.cc
+++ /dev/null
@@ -1,218 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-
-#include <array>
-#include <initializer_list>
-#include <memory>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::vector<::std::array<double, 6>> FlipPath(
-    ::std::initializer_list<::std::array<double, 6>> list) {
-  ::std::vector<::std::array<double, 6>> result;
-  result.reserve(list.size());
-  for (::std::array<double, 6> point : list) {
-    point[0] = M_PI_2 - point[0];
-    point[1] = M_PI_2 - point[1];
-    point[2] = -point[2];
-    point[3] = -point[3];
-    point[4] = -point[4];
-    point[5] = -point[5];
-    result.push_back(point);
-  }
-  return result;
-}
-
-
-::std::unique_ptr<Path> MakeDemoPath() {
-  return ::std::unique_ptr<Path>(new Path(FlipPath(
-      {{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
-         -0.77079007974101643, -0.21483375398380378, -0.17756921128311187}},
-       {{1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
-         -0.78379235452915641, -0.23084099683729808, -0.18290283743090688}},
-       {{1.4401396868545593, 0.89467165253531666, 0.606891738771278,
-         -0.79478451004733031, -0.24685749210031543, -0.18849894149927623}},
-       {{1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
-         -0.80464215016577489, -0.26311393698286972, -0.19415687958799988}},
-       {{1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
-         -0.81376511301545618, -0.27984413029527577, -0.19986598102433009}},
-       {{1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
-         -0.82237348805962973, -0.29704891442706877, -0.20550961547441812}},
-       {{1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
-         -0.83060217676278458, -0.31482379304256025, -0.21106978462024978}},
-       {{1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
-         -0.83853970802255351, -0.33321287621660045, -0.21650513557965106}},
-       {{1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
-         -0.84624681894089859, -0.35226023809265267, -0.22178093106252236}},
-       {{1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
-         -0.85376633812774205, -0.37188463022164508, -0.22678852570730737}},
-       {{1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
-         -0.86112886314731996, -0.39227952043050979, -0.23159139751917215}},
-       {{1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
-         -0.868356216187261, -0.41330592927121557, -0.23605025316897368}},
-       {{1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
-         -0.87546364639743945, -0.43479093110952094, -0.24001893904573279}},
-       {{1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
-         -0.88246128447218319, -0.45715591769925784, -0.2436817242766752}},
-       {{1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
-         -0.88935513009814537, -0.48005035022840359, -0.24679412703629519}},
-       {{1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
-         -0.89614774079971626, -0.50380737957408728, -0.24947659419788909}},
-       {{1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
-         -0.90283871923908732, -0.52783372117172589, -0.25138272577381626}},
-       {{1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
-         -0.90942506840468851, -0.55267961927927023, -0.25273346631273114}},
-       {{1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
-         -0.91590145113584731, -0.57770009966870806, -0.25318354738409404}},
-       {{1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
-         -0.92226038979969771, -0.60317645612010684, -0.25282447346857195}},
-       {{1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
-         -0.92849242044318914, -0.62907036438628838, -0.25159784841893079}},
-       {{1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
-         -0.93458622156486959, -0.6551663269414254, -0.24938020059543903}},
-       {{1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
-         -0.94052872574050006, -0.68164592193873064, -0.24620769226541636}},
-       {{1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
-         -0.94630522456833177, -0.70757451300020135, -0.24172061458903144}},
-       {{1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
-         -0.95189947515500117, -0.73378453575677771, -0.23620138680600353}},
-       {{1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
-         -0.95729381154041093, -0.75990897245769828, -0.22950448838605128}},
-       {{1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
-         -0.9624692690918778, -0.78523056852177775, -0.22141489322699751}},
-       {{1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
-         -0.96740572540110403, -0.81025448009679157, -0.21209581939563796}},
-       {{1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
-         -0.97208205946674009, -0.83408589705170777, -0.20133249883457041}},
-       {{1.8025869261905516, 0.187811431247703, 0.21562459548465329,
-         -0.97647633551565383, -0.85752536967991499, -0.18935884543741083}},
-       {{1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
-         -0.98056600913243175, -0.8793458743889202, -0.17593847769244828}},
-       {{1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
-         -0.98432816133711831, -0.89983233183814704, -0.16120962647016274}},
-       {{1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
-         -0.98773975706575867, -0.91852354093170219, -0.14517104001739675}},
-       {{1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
-         -0.99077792879471627, -0.93602333307674279, -0.12800867856143744}},
-       {{1.821071685494891, 0.078016673692446373, 0.11452572935697461,
-         -0.99342028231522084, -0.95112385045254688, -0.10965064890920556}},
-       {{1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
-         -0.99564522224667962, -0.96368362602021329, -0.090231797498176183}},
-       {{1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
-         -0.99743229095507402, -0.97395276760288163, -0.069931055757802438}},
-       {{1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
-         -0.99876251555204698, -0.98177893706111263, -0.048889055531294821}},
-       {{1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
-         -0.99961875572762016, -0.98689335559342106, -0.027260179511112551}},
-       {{1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
-         -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384}},
-       {{1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
-         -0.99985190999321838, -0.98812791978941406, 0.017006330622021722}},
-       {{1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
-         -0.99920667718760625, -0.98450390283226485, 0.039237693528591036}},
-       {{1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
-         -0.99804372668560926, -0.97719363903338485, 0.061212532653782022}},
-       {{1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
-         -0.99635971483445407, -0.9676884196650537, 0.08279433361404552}},
-       {{1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
-         -0.99415473957224865, -0.95496997071328249, 0.103708042997466}},
-       {{1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
-         -0.99143244693508337, -0.93935720014890423, 0.12375848445745616}},
-       {{1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
-         -0.98820007362522466, -0.92129127811992539, 0.14279680293047756}},
-       {{1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
-         -0.98446842190585071, -0.90059657633599033, 0.16060369204333666}},
-       {{1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
-         -0.98025176614541798, -0.87754964758424281, 0.17703366424976713}},
-       {{1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
-         -0.97556769186923664, -0.85256620853260556, 0.19199796793136439}},
-       {{1.795600235427889, -0.29568411659857541, -0.24135508622397667,
-         -0.97043687190554384, -0.82563764988942245, 0.20534143268497296}},
-       {{1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
-         -0.96488278369690872, -0.79733324597202848, 0.2170659935022537}},
-       {{1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
-         -0.95893137727265387, -0.76782873106500538, 0.22711183686998365}},
-       {{1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
-         -0.95261070030659223, -0.73738784843863214, 0.23546528518662449}},
-       {{1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
-         -0.94595049253433039, -0.70610753433104578, 0.2420821140887684}},
-       {{1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
-         -0.93898175797923322, -0.67467492193224388, 0.24714550708779132}},
-       {{1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
-         -0.93173632684916963, -0.64280482438985487, 0.25052642309609846}},
-       {{1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
-         -0.92424641493966653, -0.61125522193929116, 0.2525024014306243}},
-       {{1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
-         -0.91654419343972093, -0.58016017720123902, 0.25315181203742848}},
-       {{1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
-         -0.90866137293483673, -0.54964898595174738, 0.25256545404213349}},
-       {{1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
-         -0.9006288116955985, -0.51972307699036913, 0.25078642241782151}},
-       {{1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
-         -0.89247615157546845, -0.49088885529812037, 0.24811508685641395}},
-       {{1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
-         -0.88423148823305242, -0.46296888429931843, 0.24453849915066983}},
-       {{1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
-         -0.87592107415428122, -0.43630053035835431, 0.24031239163872659}},
-       {{1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
-         -0.86756906024763358, -0.41095471910587472, 0.23557116495526012}},
-       {{1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
-         -0.85919727393850864, -0.38675403181117768, 0.23030820687574571}},
-       {{1.5952240262184549, -0.748683349319481, -0.52544910775441078,
-         -0.85082503204836046, -0.36408233991880601, 0.22484810685703166}},
-       {{1.5751964830679697, -0.780545595975073, -0.53874483689504815,
-         -0.84246899095392713, -0.34276938388236106, 0.21919491474821423}},
-       {{1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
-         -0.83414302801658013, -0.32291038254364612, 0.21351295685970414}},
-       {{1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
-         -0.82585815491559456, -0.30444498192901781, 0.20786805829508584}},
-       {{1.509394872119298, -0.87844468632349793, -0.57575473429345714,
-         -0.81762245929198296, -0.28737482236853346, 0.20236376870812761}},
-       {{1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
-         -0.8094410733694728, -0.27173204177162957, 0.1971250636942595}},
-       {{1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
-         -0.80131616705547515, -0.25741396251510884, 0.19217806498346204}},
-       {{1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
-         -0.79324696313473042, -0.24447483076597848, 0.18765975242966434}},
-       {{1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
-         -0.78522977444184905, -0.2328103488745005, 0.18358578028961331}},
-       {{1.3812640184459468, -1.047635918033063, -0.62918193604047223,
-         -0.77725805969469564, -0.22240924347971955, 0.18003762666126619}},
-       {{1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
-         -0.76932249829443622, -0.21321450222713362, 0.17705731568712871}},
-       {{1.324117035769417, -1.1164512699023377, -0.64826936036845306,
-         -0.76141108240389876, -0.20523063810290823, 0.17473421668463424}},
-       {{1.294438654066159, -1.1508834084076087, -0.6574373330314689,
-         -0.75350922564788103, -0.19832793065660315, 0.17304104866571926}},
-       {{1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
-         -0.74559988691553936, -0.1925723524639939, 0.17211494490833781}},
-       {{1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
-         -0.73766370970960415, -0.18786875722059337, 0.17195217715824379}},
-       {{1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
-         -0.72967917440333774, -0.18419866624510314, 0.17261421082264014}},
-       {{1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
-         -0.72162276348679166, -0.18150423850780073, 0.17412530594411615}},
-       {{1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
-         -0.71346913797229905, -0.17977253485932801, 0.17655152952198189}},
-       {{1.104495174566031, -1.3535612797241663, -0.70901706362016093,
-         -0.70519132403585671, -0.17898758721428376, 0.17995840096834861}},
-       {{1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
-         -0.69676090839955884, -0.17907637813730631, 0.18435585053172451}},
-       {{1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
-         -0.68814824095848082, -0.18003314612431659, 0.18982321746977993}},
-       {{1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
-         -0.67932264404179699, -0.18184420373071258, 0.19643734513631667}},
-       {{0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
-         -0.67025262732336799, -0.18446052990619061, 0.20424250843873848}}})));
-}
-
-::std::unique_ptr<Path> MakeReversedDemoPath() {
-  return ::std::unique_ptr<Path>(new Path(Path::Reversed(*MakeDemoPath())));
-}
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
deleted file mode 100644
index 0c18103..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-
-#include <memory>
-
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::unique_ptr<Path> MakeDemoPath();
-::std::unique_ptr<Path> MakeReversedDemoPath();
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
-
-#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
deleted file mode 100644
index 02a2861..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_bool(gravity, true, "If true, enable gravity.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3 =
-    (::Eigen::Matrix<double, 2, 2>()
-         << Dynamics::kG1 * Dynamics::Kt / Dynamics::kResistance,
-     0.0, 0.0, Dynamics::kG2 *Dynamics::kNumDistalMotors *Dynamics::Kt /
-                   Dynamics::kResistance)
-        .finished();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
-    (::Eigen::Matrix<double, 2, 2>()
-         << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
-                (Dynamics::Kv * Dynamics::kResistance),
-     0.0, 0.0,
-     Dynamics::kG2 *Dynamics::kG2 *Dynamics::Kt *Dynamics::kNumDistalMotors /
-         (Dynamics::Kv * Dynamics::kResistance))
-        .finished();
-
-constexpr double Dynamics::kAlpha;
-constexpr double Dynamics::kBeta;
-constexpr double Dynamics::kGamma;
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
deleted file mode 100644
index b1ebc97..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ /dev/null
@@ -1,189 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-
-#include "Eigen/Dense"
-
-#include "frc971/control_loops/runge_kutta.h"
-#include "gflags/gflags.h"
-
-DECLARE_bool(gravity);
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// This class captures the dynamics of our system.  It doesn't actually need to
-// store state yet, so everything can be constexpr and/or static.
-class Dynamics {
- public:
-  // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
-  // Length of the joints in meters.
-  static constexpr double kL1 = 46.25 * 0.0254;
-  static constexpr double kL2 = 41.80 * 0.0254;
-
-  // Mass of the joints in kilograms.
-  static constexpr double kM1 = 9.34 / 2.2;
-  static constexpr double kM2 = 9.77 / 2.2;
-
-  // Moment of inertia of the joints in kg m^2
-  static constexpr double kJ1 = 2957.05 * 0.0002932545454545454;
-  static constexpr double kJ2 = 2824.70 * 0.0002932545454545454;
-
-  // Radius of the center of mass of the joints in meters.
-  static constexpr double r1 = 21.64 * 0.0254;
-  static constexpr double r2 = 26.70 * 0.0254;
-
-  // Gear ratios for the two joints.
-  static constexpr double kG1 = 140.0;
-  static constexpr double kG2 = 90.0;
-
-  // MiniCIM motor constants.
-  static constexpr double kEfficiencyTweak = 0.95;
-  static constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
-  static constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
-  static constexpr double kStallCurrent = 89.0;
-  static constexpr double kResistance = 12.0 / kStallCurrent;
-  static constexpr double Kv = kFreeSpeed / 12.0;
-  static constexpr double Kt = kStallTorque / kStallCurrent;
-
-  // Number of motors on the distal joint.
-  static constexpr double kNumDistalMotors = 2.0;
-
-  static constexpr double kAlpha = kJ1 + r1 * r1 * kM1 + kL1 * kL1 * kM2;
-  static constexpr double kBeta = kL1 * r2 * kM2;
-  static constexpr double kGamma = kJ2 + r2 * r2 * kM2;
-
-  // K3, K4 matricies described below.
-  static const ::Eigen::Matrix<double, 2, 2> K3;
-  static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
-  static const ::Eigen::Matrix<double, 2, 2> K4;
-
-  // Generates K1-2 for the arm ODE.
-  // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
-  // These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
-  // You probbaly want MatriciesForState.
-  static void NormilizedMatriciesForState(
-      const ::Eigen::Matrix<double, 4, 1> &X,
-      ::Eigen::Matrix<double, 2, 2> *K1_result,
-      ::Eigen::Matrix<double, 2, 2> *K2_result) {
-    const double angle = X(0, 0) - X(2, 0);
-    const double s = ::std::sin(angle);
-    const double c = ::std::cos(angle);
-    *K1_result << kAlpha, c * kBeta, c * kBeta, kGamma;
-    *K2_result << 0.0, s * kBeta, -s * kBeta, 0.0;
-  }
-
-  // Generates K1-2 for the arm ODE.
-  // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
-  static void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
-                                ::Eigen::Matrix<double, 2, 2> *K1_result,
-                                ::Eigen::Matrix<double, 2, 2> *K2_result) {
-    NormilizedMatriciesForState(X, K1_result, K2_result);
-    (*K2_result)(1, 0) *= X(1, 0);
-    (*K2_result)(0, 1) *= X(3, 0);
-  }
-
-  // TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
-
-  // Calculates the acceleration given the current state and control input.
-  static const ::Eigen::Matrix<double, 4, 1> Acceleration(
-      const ::Eigen::Matrix<double, 4, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &U) {
-    ::Eigen::Matrix<double, 2, 2> K1;
-    ::Eigen::Matrix<double, 2, 2> K2;
-
-    MatriciesForState(X, &K1, &K2);
-
-    const ::Eigen::Matrix<double, 2, 1> velocity =
-        (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
-    const ::Eigen::Matrix<double, 2, 1> torque = K3 * U - K4 * velocity;
-    const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
-    const ::Eigen::Matrix<double, 2, 1> accel =
-        K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
-    return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
-            accel(1, 0))
-        .finished();
-  }
-
-  // Calculates the acceleration given the current augmented kalman filter state
-  // and control input.
-  static const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
-      const ::Eigen::Matrix<double, 6, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &U) {
-    ::Eigen::Matrix<double, 2, 2> K1;
-    ::Eigen::Matrix<double, 2, 2> K2;
-
-    MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
-
-    const ::Eigen::Matrix<double, 2, 1> velocity =
-        (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
-    const ::Eigen::Matrix<double, 2, 1> torque =
-        K3 *
-            (U +
-             (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
-        K4 * velocity;
-    const ::Eigen::Matrix<double, 2, 1> gravity_torque =
-        GravityTorque(X.block<4, 1>(0, 0));
-
-    const ::Eigen::Matrix<double, 2, 1> accel =
-        K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
-    return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
-            accel(1, 0), 0.0, 0.0)
-        .finished();
-  }
-
-  // Calculates the voltage required to follow the trajectory.  This requires
-  // knowing the current state, desired angular velocity and acceleration.
-  static const ::Eigen::Matrix<double, 2, 1> FF_U(
-      const ::Eigen::Matrix<double, 4, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &omega_t,
-      const ::Eigen::Matrix<double, 2, 1> &alpha_t) {
-    ::Eigen::Matrix<double, 2, 2> K1;
-    ::Eigen::Matrix<double, 2, 2> K2;
-
-    MatriciesForState(X, &K1, &K2);
-
-    const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
-    return K3_inverse *
-           (K1 * alpha_t + K2 * omega_t + K4 * omega_t - gravity_torque);
-  }
-
-  static ::Eigen::Matrix<double, 2, 1> GravityTorque(
-      const ::Eigen::Matrix<double, 4, 1> &X) {
-    constexpr double kAccelDueToGravity = 9.8 * kEfficiencyTweak;
-    return (::Eigen::Matrix<double, 2, 1>() << (r1 * kM1 + kL1 * kM2) *
-                                                   ::std::sin(X(0)) *
-                                                   kAccelDueToGravity,
-            r2 * kM2 * ::std::sin(X(2)) * kAccelDueToGravity)
-               .finished() *
-           (FLAGS_gravity ? 1.0 : 0.0);
-  }
-
-  static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
-      const ::Eigen::Matrix<double, 4, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
-    return ::frc971::control_loops::RungeKuttaU(Dynamics::Acceleration, X, U,
-                                                dt);
-  }
-
-  static const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
-      const ::Eigen::Matrix<double, 6, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
-    return ::frc971::control_loops::RungeKuttaU(Dynamics::EKFAcceleration, X, U,
-                                                dt);
-  }
-};
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
-
-#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
deleted file mode 100644
index 23e5adc..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics_test.cc
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that zero inputs result in no acceleration and no motion.
-// This isn't all that rigerous, but it's a good start.
-TEST(DynamicsTest, Acceleration) {
-  Dynamics dynamics;
-
-  EXPECT_TRUE(dynamics
-                  .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
-                                ::Eigen::Matrix<double, 2, 1>::Zero())
-                  .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
-  EXPECT_TRUE(dynamics
-                  .UnboundedDiscreteDynamics(
-                      ::Eigen::Matrix<double, 4, 1>::Zero(),
-                      ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
-                  .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
-  const ::Eigen::Matrix<double, 4, 1> X =
-      (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
-
-  ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
-                               ::Eigen::Matrix<double, 2, 1>::Zero())
-              << ::std::endl;
-
-  ::std::cout << dynamics.UnboundedDiscreteDynamics(
-                     X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
-                                      ::Eigen::Matrix<double, 2, 1>::Zero()),
-                     0.01)
-              << ::std::endl;
-
-  EXPECT_TRUE(dynamics
-                  .UnboundedDiscreteDynamics(
-                      X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
-                                       ::Eigen::Matrix<double, 2, 1>::Zero()),
-                      0.01)
-                  .isApprox(X));
-}
-
-}  // namespace testing
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/ekf.cc b/y2018/control_loops/superstructure/arm/ekf.cc
deleted file mode 100644
index 48a71f4..0000000
--- a/y2018/control_loops/superstructure/arm/ekf.cc
+++ /dev/null
@@ -1,111 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-
-#include "Eigen/Dense"
-#include <iostream>
-
-#include "frc971/control_loops/jacobian.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_double(proximal_voltage_error_uncertainty, 8.0,
-              "Proximal joint voltage error uncertainty.");
-DEFINE_double(distal_voltage_error_uncertainty, 2.0,
-              "Distal joint voltage error uncertainty.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-namespace {
-// TODO(austin): When tuning this, make sure to verify that you are waiting
-// enough cycles to make sure it converges at startup. Otherwise you will have a
-// bad day.
-::Eigen::Matrix<double, 6, 6> Q_covariance(
-    (::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
-     ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
-     ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
-     ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
-        .finished()
-        .asDiagonal());
-}  // namespace
-
-EKF::EKF() {
-  X_hat_.setZero();
-  Q_covariance =
-      ((::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
-        ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
-        ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
-        ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
-           .finished()
-           .asDiagonal());
-  P_ = Q_covariance;
-  P_reset_ = P_;
-  //::std::cout << "Reset P: " << P_ << ::std::endl;
-  // TODO(austin): Running the EKF 2000 cycles works, but isn't super clever.
-  // We could just solve the DARE.
-
-  for (int i = 0; i < 1000; ++i) {
-    Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
-    Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
-  }
-  P_half_converged_ = P_;
-  //::std::cout << "Stabilized P: " << P_ << ::std::endl;
-  for (int i = 0; i < 1000; ++i) {
-    Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
-    Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
-  }
-  //::std::cout << "Really stabilized P: " << P_ << ::std::endl;
-  P_converged_ = P_;
-
-  Reset(::Eigen::Matrix<double, 4, 1>::Zero());
-}
-
-void EKF::Reset(const ::Eigen::Matrix<double, 4, 1> &X) {
-  X_hat_.setZero();
-  P_ = P_converged_;
-  X_hat_.block<4, 1>(0, 0) = X;
-}
-
-void EKF::Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
-  const ::Eigen::Matrix<double, 6, 6> A =
-      ::frc971::control_loops::NumericalJacobianX<6, 2>(
-          Dynamics::UnboundedEKFDiscreteDynamics, X_hat_, U, dt);
-
-  X_hat_ = Dynamics::UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
-  P_ = A * P_ * A.transpose() + Q_covariance;
-}
-
-void EKF::Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double /*dt*/) {
-  const ::Eigen::Matrix<double, 2, 2> R_covariance(
-      (::Eigen::DiagonalMatrix<double, 2>().diagonal() << ::std::pow(0.01, 2),
-       ::std::pow(0.01, 2))
-          .finished()
-          .asDiagonal());
-  // H is the jacobian of the h(x) measurement prediction function
-  const ::Eigen::Matrix<double, 2, 6> H_jacobian =
-      (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
-          .finished();
-
-  // Update step Measurement residual error of proximal and distal joint
-  // angles.
-  const ::Eigen::Matrix<double, 2, 1> Y_hat =
-      Y - (::Eigen::Matrix<double, 2, 1>() << X_hat_(0), X_hat_(2)).finished();
-  // Residual covariance
-  const ::Eigen::Matrix<double, 2, 2> S =
-      H_jacobian * P_ * H_jacobian.transpose() + R_covariance;
-
-  // K is the Near-optimal Kalman gain
-  const ::Eigen::Matrix<double, 6, 2> kalman_gain =
-      P_ * H_jacobian.transpose() * S.inverse();
-  // Updated state estimate
-  X_hat_ = X_hat_ + kalman_gain * Y_hat;
-  // Updated covariance estimate
-  P_ = (::Eigen::Matrix<double, 6, 6>::Identity() - kalman_gain * H_jacobian) *
-       P_;
-}
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/y2018/control_loops/superstructure/arm/ekf.h
deleted file mode 100644
index 9ae8b47..0000000
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-
-#include "Eigen/Dense"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// An extended kalman filter for the Arm.
-// Our states are:
-//   [theta0, omega0, theta1, omega1, voltage error0, voltage error1]
-class EKF {
- public:
-  EKF();
-
-  // Resets the internal state back to X.  Resets the torque disturbance to 0.
-  void Reset(const ::Eigen::Matrix<double, 4, 1> &X);
-  // TODO(austin): Offset the internal state when we calibrate.
-
-  // Corrects the state estimate with the provided sensor reading.
-  void Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double dt);
-
-  // Predicts the next state and covariance given the control input.
-  void Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt);
-
-  // Returns the current state and covariance estimates.
-  const ::Eigen::Matrix<double, 6, 1> &X_hat() const { return X_hat_; }
-  double X_hat(int i) const { return X_hat_(i); }
-  const ::Eigen::Matrix<double, 6, 6> &P() const { return P_; }
-  const ::Eigen::Matrix<double, 6, 6> &P_reset() const { return P_reset_; }
-  const ::Eigen::Matrix<double, 6, 6> &P_half_converged() const {
-    return P_half_converged_;
-  }
-  const ::Eigen::Matrix<double, 6, 6> &P_converged() const {
-    return P_converged_;
-  }
-
- private:
-  ::Eigen::Matrix<double, 6, 1> X_hat_;
-  ::Eigen::Matrix<double, 6, 6> P_;
-  ::Eigen::Matrix<double, 6, 6> P_half_converged_;
-  ::Eigen::Matrix<double, 6, 6> P_converged_;
-  ::Eigen::Matrix<double, 6, 6> P_reset_;
-};
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
-
-#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
deleted file mode 100644
index ad6f982..0000000
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ /dev/null
@@ -1,73 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
-
-#include <algorithm>
-#include <cassert>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
-    : SearchGraph(num_vertexes, ::std::vector<Edge>(edges)) {}
-
-SearchGraph::SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges)
-    : edges_(::std::move(edges)) {
-  vertexes_.resize(num_vertexes);
-  size_t i = 0;
-  for (const auto &edge : edges_) {
-    assert(edge.start < num_vertexes);
-    assert(edge.end < num_vertexes);
-    assert(edge.start != edge.end);
-    assert(edge.cost > 0);
-    vertexes_[edge.start].forward.emplace_back(HalfEdge{edge.end, i});
-    vertexes_[edge.end].reverse.emplace_back(HalfEdge{edge.start, i});
-    ++i;
-  }
-  for (auto &vertex : vertexes_) {
-    ::std::sort(vertex.forward.begin(), vertex.forward.end());
-    ::std::sort(vertex.reverse.begin(), vertex.reverse.end());
-  }
-  vertex_heap_.Reserve(num_vertexes);
-  SetGoal(0);
-}
-
-SearchGraph::~SearchGraph() {}
-
-void SearchGraph::SetGoal(size_t vertex) {
-  if (last_searched_vertex_ == vertex) {
-    return;
-  }
-  assert(vertex < vertexes_.size());
-  vertex_heap_.Clear();
-
-  for (auto &vertex : vertexes_) {
-    vertex.cached_distance = std::numeric_limits<double>::infinity();
-  }
-
-  vertexes_[vertex].cached_distance = 0.0;
-  vertex_heap_.InsertOrDecrease(&vertexes_[vertex]);
-
-  while (!vertex_heap_.Empty()) {
-    Vertex *vertex = vertex_heap_.PopMin();
-    for (const auto &half_edge : vertex->reverse) {
-      Vertex *to_adjust = &vertexes_[half_edge.dest];
-      double adjust_cost = vertex->cached_distance + GetCost(half_edge);
-      if (adjust_cost < to_adjust->cached_distance) {
-        to_adjust->cached_distance = adjust_cost;
-        vertex_heap_.InsertOrDecrease(to_adjust);
-      }
-    }
-  }
-  last_searched_vertex_ = vertex;
-}
-
-double SearchGraph::GetCostToGoal(size_t vertex) {
-  assert(vertex < vertexes_.size());
-  return vertexes_[vertex].cached_distance;
-}
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/y2018/control_loops/superstructure/arm/graph.h
deleted file mode 100644
index 2300d6a..0000000
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ /dev/null
@@ -1,190 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-
-#include <algorithm>
-#include <cassert>
-#include <cstdlib>
-#include <limits>
-#include <memory>
-#include <vector>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// Grr... normal priority queues don't allow modifying the node cost.
-// This relies on pointer stability for the passed in T.
-template <typename T>
-class IntrusivePriorityQueue {
- public:
-  // T is expected to be like so:
-  //
-  // struct T {
-  //   IntrusivePriorityQueue<T>::QueueEntry* entry_ = nullptr;
-  // };
-  //
-  // This QueueEntry is like a unique_ptr that always maintains a link
-  // back to the unique_ptr pointing to that entry. This maintains the
-  // mapping between the elements and their indexes in the list.
-  // This mapping is crucial for supporting O(log(n)) decrease-key
-  // operations.
-  class QueueEntry {
-   public:
-    QueueEntry(T *value) : value_(value) { value_->entry_ = this; }
-
-    QueueEntry(QueueEntry &&o) : value_(::std::move(o.value_)) {
-      if (value_) {
-        value_->entry_ = this;
-      }
-      o.value_ = nullptr;
-    }
-
-    QueueEntry &operator=(QueueEntry &&o) {
-      if (this == &o) return *this;
-      std::swap(value_, o.value_);
-      if (value_) value_->entry_ = this;
-      if (o.value_) o.value_->entry_ = this;
-      return *this;
-    }
-
-    ~QueueEntry() {
-      if (value_) value_->entry_ = nullptr;
-    }
-
-    T *get() { return value_; }
-
-    bool operator<(const QueueEntry &o) const { return *value_ < *o.value_; }
-
-   private:
-    friend class IntrusivePriorityQueue;
-    T *value_ = nullptr;
-  };
-
-  // Conceptually, nodes start at inifinity and can only decrease in cost.
-  // The infinity nodes are not in the queue.
-  void InsertOrDecrease(T *t) {
-    if (t->entry_ != nullptr) {
-      std::push_heap(queue_.begin(), queue_.begin() + (GetIndex(t) + 1));
-    } else {
-      queue_.emplace_back(t);
-      std::push_heap(queue_.begin(), queue_.end());
-    }
-  }
-
-  // For testing.
-  void VerifyCorrectness() {
-    for (size_t i = 0; i < queue_.size(); ++i) {
-      assert(queue_[i].value_->entry_ == &queue_[i]);
-    }
-
-    assert(std::is_heap(queue_.begin(), queue_.end()));
-  }
-
-  T *PopMin() {
-    T *result = queue_.front().get();
-    std::pop_heap(queue_.begin(), queue_.end());
-    queue_.pop_back();
-    return result;
-  }
-
-  bool Empty() const { return queue_.empty(); }
-
-  void Clear() { queue_.clear(); }
-
-  void Reserve(size_t n) { queue_.reserve(n); }
-
- private:
-  size_t GetIndex(T *t) { return t->entry_ - &queue_[0]; }
-
-  std::vector<QueueEntry> queue_;
-};
-
-// You pass in a graph (with num_vertexes) and a set of edges described by
-// edges.
-// Then, this will search that graph to find the optimal path from all points
-// to a goal point.
-//
-// A traversal algorithm would be as such:
-// // Initialize graph...
-// graph.SetGoal(my_goal);
-// size_t current_node = node;
-//
-// while (current_node != my_goal) {
-//   edge = argmin(GetCostToGoal(edge.dest) for edge in
-//   Neighbors(current_node));
-//   Travel along edge # edge.edge_id.
-//   current_node = edge.dest;
-// }
-class SearchGraph {
- public:
-  struct Edge {
-    size_t start;
-    size_t end;
-    double cost;
-  };
-
-  // The part of an edge stored at every vertex.
-  struct HalfEdge {
-    size_t dest;
-    // This id matches the corresponding id in the edges array from the
-    // constructor.
-    size_t edge_id;
-    bool operator<(const HalfEdge &o) const { return dest < o.dest; }
-  };
-
-  SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges);
-  SearchGraph(size_t num_vertexes, ::std::initializer_list<Edge> edges);
-  SearchGraph(SearchGraph &&o)
-      : vertexes_(::std::move(o.vertexes_)),
-        edges_(::std::move(o.edges_)),
-        vertex_heap_(::std::move(o.vertex_heap_)) {
-    last_searched_vertex_ = std::numeric_limits<size_t>::max();
-  }
-
-  ~SearchGraph();
-
-  // Set the goal vertex.
-  void SetGoal(size_t vertex);
-
-  // Compute the cost if you're at vertex.
-  double GetCostToGoal(size_t vertex);
-
-  // For walking the graph yourself.
-  const std::vector<HalfEdge> &Neighbors(size_t vertex) {
-    assert(vertex < vertexes_.size());
-    return vertexes_[vertex].forward;
-  }
-
-  size_t num_vertexes() const { return vertexes_.size(); }
-
-  const std::vector<Edge> &edges() const { return edges_; }
-
- private:
-  struct Vertex {
-    std::vector<HalfEdge> forward;
-    std::vector<HalfEdge> reverse;
-    double cached_distance = std::numeric_limits<double>::infinity();
-
-    bool operator<(const Vertex &o) {
-      return cached_distance > o.cached_distance;
-    }
-
-    IntrusivePriorityQueue<Vertex>::QueueEntry *entry_ = nullptr;
-  };
-
-  double GetCost(const HalfEdge &edge) { return edges_[edge.edge_id].cost; }
-  std::vector<Vertex> vertexes_;
-  std::vector<Edge> edges_;
-
-  IntrusivePriorityQueue<Vertex> vertex_heap_;
-
-  size_t last_searched_vertex_ = std::numeric_limits<size_t>::max();
-};
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
-
-#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/y2018/control_loops/superstructure/arm/graph_test.cc
deleted file mode 100644
index 8f246a5..0000000
--- a/y2018/control_loops/superstructure/arm/graph_test.cc
+++ /dev/null
@@ -1,73 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-class HeapTest {
- public:
-  explicit HeapTest(size_t id, double cost) : id_(id), cost_(cost) {}
-
-  void SetCost(double cost) { cost_ = cost; }
-
-  bool operator<(const HeapTest &o) { return cost_ > o.cost_; }
-
- private:
-  size_t id_;
-  double cost_;
-
-  IntrusivePriorityQueue<HeapTest>::QueueEntry *entry_ = nullptr;
-  friend class IntrusivePriorityQueue<HeapTest>;
-};
-
-TEST(IntrusivePriorityQueue, HeapTest) {
-  std::vector<HeapTest> items;
-  IntrusivePriorityQueue<HeapTest> queue;
-  size_t i = 0;
-  for (double value :
-       {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0}) {
-    items.push_back(HeapTest(i, value));
-    ++i;
-  }
-  for (auto &item : items) {
-    queue.InsertOrDecrease(&item);
-    queue.VerifyCorrectness();
-  }
-
-  items[6].SetCost(-1.0);
-  queue.InsertOrDecrease(&items[6]);
-  queue.VerifyCorrectness();
-
-  while (!queue.Empty()) {
-    queue.PopMin();
-    queue.VerifyCorrectness();
-  }
-}
-
-TEST(GraphTest, Distances) {
-  SearchGraph graph(6, {{0, 1, 4.0},
-                        {1, 5, 2.0},
-                        {2, 0, 7.0},
-                        {2, 3, 8.0},
-                        {3, 1, 2.0},
-                        {4, 0, 1.0},
-                        {5, 2, 9.0},
-                        {5, 4, 8.0}});
-
-  graph.SetGoal(2);
-  size_t i = 0;
-  for (double expected_cost : {15, 11, 0, 13, 16, 9}) {
-    EXPECT_EQ(graph.GetCostToGoal(i), expected_cost);
-    ++i;
-  }
-}
-
-}  // namespace testing
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
deleted file mode 100644
index c6e09dd..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ /dev/null
@@ -1,618 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-#include "Eigen/Dense"
-#include "aos/logging/logging.h"
-#include "frc971/control_loops/dlqr.h"
-#include "frc971/control_loops/jacobian.h"
-#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
-DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
-DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
-DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-Path Path::Reversed(const Path &p) {
-  ::std::vector<::std::array<double, 6>> list(p.distances().size());
-  for (ssize_t i = list.size() - 1; i >= 0; --i) {
-    list[i][0] = p.thetas_[list.size() - 1 - i][0];
-    list[i][1] = p.thetas_[list.size() - 1 - i][1];
-
-    list[i][2] = -p.omegas_[list.size() - 1 - i][0];
-    list[i][3] = -p.omegas_[list.size() - 1 - i][1];
-
-    list[i][4] = p.alphas_[list.size() - 1 - i][0];
-    list[i][5] = p.alphas_[list.size() - 1 - i][1];
-  }
-  return Path(list);
-}
-
-::std::unique_ptr<Path> Path::Reversed(::std::unique_ptr<Path> p) {
-  return ::std::unique_ptr<Path>(new Path(Path::Reversed(*p)));
-}
-
-Path::Path(::std::vector<::std::array<double, 6>> list) {
-  distances_.reserve(list.size());
-  ::Eigen::Matrix<double, 2, 1> last_theta;
-  bool first = true;
-
-  // Pull apart the initializer list into points, and compute the distance
-  // lookup for efficient path lookups.
-  for (const auto &e : list) {
-    thetas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
-    omegas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
-    alphas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
-
-    if (first) {
-      distances_.push_back(0.0);
-    } else {
-      distances_.push_back((thetas_.back() - last_theta).norm() +
-                           distances_.back());
-    }
-    first = false;
-    last_theta = thetas_.back();
-  }
-}
-
-Path::Path(::std::initializer_list<::std::array<double, 6>> list) {
-  distances_.reserve(list.size());
-  ::Eigen::Matrix<double, 2, 1> last_theta;
-  bool first = true;
-
-  // Pull apart the initializer list into points, and compute the distance
-  // lookup for efficient path lookups.
-  for (const auto &e : list) {
-    thetas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
-    omegas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
-    alphas_.push_back(
-        (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
-
-    if (first) {
-      distances_.push_back(0.0);
-    } else {
-      distances_.push_back((thetas_.back() - last_theta).norm() +
-                           distances_.back());
-    }
-    first = false;
-    last_theta = thetas_.back();
-  }
-}
-
-::std::vector<double> Trajectory::CurvatureOptimizationPass(
-    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
-  ::std::vector<double> max_dvelocity_unfiltered;
-  max_dvelocity_unfiltered.reserve(num_plan_points_);
-  for (size_t i = 0; i < num_plan_points_; ++i) {
-    const double distance = DistanceForIndex(i);
-
-    // TODO(austin): We are looking up the index in the path 3 times here.
-    const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(distance);
-    const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(distance);
-    const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(distance);
-    const ::Eigen::Matrix<double, 4, 1> X =
-        (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
-            .finished();
-    ::Eigen::Matrix<double, 2, 2> K1;
-    ::Eigen::Matrix<double, 2, 2> K2;
-
-    const ::Eigen::Matrix<double, 2, 1> gravity_volts =
-        Dynamics::K3_inverse * Dynamics::GravityTorque(X);
-
-    Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
-    const ::Eigen::Matrix<double, 2, 2> omega_square =
-        (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
-            .finished();
-
-    // Here, we can say that
-    //   d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
-    // Normalize so that the max accel is 1, and take magnitudes. This
-    // gives us the max velocity we can be at each point due to
-    // curvature.
-    double min_good_ddot =
-        ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
-
-    const ::Eigen::Matrix<double, 2, 1> vk1 =
-        Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
-    const ::Eigen::Matrix<double, 2, 1> vk2 =
-        Dynamics::K3_inverse * Dynamics::K4 * omega;
-
-    // Loop through all the various vmin, plan_vmax combinations.
-    for (const double c : {-plan_vmax, plan_vmax}) {
-      // Also loop through saturating theta0 and theta1
-      for (const ::std::tuple<double, double, double> &abgravity :
-           {::std::tuple<double, double, double>{vk1(0), vk2(0),
-                                                gravity_volts(0)},
-            ::std::tuple<double, double, double>{vk1(1), vk2(1),
-                                                gravity_volts(1)}}) {
-        const double a = ::std::get<0>(abgravity);
-        const double b = ::std::get<1>(abgravity);
-        const double gravity = ::std::get<2>(abgravity);
-        const double sqrt_number = b * b - 4.0 * a * (c - gravity);
-        // Throw out imaginary solutions to the quadratic.
-        if (sqrt_number > 0) {
-          const double sqrt_result = ::std::sqrt(sqrt_number);
-          const double ddot1 = (-b + sqrt_result) / (2.0 * a);
-          const double ddot2 = (-b - sqrt_result) / (2.0 * a);
-          // Loop through both solutions.
-          for (const double ddot : {ddot1, ddot2}) {
-            const ::Eigen::Matrix<double, 2, 1> U =
-                vk1 * ddot * ddot + vk2 * ddot - gravity_volts;
-
-            // Finally, make sure the velocity is positive and valid.
-            if ((U.array().abs() <= plan_vmax + 1e-6).all() && ddot > 0.0) {
-              min_good_ddot = ::std::min(min_good_ddot, ddot);
-            }
-          }
-        }
-      }
-    }
-
-    max_dvelocity_unfiltered.push_back(min_good_ddot);
-  }
-  return max_dvelocity_unfiltered;
-}
-
-double Trajectory::FeasableForwardsAcceleration(
-    double goal_distance, double goal_velocity, double plan_vmax,
-    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
-  // TODO(austin): We are looking up the index in the path 3 times here.
-  const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
-  const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
-  const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
-
-  const ::Eigen::Matrix<double, 4, 1> X =
-      (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
-          .finished();
-
-  ::Eigen::Matrix<double, 2, 2> K1;
-  ::Eigen::Matrix<double, 2, 2> K2;
-
-  Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
-  const ::Eigen::Matrix<double, 2, 2> omega_square =
-      (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
-          .finished();
-
-  const ::Eigen::Matrix<double, 2, 1> k_constant =
-      Dynamics::K3_inverse *
-      ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
-           goal_velocity +
-       Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
-  const ::Eigen::Matrix<double, 2, 1> k_scalar =
-      Dynamics::K3_inverse * K1 * omega;
-
-  const double constraint_goal_acceleration =
-      ::std::sqrt(
-          ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
-                                               goal_velocity * goal_velocity,
-                                           2))) /
-      (alpha_unitizer * omega).norm();
-
-  double goal_acceleration = -::std::numeric_limits<double>::infinity();
-  for (double c : {-plan_vmax, plan_vmax}) {
-    for (const ::std::pair<double, double> &ab :
-         {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
-          ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
-      const double a = ab.first;
-      const double b = ab.second;
-      const double voltage_accel = (c - a) / b;
-      const ::Eigen::Matrix<double, 2, 1> U =
-          k_constant + k_scalar * voltage_accel;
-
-      if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
-        goal_acceleration = ::std::max(voltage_accel, goal_acceleration);
-      }
-    }
-  }
-  if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
-    // TODO(austin): The math above doesn't always give a valid solution.
-    // Figure out why.  It should be an affine transformation.
-    return constraint_goal_acceleration;
-  }
-
-  return ::std::min(constraint_goal_acceleration, goal_acceleration);
-}
-
-double Trajectory::FeasableBackwardsAcceleration(
-    double goal_distance, double goal_velocity, double plan_vmax,
-    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
-  const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
-  const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
-  const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
-  const ::Eigen::Matrix<double, 4, 1> X =
-      (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
-          .finished();
-  ::Eigen::Matrix<double, 2, 2> K1;
-  ::Eigen::Matrix<double, 2, 2> K2;
-
-  Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
-  const ::Eigen::Matrix<double, 2, 2> omega_square =
-      (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
-          .finished();
-
-  const ::Eigen::Matrix<double, 2, 1> k_constant =
-      Dynamics::K3_inverse *
-      ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
-           goal_velocity +
-       Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
-  const ::Eigen::Matrix<double, 2, 1> k_scalar =
-      Dynamics::K3_inverse * K1 * omega;
-
-  const double constraint_goal_acceleration =
-      ::std::sqrt(
-          ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
-                                            goal_velocity * goal_velocity),
-                                           2))) /
-      (alpha_unitizer * omega).norm();
-
-  double goal_acceleration = -::std::numeric_limits<double>::infinity();
-  for (double c : {-plan_vmax, plan_vmax}) {
-    for (const ::std::pair<double, double> &ab :
-         {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
-          ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
-      const double a = ab.first;
-      const double b = ab.second;
-      // This time, we are doing the other pass.  So, find all the
-      // decelerations (and flip them) to find the prior velocity.
-      const double voltage_accel = (c - a) / b;
-
-      const ::Eigen::Matrix<double, 2, 1> U =
-          k_constant + k_scalar * voltage_accel;
-
-      // TODO(austin): This doesn't always give a valid solution.  It really
-      // should.  Figure out why.
-      if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
-        goal_acceleration = ::std::max(-voltage_accel, goal_acceleration);
-      }
-    }
-  }
-  if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
-    return constraint_goal_acceleration;
-  }
-
-  return ::std::min(goal_acceleration, constraint_goal_acceleration);
-}
-
-::std::vector<double> Trajectory::BackwardsOptimizationPass(
-    const ::std::vector<double> &max_dvelocity,
-    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
-  ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
-
-  // Now, iterate over the list of velocities and constrain the acceleration.
-  for (int i = num_plan_points_ - 2; i >= 0; --i) {
-    const double previous_velocity = max_dvelocity_back_pass[i + 1];
-    const double prev_distance = DistanceForIndex(i + 1);
-
-    // Now, integrate with respect to distance (not time like normal).
-    // Subdivide the plan step size into kNumSteps steps.
-    double integrated_distance = 0.0;
-    double integrated_velocity = previous_velocity;
-    constexpr int kNumSteps = 50;
-    for (int j = 0; j < kNumSteps; ++j) {
-      // Compute the acceleration with respect to time.
-      const double acceleration = FeasableBackwardsAcceleration(
-          prev_distance - integrated_distance, integrated_velocity, plan_vmax,
-          alpha_unitizer);
-
-      // And then integrate it with respect to distance.
-      const double integration_step_size_distance =
-          step_size_ / static_cast<double>(kNumSteps);
-      integrated_distance += integration_step_size_distance;
-      integrated_velocity =
-          ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
-                      ::std::pow(integrated_velocity, 2));
-    }
-    max_dvelocity_back_pass[i] =
-        ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
-  }
-
-  return max_dvelocity_back_pass;
-}
-
-::std::vector<double> Trajectory::ForwardsOptimizationPass(
-    const ::std::vector<double> &max_dvelocity,
-    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
-  ::std::vector<double> max_dvelocity_forward_pass = max_dvelocity;
-  // Now, iterate over the list of velocities and constrain the acceleration.
-  for (size_t i = 1; i < num_plan_points_; ++i) {
-    const double previous_velocity = max_dvelocity_forward_pass[i - 1];
-    const double previous_distance = DistanceForIndex(i - 1);
-
-    // Now, integrate with respect to distance (not time like normal).
-    // Subdivide the plan step size into kNumSteps steps.
-    double integrated_distance = 0.0;
-    double integrated_velocity = previous_velocity;
-    constexpr int kNumSteps = 10;
-    for (int j = 0; j < kNumSteps; ++j) {
-      // Compute the acceleration with respect to time.
-      const double acceleration = FeasableForwardsAcceleration(
-          previous_distance + integrated_distance, integrated_velocity,
-          plan_vmax, alpha_unitizer);
-
-      // And then integrate it with respect to distance.
-      const double integration_step_size_distance =
-          step_size_ / static_cast<double>(kNumSteps);
-      integrated_distance += integration_step_size_distance;
-      integrated_velocity =
-          ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
-                      ::std::pow(integrated_velocity, 2));
-    }
-
-    max_dvelocity_forward_pass[i] =
-        ::std::min(integrated_velocity, max_dvelocity_forward_pass[i]);
-  }
-
-  return max_dvelocity_forward_pass;
-}
-
-void TrajectoryFollower::Reset() {
-  next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
-  U_unsaturated_ = U_ff_ = U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
-  goal_acceleration_ = 0.0;
-  saturation_fraction_along_path_ = 0.0;
-  omega_.setZero();
-  if (trajectory_ != nullptr) {
-    set_theta(trajectory_->ThetaT(goal_(0)));
-  }
-}
-
-::Eigen::Matrix<double, 2, 6> TrajectoryFollower::K_at_state(
-    const ::Eigen::Matrix<double, 6, 1> &X,
-    const ::Eigen::Matrix<double, 2, 1> &U) {
-  const double kProximalPos = FLAGS_lqr_proximal_pos;
-  const double kProximalVel = FLAGS_lqr_proximal_vel;
-  const double kDistalPos = FLAGS_lqr_distal_pos;
-  const double kDistalVel = FLAGS_lqr_distal_vel;
-  const ::Eigen::DiagonalMatrix<double, 4> Q =
-      (::Eigen::DiagonalMatrix<double, 4>().diagonal()
-           << 1.0 / ::std::pow(kProximalPos, 2),
-       1.0 / ::std::pow(kProximalVel, 2), 1.0 / ::std::pow(kDistalPos, 2),
-       1.0 / ::std::pow(kDistalVel, 2))
-          .finished()
-          .asDiagonal();
-
-  const ::Eigen::DiagonalMatrix<double, 2> R =
-      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
-           << 1.0 / ::std::pow(12.0, 2),
-       1.0 / ::std::pow(12.0, 2))
-          .finished()
-          .asDiagonal();
-
-  const ::Eigen::Matrix<double, 4, 4> final_A =
-      ::frc971::control_loops::NumericalJacobianX<4, 2>(
-          Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
-  const ::Eigen::Matrix<double, 4, 2> final_B =
-      ::frc971::control_loops::NumericalJacobianU<4, 2>(
-          Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
-
-  ::Eigen::Matrix<double, 4, 4> S;
-  ::Eigen::Matrix<double, 2, 4> sub_K;
-  if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
-    ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
-        final_A - final_B * sub_K);
-
-    ::Eigen::Matrix<double, 2, 6> K;
-    K.setZero();
-    K.block<2, 4>(0, 0) = sub_K;
-    K(0, 4) = 1.0;
-    K(1, 5) = 1.0;
-    failed_solutions_ = 0;
-    last_K_ = K;
-  } else {
-    ++failed_solutions_;
-  }
-  return last_K_;
-}
-
-void TrajectoryFollower::USaturationSearch(
-    double goal_distance, double last_goal_distance, double goal_velocity,
-    double last_goal_velocity, double saturation_fraction_along_path,
-    const ::Eigen::Matrix<double, 2, 6> &K,
-    const ::Eigen::Matrix<double, 6, 1> &X, const Trajectory &trajectory,
-    ::Eigen::Matrix<double, 2, 1> *U, double *saturation_goal_velocity,
-    double *saturation_goal_acceleration) {
-  double saturation_goal_distance =
-      ((goal_distance - last_goal_distance) * saturation_fraction_along_path +
-       last_goal_distance);
-
-  const ::Eigen::Matrix<double, 2, 1> theta_t =
-      trajectory.ThetaT(saturation_goal_distance);
-  *saturation_goal_velocity = trajectory.InterpolateVelocity(
-      saturation_goal_distance, last_goal_distance, goal_distance,
-      last_goal_velocity, goal_velocity);
-  *saturation_goal_acceleration = trajectory.InterpolateAcceleration(
-      last_goal_distance, goal_distance, last_goal_velocity, goal_velocity);
-  const ::Eigen::Matrix<double, 2, 1> omega_t =
-      trajectory.OmegaT(saturation_goal_distance, *saturation_goal_velocity);
-  const ::Eigen::Matrix<double, 2, 1> alpha_t =
-      trajectory.AlphaT(saturation_goal_distance, *saturation_goal_velocity,
-                        *saturation_goal_acceleration);
-  const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
-  const ::Eigen::Matrix<double, 2, 1> U_ff =
-      Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
-
-  *U = U_ff + K * (R - X);
-}
-
-::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
-    const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt) {
-  // Figure out where we would be if we were to accelerate as fast as
-  // our constraints allow.
-  ::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
-      [this, &plan_vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
-        return (::Eigen::Matrix<double, 2, 1>() << X(1),
-                trajectory_->FeasableForwardsAcceleration(
-                    X(0), X(1), plan_vmax, trajectory_->alpha_unitizer()))
-            .finished();
-      },
-      goal, dt);
-
-  // Min that with the backwards pass velocity in case the backwards pass
-  // wants us to slow down.
-  const double next_trajectory_velocity =
-      trajectory_->GetDVelocity(next_goal(0));
-  if (next_trajectory_velocity < next_goal(1)) {
-    next_goal(1) = next_trajectory_velocity;
-    // And then recompute how far to go with our new trajectory in mind.
-    double goal_acceleration = trajectory_->InterpolateAcceleration(
-        goal(0), next_goal(0), goal(1), next_goal(1));
-    next_goal(0) = (goal(0) + goal(1) * dt + 0.5 * dt * dt * goal_acceleration);
-    next_goal(1) = trajectory_->GetDVelocity(next_goal(0));
-  }
-  return next_goal;
-}
-
-void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 6, 1> &X,
-                                bool disabled, double dt, double plan_vmax,
-                                double voltage_limit) {
-  // TODO(austin): Separate voltage limit for shoulder for no path.
-  last_goal_ = goal_;
-
-  if (!has_path()) {
-    // No path, so go to the last theta (no velocity).
-    last_goal_.setZero();
-    next_goal_.setZero();
-    goal_.setZero();
-    goal_acceleration_ = 0.0;
-    saturation_fraction_along_path_ = 0.0;
-
-    if (disabled) {
-      U_ff_.setZero();
-      U_.setZero();
-      U_unsaturated_.setZero();
-    } else {
-      const ::Eigen::Matrix<double, 6, 1> R =
-          trajectory_->R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
-
-      U_ff_ = Dynamics::FF_U(X.block<4, 1>(0, 0),
-                             ::Eigen::Matrix<double, 2, 1>::Zero(),
-                             ::Eigen::Matrix<double, 2, 1>::Zero());
-      const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
-      U_ = U_unsaturated_ = U_ff_ + K * (R - X);
-
-      U_ = U_.array().max(-voltage_limit).min(voltage_limit);
-    }
-    return;
-  }
-
-  if (disabled) {
-    // If we are disabled, it's likely for a bit of time.  So, lets freeze
-    // ourselves on the path (accept the previous motion, but then zero out the
-    // velocity).  Set all outputs to 0 as well.
-    next_goal_(1) = 0.0;
-    goal_ = next_goal_;
-    goal_acceleration_ = 0.0;
-    U_unsaturated_.setZero();
-    U_.setZero();
-    U_ff_.setZero();
-    saturation_fraction_along_path_ = 1.0;
-    theta_ = trajectory_->ThetaT(goal_(0));
-    omega_.setZero();
-    return;
-  }
-
-  // To avoid exposing the new goals before the outer code has a chance to
-  // querry the internal state, move to the new goals here.
-  goal_ = next_goal_;
-
-  if (::std::abs(goal_(0) - path()->length()) < 1e-2) {
-    // If we go backwards along the path near the goal, snap us to the end
-    // point or we'll never actually finish.
-    if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
-        goal_(0) > path()->length()) {
-      goal_(0) = path()->length();
-      goal_(1) = 0.0;
-    }
-  }
-
-  if (goal_(0) == path()->length()) {
-    next_goal_(0) = goal_(0);
-    next_goal_(1) = 0.0;
-    goal_acceleration_ = 0.0;
-  } else {
-    // Figure out where we would be if we were to accelerate as fast as
-    // our constraints allow.
-    next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
-
-    goal_acceleration_ = trajectory_->InterpolateAcceleration(
-        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
-  }
-
-  const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory_->ThetaT(goal_(0));
-  const ::Eigen::Matrix<double, 2, 1> omega_t =
-      trajectory_->OmegaT(goal_(0), goal_(1));
-  const ::Eigen::Matrix<double, 2, 1> alpha_t =
-      trajectory_->AlphaT(goal_(0), goal_(1), goal_acceleration_);
-
-  const ::Eigen::Matrix<double, 6, 1> R = trajectory_->R(theta_t, omega_t);
-
-  U_ff_ = Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
-
-  const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
-  U_ = U_unsaturated_ = U_ff_ + K * (R - X);
-
-  // Ok, now we know if we are staturated or not.  If we are, time to search
-  // between here and our previous goal either until we find a state where we
-  // aren't saturated, or we are really close to our starting point.
-  saturation_fraction_along_path_ = 1.0;
-  if ((U_.array().abs() > voltage_limit).any()) {
-    // Saturated.  Let's do a binary search.
-    double step_size;
-    if ((goal_(0) - last_goal_(0)) < 1e-8) {
-      // print "Not bothering to move"
-      // Avoid the divide by 0 when interpolating.  Just don't move since we
-      // are saturated.
-      saturation_fraction_along_path_ = 0.0;
-      step_size = 0.0;
-    } else {
-      saturation_fraction_along_path_ = 0.5;
-      step_size = 0.5;
-    }
-
-    // Pull us back to the previous point until we aren't saturated anymore.
-    double saturation_goal_velocity;
-    double saturation_goal_acceleration;
-    while (step_size > 0.01) {
-      USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
-                        saturation_fraction_along_path_, K, X, *trajectory_,
-                        &U_, &saturation_goal_velocity,
-                        &saturation_goal_acceleration);
-      step_size = step_size * 0.5;
-      if ((U_.array().abs() > voltage_limit).any()) {
-        saturation_fraction_along_path_ -= step_size;
-      } else {
-        saturation_fraction_along_path_ += step_size;
-      }
-    }
-
-    goal_(0) = ((goal_(0) - last_goal_(0)) * saturation_fraction_along_path_ +
-                last_goal_(0));
-    goal_(1) = saturation_goal_velocity;
-
-    next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
-
-    goal_acceleration_ = trajectory_->InterpolateAcceleration(
-        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
-
-    U_ = U_.array().max(-voltage_limit).min(voltage_limit);
-  }
-  theta_ = trajectory_->ThetaT(goal_(0));
-  omega_ = trajectory_->OmegaT(goal_(0), goal_(1));
-}
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
deleted file mode 100644
index a3a4246..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ /dev/null
@@ -1,438 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-
-#include <array>
-#include <initializer_list>
-#include <memory>
-#include <vector>
-
-#include "Eigen/Dense"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// This class represents a path in theta0, theta1 space.  It also returns the
-// position, velocity and acceleration of the path as a function of path
-// distance.
-class Path {
- public:
-  // Constructs a path from an initializer list of (theta0, theta1, omega0,
-  // omega1, alpha0, alpha1)
-  Path(::std::initializer_list<::std::array<double, 6>> list);
-  // TODO(austin): I'd like to not have 2 constructors, but it looks like C++
-  // doesn't want that.
-  Path(::std::vector<::std::array<double, 6>> list);
-
-  static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
-  static Path Reversed(const Path &p);
-
-  // Returns the length of the path in radians.
-  double length() const { return distances_.back(); }
-
-  // Returns theta, omega, and alpha as a function of path distance.  Distances
-  // before the beginning of the path or after the end of the path will get
-  // truncated to the nearest end.
-  ::Eigen::Matrix<double, 2, 1> Theta(double distance) const {
-    return Interpolate(thetas_, distance);
-  }
-  ::Eigen::Matrix<double, 2, 1> Omega(double distance) const {
-    return Interpolate(omegas_, distance);
-  }
-  ::Eigen::Matrix<double, 2, 1> Alpha(double distance) const {
-    return Interpolate(alphas_, distance);
-  }
-
-  const ::std::vector<double> &distances() const { return distances_; }
-
- private:
-  // Interpolates the function represented by the list of points at the provided
-  // distance.  Distances outside the range will get truncated.  This is a
-  // log(n) algorithm so we can run it live on the bot.
-  ::Eigen::Matrix<double, 2, 1> Interpolate(
-      const ::std::vector<::Eigen::Matrix<double, 2, 1>> &points,
-      double distance) const {
-    if (distance <= 0.0) {
-      return points[0];
-    }
-    if (distance >= length()) {
-      return points.back();
-    }
-    // Keep a max and min, and pull them in towards eachother until they bound
-    // our point exactly.
-    size_t after = ::std::distance(
-        distances_.begin(),
-        ::std::lower_bound(distances_.begin(), distances_.end(), distance));
-    size_t before = after - 1;
-    return (distance - distances_[before]) * (points[after] - points[before]) /
-               (distances_[after] - distances_[before]) +
-           points[before];
-  }
-
-  // The list of theta points in the path.
-  ::std::vector<::Eigen::Matrix<double, 2, 1>> thetas_;
-  // The list of omega points in the path.
-  ::std::vector<::Eigen::Matrix<double, 2, 1>> omegas_;
-  // The list of alpha points in the path.
-  ::std::vector<::Eigen::Matrix<double, 2, 1>> alphas_;
-
-  // The distance along the path of each point.
-  ::std::vector<double> distances_;
-};
-
-namespace testing {
-class TrajectoryTest_IndicesForDistanceTest_Test;
-}  // namespace testing
-
-// A trajectory is a path and a set of velocities as a function of distance.
-class Trajectory {
- public:
-  // Constructs a trajectory (but doesn't calculate it) given a path and a step
-  // size.
-  Trajectory(::std::unique_ptr<const Path> path, double gridsize)
-      : path_(::std::move(path)),
-        num_plan_points_(
-            static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
-        step_size_(path_->length() /
-                   static_cast<double>(num_plan_points_ - 1)) {
-    alpha_unitizer_.setZero();
-  }
-
-  // Optimizes the trajectory.  The path will adhere to the constraints that
-  // || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
-  // plan_vmax.
-  void OptimizeTrajectory(const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,
-                          double plan_vmax) {
-    if (path_ == nullptr) {
-      abort();
-    }
-    alpha_unitizer_ = alpha_unitizer;
-
-    max_dvelocity_unfiltered_ =
-        CurvatureOptimizationPass(alpha_unitizer, plan_vmax);
-
-    // We need to start and end the trajectory with 0 velocity.
-    max_dvelocity_unfiltered_[0] = 0.0;
-    max_dvelocity_unfiltered_[max_dvelocity_unfiltered_.size() - 1] = 0.0;
-
-    max_dvelocity_ = BackwardsOptimizationPass(max_dvelocity_unfiltered_,
-                                               alpha_unitizer, plan_vmax);
-    max_dvelocity_forward_pass_ =
-        ForwardsOptimizationPass(max_dvelocity_, alpha_unitizer, plan_vmax);
-  }
-
-  // Returns an array of the distances used in the plan.  The starting point
-  // (0.0), and end point (path->length()) are included in the array.
-  ::std::vector<double> DistanceArray() const {
-    ::std::vector<double> result;
-    result.reserve(num_plan_points_);
-    for (size_t i = 0; i < num_plan_points_; ++i) {
-      result.push_back(DistanceForIndex(i));
-    }
-    return result;
-  }
-
-  // Computes the maximum velocity that we can follow the path while adhering to
-  // the constraints that || angular acceleration * alpha_unitizer || < 1, and
-  // the applied voltage < plan_vmax.  Returns the velocities.
-  ::std::vector<double> CurvatureOptimizationPass(
-      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
-  // Computes the maximum forwards feasable acceleration at a given position
-  // while adhering to the plan_vmax and alpha_unitizer constraints.
-  // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
-  // initial position and velocity for the forwards path.
-  double FeasableForwardsAcceleration(
-      double goal_distance, double goal_velocity, double plan_vmax,
-      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
-
-  // Computes the maximum backwards feasable acceleration at a given position
-  // while adhering to the plan_vmax and alpha_unitizer constraints.
-  // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
-  // initial position and velocity for the backwards path.
-  // Note: positive acceleration means speed up while going in the negative
-  // direction on the path.
-  double FeasableBackwardsAcceleration(
-      double goal_distance, double goal_velocity, double plan_vmax,
-      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
-
-  // Executes the backwards path optimization pass.
-  ::std::vector<double> BackwardsOptimizationPass(
-      const ::std::vector<double> &max_dvelocity,
-      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
-  // Executes the forwards path optimization pass.
-  ::std::vector<double> ForwardsOptimizationPass(
-      const ::std::vector<double> &max_dvelocity,
-      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
-  // Returns the number of points used for the plan.
-  size_t num_plan_points() const { return num_plan_points_; }
-
-  // Returns the curvature only velocity plan.
-  const ::std::vector<double> &max_dvelocity_unfiltered() const {
-    return max_dvelocity_unfiltered_;
-  }
-  // Returns the backwards pass + curvature plan.
-  const ::std::vector<double> &max_dvelocity() const { return max_dvelocity_; }
-  // Returns the full plan.  This isn't useful at runtime since we don't want to
-  // be tied to a specific acceleration plan when we hit saturation.
-  const ::std::vector<double> &max_dvelocity_forward_pass() const {
-    return max_dvelocity_forward_pass_;
-  }
-
-  // Returns the interpolated velocity at the distance d along the path.  The
-  // math assumes constant acceleration from point (d0, v0), to point (d1, v1),
-  // and that we are at point d between the two.
-  static double InterpolateVelocity(double d, double d0, double d1, double v0,
-                                    double v1) {
-    // We don't support negative velocities.  Report 0 velocity in that case.
-    // TODO(austin): Verify this doesn't show up in the real world.
-    if (v0 < 0 || v1 < 0) {
-      return 0.0;
-    }
-    if (d <= d0) {
-      return v0;
-    }
-    if (d >= d1) {
-      return v1;
-    }
-    return ::std::sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0));
-  }
-
-  // Computes the path distance velocity of the plan as a function of the
-  // distance.
-  double GetDVelocity(double distance) const {
-    return GetDVelocity(distance, max_dvelocity_);
-  }
-  double GetDVelocity(double d, const ::std::vector<double> &plan) const {
-    ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
-    const double v0 = plan[indices.first];
-    const double v1 = plan[indices.second];
-    const double d0 = DistanceForIndex(indices.first);
-    const double d1 = DistanceForIndex(indices.second);
-    return InterpolateVelocity(d, d0, d1, v0, v1);
-  }
-
-  // Computes the path distance acceleration of the plan as a function of the
-  // distance.
-  double GetDAcceleration(double distance) const {
-    return GetDAcceleration(distance, max_dvelocity_);
-  }
-  double GetDAcceleration(double distance, const ::std::vector<double> &plan) const {
-    ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
-    const double v0 = plan[indices.first];
-    const double v1 = plan[indices.second];
-    const double d0 = DistanceForIndex(indices.first);
-    const double d1 = DistanceForIndex(indices.second);
-    return InterpolateAcceleration(d0, d1, v0, v1);
-  }
-
-  // Returns the acceleration along the path segment assuming constant
-  // acceleration.
-  static double InterpolateAcceleration(double d0, double d1, double v0,
-                                        double v1) {
-    return 0.5 * (::std::pow(v1, 2) - ::std::pow(v0, 2)) / (d1 - d0);
-  }
-
-  ::Eigen::Matrix<double, 2, 1> ThetaT(double d) const {
-    return path_->Theta(d);
-  }
-
-  // Returns d theta/dt at a specified path distance and velocity.
-  ::Eigen::Matrix<double, 2, 1> OmegaT(double distance, double velocity) const {
-    if (distance > path_->length() || distance < 0.0) {
-      return ::Eigen::Matrix<double, 2, 1>::Zero();
-    } else {
-      return path_->Omega(distance) * velocity;
-    }
-  }
-
-  // Returns d^2 theta/dt^2 at a specified path distance, velocity and
-  // acceleration.
-  ::Eigen::Matrix<double, 2, 1> AlphaT(double distance, double velocity,
-                                       double acceleration) const {
-    if (distance > path_->length() || distance < 0.0) {
-      return ::Eigen::Matrix<double, 2, 1>::Zero();
-    } else {
-      return path_->Alpha(distance) * ::std::pow(velocity, 2) +
-             path_->Omega(distance) * acceleration;
-    }
-  }
-
-  // Converts a theta and omega vector to a full state vector.
-  static ::Eigen::Matrix<double, 6, 1> R(
-      ::Eigen::Matrix<double, 2, 1> theta_t,
-      ::Eigen::Matrix<double, 2, 1> omega_t) {
-    return (::Eigen::Matrix<double, 6, 1>() << theta_t(0, 0), omega_t(0, 0),
-            theta_t(1, 0), omega_t(1, 0), 0.0, 0.0)
-        .finished();
-  }
-
-  const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer() const {
-    return alpha_unitizer_;
-  }
-
-  const Path &path() const { return *path_; }
-
- private:
-  friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
-
-  // Returns the distance along the path for a specific index in the plan.
-  double DistanceForIndex(size_t index) const {
-    return static_cast<double>(index) * step_size_;
-  }
-
-  // Returns the before and after indices for a specific distance in the plan.
-  ::std::pair<size_t, size_t> IndicesForDistance(double distance) const {
-    const double path_length = path_->length();
-    if (distance <= 0.0) {
-      return ::std::pair<size_t, size_t>(0, 1);
-    }
-    const size_t lower_index =
-        ::std::min(static_cast<size_t>(num_plan_points_ - 2),
-                   static_cast<size_t>(::std::floor((num_plan_points_ - 1) *
-                                                    distance / path_length)));
-
-    return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
-  }
-
-  // The path to follow.
-  ::std::unique_ptr<const Path> path_;
-  // The number of points in the plan.
-  const size_t num_plan_points_;
-  // A cached version of the step size since we need this a *lot*.
-  const double step_size_;
-
-  ::std::vector<double> max_dvelocity_unfiltered_;
-  ::std::vector<double> max_dvelocity_;
-  ::std::vector<double> max_dvelocity_forward_pass_;
-
-  ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
-};
-
-// This class tracks the current goal along trajectories and paths.
-class TrajectoryFollower {
- public:
-  TrajectoryFollower(const ::Eigen::Matrix<double, 2, 1> &theta)
-      : trajectory_(nullptr), theta_(theta) {
-    omega_.setZero();
-    last_K_.setZero();
-    Reset();
-  }
-
-  TrajectoryFollower(Trajectory *const trajectory) : trajectory_(trajectory) {
-    last_K_.setZero();
-    Reset();
-  }
-
-  bool has_path() const { return trajectory_ != nullptr; }
-
-  void set_theta(const ::Eigen::Matrix<double, 2, 1> &theta) { theta_ = theta; }
-
-  // Returns the goal distance along the path.
-  const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
-  double goal(int i) const { return goal_(i); }
-
-  // Starts over at the beginning of the path.
-  void Reset();
-
-  // Switches paths and starts at the beginning of the path.
-  void SwitchTrajectory(const Trajectory *trajectory) {
-    trajectory_ = trajectory;
-    Reset();
-  }
-
-  // Returns the controller gain at the provided state.
-  ::Eigen::Matrix<double, 2, 6> K_at_state(
-      const ::Eigen::Matrix<double, 6, 1> &X,
-      const ::Eigen::Matrix<double, 2, 1> &U);
-
-  // Returns the voltage, velocity and acceleration if we were to be partially
-  // along the path.
-  void USaturationSearch(double goal_distance, double last_goal_distance,
-                         double goal_velocity, double last_goal_velocity,
-                         double saturation_fraction_along_path,
-                         const ::Eigen::Matrix<double, 2, 6> &K,
-                         const ::Eigen::Matrix<double, 6, 1> &X,
-                         const Trajectory &trajectory,
-                         ::Eigen::Matrix<double, 2, 1> *U,
-                         double *saturation_goal_velocity,
-                         double *saturation_goal_acceleration);
-
-  // Returns the next goal given a planning plan_vmax and timestep.  This
-  // ignores the
-  // backwards pass.
-  ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
-      const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt);
-
-  // Plans the next cycle and updates the internal state for consumption.
-  void Update(const ::Eigen::Matrix<double, 6, 1> &X, bool disabled, double dt,
-              double plan_vmax, double voltage_limit);
-
-  // Returns the goal acceleration for this cycle.
-  double goal_acceleration() const { return goal_acceleration_; }
-
-  // Returns U(s) for this cycle.
-  const ::Eigen::Matrix<double, 2, 1> &U() const { return U_; }
-  double U(int i) const { return U_(i); }
-  const ::Eigen::Matrix<double, 2, 1> &U_unsaturated() const {
-    return U_unsaturated_;
-  }
-  const ::Eigen::Matrix<double, 2, 1> &U_ff() const { return U_ff_; }
-
-  double saturation_fraction_along_path() const {
-    return saturation_fraction_along_path_;
-  }
-
-  const ::Eigen::Matrix<double, 2, 1> &theta() const { return theta_; }
-  double theta(int i) const { return theta_(i); }
-  const ::Eigen::Matrix<double, 2, 1> &omega() const { return omega_; }
-  double omega(int i) const { return omega_(i); }
-
-  // Distance left on the path before we get to the end of the path.
-  double path_distance_to_go() const {
-    if (has_path()) {
-      return ::std::max(0.0, path()->length() - goal_(0));
-    } else {
-      return 0.0;
-    }
-  }
-
-  const Path *path() const { return &trajectory_->path(); }
-
-  int failed_solutions() const { return failed_solutions_; }
-
- private:
-  // The trajectory plan.
-  const Trajectory *trajectory_ = nullptr;
-
-  // The current goal.
-  ::Eigen::Matrix<double, 2, 1> goal_;
-  // The previously executed goal.
-  ::Eigen::Matrix<double, 2, 1> last_goal_;
-  // The goal to use next cycle.  We always plan 1 cycle ahead.
-  ::Eigen::Matrix<double, 2, 1> next_goal_;
-
-  ::Eigen::Matrix<double, 2, 1> U_;
-  ::Eigen::Matrix<double, 2, 1> U_unsaturated_;
-  ::Eigen::Matrix<double, 2, 1> U_ff_;
-  double goal_acceleration_ = 0.0;
-
-  double saturation_fraction_along_path_ = 1.0;
-
-  // Holds the last valid goal position for when we loose our path.
-  ::Eigen::Matrix<double, 2, 1> theta_;
-  ::Eigen::Matrix<double, 2, 1> omega_;
-
-  ::Eigen::Matrix<double, 2, 6> last_K_;
-  int failed_solutions_ = 0;
-};
-
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
-
-#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 10b39bc..0cee664 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,10 +1,11 @@
 #include "aos/init.h"
 #include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
 #include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
 #include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
 
 DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
 DEFINE_bool(plot, true, "If true, plot");
@@ -16,10 +17,12 @@
 namespace arm {
 
 void Main() {
-  Trajectory trajectory(FLAGS_forwards
-                            ? MakeNeutralToFrontHighPath()
-                            : Path::Reversed(MakeNeutralToFrontHighPath()),
-                        0.001);
+  frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+  frc971::control_loops::arm::Trajectory trajectory(
+      &dynamics,
+      FLAGS_forwards ? MakeNeutralToFrontHighPath()
+                     : Path::Reversed(MakeNeutralToFrontHighPath()),
+      0.001);
 
   constexpr double kAlpha0Max = 30.0;
   constexpr double kAlpha1Max = 50.0;
@@ -96,7 +99,7 @@
 
     const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
     const ::Eigen::Matrix<double, 2, 1> U =
-        Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+        dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
             .array()
             .max(-20)
             .min(20);
@@ -118,7 +121,7 @@
 
     const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
     const ::Eigen::Matrix<double, 2, 1> U =
-        Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+        dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
             .array()
             .max(-20)
             .min(20);
@@ -140,7 +143,7 @@
 
     const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
     const ::Eigen::Matrix<double, 2, 1> U =
-        Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+        dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
             .array()
             .max(-20)
             .min(20);
@@ -156,7 +159,8 @@
     X << theta_t(0), 0.0, theta_t(1), 0.0;
   }
 
-  TrajectoryFollower follower(&trajectory);
+  frc971::control_loops::arm::TrajectoryFollower follower(&dynamics,
+                                                          &trajectory);
 
   ::std::vector<double> t_array;
   ::std::vector<double> theta0_goal_t_array;
@@ -187,7 +191,7 @@
   ::std::vector<double> torque0_hat_t_array;
   ::std::vector<double> torque1_hat_t_array;
 
-  EKF arm_ekf;
+  frc971::control_loops::arm::EKF arm_ekf(&dynamics);
   arm_ekf.Reset(X);
   ::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
   ::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
@@ -236,9 +240,9 @@
     actual_U(0) += 1.0;
 
     const ::Eigen::Matrix<double, 4, 1> xdot =
-        Dynamics::Acceleration(X, actual_U);
+        dynamics.Acceleration(X, actual_U);
 
-    X = Dynamics::UnboundedDiscreteDynamics(X, actual_U, sim_dt);
+    X = dynamics.UnboundedDiscreteDynamics(X, actual_U, sim_dt);
     arm_ekf.Predict(follower.U(), sim_dt);
 
     alpha0_t_array.push_back(xdot(1));
@@ -270,10 +274,14 @@
     plotter.AddLine(distance_array, alpha0_array, "alpha0");
     plotter.AddLine(distance_array, alpha1_array, "alpha1");
 
-    plotter.AddLine(integrated_distance, integrated_theta0_array, "integrated theta0");
-    plotter.AddLine(integrated_distance, integrated_theta1_array, "integrated theta1");
-    plotter.AddLine(integrated_distance, integrated_omega0_array, "integrated omega0");
-    plotter.AddLine(integrated_distance, integrated_omega1_array, "integrated omega1");
+    plotter.AddLine(integrated_distance, integrated_theta0_array,
+                    "integrated theta0");
+    plotter.AddLine(integrated_distance, integrated_theta1_array,
+                    "integrated theta1");
+    plotter.AddLine(integrated_distance, integrated_omega0_array,
+                    "integrated omega0");
+    plotter.AddLine(integrated_distance, integrated_omega1_array,
+                    "integrated omega1");
     plotter.Publish();
 
     plotter.AddFigure();
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
deleted file mode 100644
index b91c5a4..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ /dev/null
@@ -1,209 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-#include "gtest/gtest.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that we can pull out values along the path.
-TEST(TrajectoryTest, Theta) {
-  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{4.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
-
-  EXPECT_TRUE(p.Theta(0.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
-  EXPECT_TRUE(p.Omega(0.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.0).finished()));
-  EXPECT_TRUE(p.Alpha(0.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
-
-  EXPECT_TRUE(p.Theta(1.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
-
-  for (double d = 0.0; d <= 4.0; d += 0.01) {
-    EXPECT_TRUE(p.Theta(d).isApprox(
-        (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
-  }
-}
-
-// Tests a path where picking the right point to interpolate for matters.
-TEST(TrajectoryTest, HarderPath) {
-  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 1.0, 0.0, 1.0, 0.0, 0.0}}});
-
-  EXPECT_TRUE(p.Theta(0.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
-
-  EXPECT_TRUE(p.Theta(1.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.5).finished()));
-}
-
-// Tests that a path with an even number of points works as expected.
-TEST(TrajectoryTest, EvenPointNumbers) {
-  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
-  EXPECT_TRUE(p.Theta(1.5).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
-
-  for (double d = 0.0; d <= 3.0; d += 0.01) {
-    EXPECT_TRUE(p.Theta(d).isApprox(
-        (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
-  }
-}
-
-// Tests that out of range points work as expected.
-TEST(TrajectoryTest, OutOfBounds) {
-  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
-  EXPECT_TRUE(p.Theta(-1.0).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
-  EXPECT_TRUE(p.Theta(10.0).isApprox(
-      (::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
-}
-
-
-// Tests that we can compute the indices of the plan for a given distance correctly.
-TEST(TrajectoryTest, IndicesForDistanceTest) {
-  // Start with a stupid simple plan.
-  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
-          {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
-  Trajectory t(::std::unique_ptr<Path>(new Path(p)), 0.1);
-
-  // 0 - 3.0 every 0.1 should be 31 points.
-  EXPECT_EQ(t.num_plan_points(), 31);
-
-  // Verify that something centered in a grid cell returns the points on either side.
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
-            t.IndicesForDistance(0.05));
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
-            t.IndicesForDistance(0.15));
-
-  // Verify that something on an edge returns the expected result.
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
-            t.IndicesForDistance(0.1));
-
-  // Verify what small deviations result.
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
-            t.IndicesForDistance(0.1001));
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
-            t.IndicesForDistance(0.0999));
-
-  // Verify that blowing past the ends works.
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(29), static_cast<size_t>(30)),
-            t.IndicesForDistance(4.0));
-  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
-            t.IndicesForDistance(-0.1));
-
-  // Verify that the index to distance calculation also works.
-  EXPECT_EQ(0.0, t.DistanceForIndex(0));
-  EXPECT_EQ(0.1, t.DistanceForIndex(1));
-  EXPECT_EQ(3.0, t.DistanceForIndex(30));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, InterpolateVelocity) {
-  // x = 0.5 * a * t^2
-  // v = a * t
-  // a = 2.0
-  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(0.0, 0.0, 1.0, 0.0, 2.0));
-  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(1.0, 0.0, 1.0, 0.0, 2.0));
-  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(-1.0, 0.0, 1.0, 0.0, 2.0));
-  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(20.0, 0.0, 1.0, 0.0, 2.0));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, InterpolateAcceleration) {
-  // x = 0.5 * a * t^2
-  // v = a * t
-  // a = 2.0
-  EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, ReversedPath) {
-  // Tests that a reversed path is actually reversed.
-  ::std::unique_ptr<Path> path = MakeDemoPath();
-  ::std::unique_ptr<Path> reversed_path(
-      new Path(Path::Reversed(*MakeDemoPath())));
-
-  EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
-
-  for (double d = 0; d < path->length(); d += 0.01) {
-    EXPECT_TRUE(path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
-    EXPECT_TRUE(path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
-    EXPECT_TRUE(path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
-  }
-}
-
-// Tests that we can follow a path.  Look at :trajectory_plot if you want to see
-// the path.
-TEST(TrajectoryTest, RunTrajectory) {
-  ::std::unique_ptr<Path> path = MakeDemoPath();
-  Trajectory trajectory(::std::move(path), 0.001);
-
-  constexpr double kAlpha0Max = 40.0;
-  constexpr double kAlpha1Max = 60.0;
-  constexpr double vmax = 11.95;
-
-  const ::Eigen::Matrix<double, 2, 2> alpha_unitizer =
-      (::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max, 0.0, 0.0,
-       1.0 / kAlpha1Max)
-          .finished();
-  trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
-
-  double t = 0;
-  ::Eigen::Matrix<double, 4, 1> X;
-  {
-    ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(0.0);
-    X << theta_t(0), 0.0, theta_t(1), 0.0;
-  }
-
-  EKF arm_ekf;
-  arm_ekf.Reset(X);
-
-  TrajectoryFollower follower(&trajectory);
-  constexpr double sim_dt = 0.00505;
-  while (t < 1.0) {
-    arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
-                    sim_dt);
-    follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
-    X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
-    arm_ekf.Predict(follower.U(), sim_dt);
-    t += sim_dt;
-  }
-
-  ::Eigen::Matrix<double, 4, 1> final_X;
-  ::Eigen::Matrix<double, 2, 1> final_theta_t =
-      trajectory.ThetaT(trajectory.path().length());
-  final_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
-
-  // Verify that we got to the end.
-  EXPECT_TRUE(X.isApprox(final_X, 0.01))
-      << ": X is " << X.transpose() << " final_X is " << final_X.transpose();
-
-  // Verify that our goal is at the end.
-  EXPECT_TRUE(
-      final_theta_t.isApprox(trajectory.path().Theta(follower.goal(0))));
-}
-
-}  // namespace testing
-}  // namespace arm
-}  // namespace superstructure
-}  // namespace control_loops
-}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 285c8c3..54342c5 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,7 +8,8 @@
 #include "frc971/control_loops/team_number_test_environment.h"
 #include "gtest/gtest.h"
 #include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.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"
 #include "y2018/control_loops/superstructure/intake/intake_plant.h"
 #include "y2018/control_loops/superstructure/superstructure.h"
@@ -129,7 +130,8 @@
                               constants::Values::kProximalEncoderRatio()),
         distal_zeroing_constants_(distal_zeroing_constants),
         distal_pot_encoder_(M_PI * 2.0 *
-                            constants::Values::kDistalEncoderRatio()) {
+                            constants::Values::kDistalEncoderRatio()),
+        dynamics_(arm::kArmConstants) {
     X_.setZero();
   }
 
@@ -174,7 +176,7 @@
     AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
 
     if (release_arm_brake) {
-      X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+      X_ = dynamics_.UnboundedDiscreteDynamics(X_, U, 0.00505);
     } else {
       // Well, the brake shouldn't stop both joints, but this will get the tests
       // to pass.
@@ -197,6 +199,8 @@
   const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
       distal_zeroing_constants_;
   PositionSensorSimulator distal_pot_encoder_;
+
+  ::frc971::control_loops::arm::Dynamics dynamics_;
 };
 
 class SuperstructureSimulation {