commit bcce26ac1640be91a15d3c0c60369f353c51e332
author Austin Schuh <austin.linux@gmail.com> Mon Mar 26 23:41:24 2018 -0700
committer Brian Silverman <bsilver16384@gmail.com> Thu Apr 05 23:09:40 2018 -0400
tree 9564cfbd2adb87593d9ef622646372960cc237e9
parent d0180f18609785dcb224067e110b613baa9a97e8

Drive code works on Tantrum.

Need to write the spring code.  Drive now supports doubles...  What a
pain.

Change-Id: Id589acdc443dcd81242a21e3b0c26f81d6974dc8

frc971/control_loops/drivetrain/ssdrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 39c4ade..ad86b1f 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -59,7 +59,7 @@
               (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
           (U_poly_.static_k() * max_voltage_),
       (position_K * T_).inverse() *
-          ::aos::controls::ShiftPoints<2, 4>(
+          ::aos::controls::ShiftPoints<2, 4, double>(
               (U_poly_.StaticVertices() * max_voltage_),
               -velocity_K * velocity_error + U_integral - kf_->ff_U()));
 
@@ -87,9 +87,9 @@
 
     bool is_inside_h, is_inside_45;
     const auto adjusted_pos_error_h =
-        DoCoerceGoal(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
+        DoCoerceGoal<double>(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
     const auto adjusted_pos_error_45 =
-        DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+        DoCoerceGoal<double>(pos_poly_hv, L45, w45, intersection, &is_inside_45);
     if (pos_poly_hv.IsInside(intersection)) {
       adjusted_pos_error = adjusted_pos_error_h;
     } else {
@@ -116,21 +116,25 @@
   }
 }
 
-DrivetrainMotorsSS::DrivetrainMotorsSS(const DrivetrainConfig &dt_config,
-                                       StateFeedbackLoop<7, 2, 4> *kf,
-                                       double *integrated_kf_heading)
+DrivetrainMotorsSS::DrivetrainMotorsSS(
+    const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *kf,
+    double *integrated_kf_heading)
     : dt_config_(dt_config),
       kf_(kf),
-      U_poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
-               /*[*/ -1, 0 /*]*/,
-               /*[*/ 0, 1 /*]*/,
-               /*[*/ 0, -1 /*]]*/).finished(),
-              (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]*/,
-               /*[*/ 1.0 /*]]*/).finished(),
-              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
-               /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/).finished()),
+      U_poly_(
+          (Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+           /*[*/ -1, 0 /*]*/,
+           /*[*/ 0, 1 /*]*/,
+           /*[*/ 0, -1 /*]]*/)
+              .finished(),
+          (Eigen::Matrix<double, 4, 1>() << /*[[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]*/,
+           /*[*/ 1.0 /*]]*/)
+              .finished(),
+          (Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
+           /*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/)
+              .finished()),
       linear_profile_(::aos::controls::kLoopFrequency),
       angular_profile_(::aos::controls::kLoopFrequency),
       integrated_kf_heading_(integrated_kf_heading) {