commit 651fc3ff0105ab373cd26121e081d7080b5780db
author James Kuszmaul <jabukuszmaul@gmail.com> Wed May 15 21:14:25 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Thu May 16 20:49:03 2019 -0700
tree 2c9e4010ac8fc3e6043a5054136e51d0eef6d218
parent f5f50dd8044247504eec5c89656caac3f8d6588a

Make a DurationInSeconds function

Also, run clang-format on all the files I changed because I am too lazy
to figure out how to call clang-format on just the lines I changed.

Change-Id: I071c6f81dced533a0a269f25a258348132a7056a

frc971/control_loops/c2d.h

diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
index b6f4198..50315e6 100644
--- a/frc971/control_loops/c2d.h
+++ b/frc971/control_loops/c2d.h
@@ -4,6 +4,7 @@
 #include <chrono>
 
 #include <Eigen/Dense>
+#include "aos/time/time.h"
 // We need to include MatrixFunctions for the matrix exponential.
 #include "unsupported/Eigen/MatrixFunctions"
 
@@ -23,11 +24,9 @@
       M_state_continuous;
   M_state_continuous.setZero();
   M_state_continuous.template block<num_states, num_states>(0, 0) =
-      A_continuous *
-      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+      A_continuous * ::aos::time::TypedDurationInSeconds<Scalar>(dt);
   M_state_continuous.template block<num_states, num_inputs>(0, num_states) =
-      B_continuous *
-      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+      B_continuous * ::aos::time::TypedDurationInSeconds<Scalar>(dt);
 
   Eigen::Matrix<Scalar, num_states + num_inputs, num_states + num_inputs>
       M_state = M_state_continuous.exp();
@@ -52,9 +51,7 @@
       A_continuous.transpose();
 
   Eigen::Matrix<Scalar, 2 * num_states, 2 *num_states> phi =
-      (M_gain *
-       ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
-           .count()).exp();
+      (M_gain * ::aos::time::TypedDurationInSeconds<Scalar>(dt)).exp();
 
   // Phi12 = phi[0:num_states, num_states:2*num_states]
   // Phi22 = phi[num_states:2*num_states,
@@ -88,8 +85,7 @@
     Eigen::Matrix<Scalar, num_states, num_states> *A_d) {
   Eigen::Matrix<Scalar, num_states, num_states> Qtemp =
       (Q_continuous + Q_continuous.transpose()) / static_cast<Scalar>(2.0);
-  Scalar dt_d =
-      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+  Scalar dt_d = ::aos::time::TypedDurationInSeconds<Scalar>(dt);
   Eigen::Matrix<Scalar, num_states, num_states> last_term = Qtemp;
   double last_coeff = dt_d;
   const Eigen::Matrix<Scalar, num_states, num_states> At =
@@ -112,8 +108,7 @@
     Atn *= At;
     phi22 += last_coeff * Atn;
   }
-  Eigen::Matrix<Scalar, num_states, num_states> phi22t =
-    phi22.transpose();
+  Eigen::Matrix<Scalar, num_states, num_states> phi22t = phi22.transpose();
 
   Qtemp = phi22t * phi12;
   *Q_d = (Qtemp + Qtemp.transpose()) / static_cast<Scalar>(2.0);