commit bb735b71c8a83e6072f0eaf0ccaaf213eb236cdc
author Austin Schuh <austin.linux@gmail.com> Thu Jan 03 12:58:41 2019 -0800
committer Austin Schuh <austin.linux@gmail.com> Fri Jan 04 11:10:34 2019 -0800
tree f9e247bccdeb12155079190ff7512ebef1e8fb31
parent 662e3c7cf69c2fa38200f79c26f2794a95f8d93b

Convert dt to a chrono::duration in drivetrain config

Change-Id: I8d44e181db3646330fa69ddd809cfff21fe236aa

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 177c0a5..523c9b3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -227,9 +227,9 @@
     Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
         last_accel_;
     kf_.Correct(Y);
-    integrated_kf_heading_ += dt_config_.dt *
-                              (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) /
-                              (dt_config_.robot_radius * 2.0);
+    integrated_kf_heading_ +=
+        chrono::duration_cast<chrono::duration<double>>(dt_config_.dt).count() *
+        (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (dt_config_.robot_radius * 2.0);
 
     // gyro_heading = (real_right - real_left) / width
     // wheel_heading = (wheel_right - wheel_left) / width