commit 06b65b8046779587ea2c1ac7c329f3c4154dbdb0
author Austin Schuh <austin.linux@gmail.com> Sun Dec 23 09:21:44 2018 +1100
committer Austin Schuh <austin.linux@gmail.com> Sun Dec 23 09:25:55 2018 +1100
tree e8cdb22f617ab6a2f34be5458b682365196f1c69
parent 5bf7d7e2de8fe648e427c1b9ef1119479b74d731

Move DrivetrainMotorsSS constructor to top

It's hard to find when it's out of order...

Change-Id: Id4465927a33ebfb4eb17217a971e1af05fc4b6da

frc971/control_loops/drivetrain/ssdrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 925256b..0d01622 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -13,6 +13,34 @@
 namespace control_loops {
 namespace drivetrain {
 
+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()),
+      linear_profile_(::aos::controls::kLoopFrequency),
+      angular_profile_(::aos::controls::kLoopFrequency),
+      integrated_kf_heading_(integrated_kf_heading) {
+  ::aos::controls::HPolytope<0>::Init();
+  T_ << 1, 1, 1, -1;
+  T_inverse_ = T_.inverse();
+  unprofiled_goal_.setZero();
+}
+
 void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
   output_was_capped_ = ::std::abs((*U)(0, 0)) > max_voltage_ ||
                        ::std::abs((*U)(1, 0)) > max_voltage_;
@@ -116,34 +144,6 @@
   }
 }
 
-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()),
-      linear_profile_(::aos::controls::kLoopFrequency),
-      angular_profile_(::aos::controls::kLoopFrequency),
-      integrated_kf_heading_(integrated_kf_heading) {
-  ::aos::controls::HPolytope<0>::Init();
-  T_ << 1, 1, 1, -1;
-  T_inverse_ = T_.inverse();
-  unprofiled_goal_.setZero();
-}
-
 void DrivetrainMotorsSS::SetGoal(
     const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
   unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,