Convert dt to a chrono::duration in drivetrain config
Change-Id: I8d44e181db3646330fa69ddd809cfff21fe236aa
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
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 92d15b0..476128d 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -54,10 +54,10 @@
::std::function<StateFeedbackLoop<2, 2, 2, Scalar>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<7, 2, 4, Scalar>()> make_kf_drivetrain_loop;
- Scalar dt; // Control loop time step.
- Scalar robot_radius; // Robot radius, in meters.
- Scalar wheel_radius; // Wheel radius, in meters.
- Scalar v; // Motor velocity constant.
+ ::std::chrono::nanoseconds dt; // Control loop time step.
+ Scalar robot_radius; // Robot radius, in meters.
+ Scalar wheel_radius; // Wheel radius, in meters.
+ Scalar v; // Motor velocity constant.
// Gear ratios, from wheel to motor shaft.
Scalar high_gear_ratio;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 1c11ae2..383059f 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -44,7 +44,8 @@
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- ::y2016::control_loops::drivetrain::kDt,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
::y2016::control_loops::drivetrain::kRobotRadius,
::y2016::control_loops::drivetrain::kWheelRadius,
::y2016::control_loops::drivetrain::kV,
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index c09c527..64b1aff 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -372,11 +372,14 @@
current_left_velocity_ = kZero;
current_right_velocity_ = kZero;
} else {
+ const Scalar dt =
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(
+ dt_config_.dt)
+ .count();
current_left_velocity_ =
- (position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
+ (position_.left_encoder - last_position_.left_encoder) / dt;
current_right_velocity_ =
- (position_.right_encoder - last_position_.right_encoder) /
- dt_config_.dt;
+ (position_.right_encoder - last_position_.right_encoder) / dt;
left_motor_speed_ =
MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
current_left_velocity_, left_gear_);
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 36d489f..91e2229 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -3,6 +3,7 @@
#include <inttypes.h>
#include <stdio.h>
#include <atomic>
+#include <chrono>
#include <cmath>
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
@@ -28,6 +29,8 @@
using ::frc971::control_loops::DrivetrainQueue_Output;
using ::motors::seems_reasonable::Spring;
+namespace chrono = ::std::chrono;
+
struct SimpleAdcReadings {
uint16_t sin, cos;
};
@@ -67,7 +70,9 @@
::motors::seems_reasonable::MakeVelocityDrivetrainLoop,
::std::function<StateFeedbackLoop<7, 2, 4, float>()>(),
- ::motors::seems_reasonable::kDt, ::motors::seems_reasonable::kRobotRadius,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<float>(::motors::seems_reasonable::kDt)),
+ ::motors::seems_reasonable::kRobotRadius,
::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV,
::motors::seems_reasonable::kHighGearRatio,
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
index b94c4f0..8ca87bc 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -1,14 +1,17 @@
#include "y2012/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2012/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2012/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include "y2012/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2012/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2012 {
namespace control_loops {
namespace drivetrain {
@@ -28,7 +31,8 @@
::y2012::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2012::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
drivetrain::kRobotRadius,
drivetrain::kWheelRadius,
drivetrain::kV,
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index c3357aa..07a74c4 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -1,14 +1,17 @@
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
-#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2014/constants.h"
+#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2014/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2014 {
namespace control_loops {
@@ -24,7 +27,8 @@
::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
drivetrain::kRobotRadius,
drivetrain::kWheelRadius,
drivetrain::kV,
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index 2daaf6a..0380564 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -1,14 +1,17 @@
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include "y2014_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2014_bot3 {
namespace control_loops {
namespace drivetrain {
@@ -28,8 +31,9 @@
::y2014_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2014_bot3::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
- drivetrain::kV,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
drivetrain::kHighGearRatio, drivetrain::kLowGearRatio,
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index b469f77..bee3666 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -1,15 +1,18 @@
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2016 {
namespace control_loops {
namespace drivetrain {
@@ -29,7 +32,8 @@
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
drivetrain::kRobotRadius,
drivetrain::kWheelRadius,
drivetrain::kV,
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index b351cbf..ecea96c 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -1,15 +1,18 @@
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2017/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2017/constants.h"
+#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2017/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2017 {
namespace control_loops {
namespace drivetrain {
@@ -29,7 +32,8 @@
::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2017::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
drivetrain::kRobotRadius,
drivetrain::kWheelRadius,
drivetrain::kV,
diff --git a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
index f4be260..b9b5728 100644
--- a/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -1,14 +1,17 @@
#include "y2017_bot3/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include "y2017_bot3/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2017_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2017_bot3 {
namespace control_loops {
namespace drivetrain {
@@ -28,8 +31,9 @@
::y2017_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2017_bot3::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
- drivetrain::kV,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
drivetrain::kHighGearRatio, drivetrain::kHighGearRatio,
kThreeStateDriveShifter, kThreeStateDriveShifter,
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.cc b/y2018/control_loops/drivetrain/drivetrain_base.cc
index ed40082..78e4b84 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_base.cc
@@ -1,14 +1,17 @@
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include <chrono>
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2018/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include "y2018/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2018/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2018 {
namespace control_loops {
namespace drivetrain {
@@ -28,8 +31,9 @@
::y2018::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2018::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
- drivetrain::kV,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
drivetrain::kHighGearRatio, drivetrain::kLowGearRatio,
kThreeStateDriveShifter, kThreeStateDriveShifter,
diff --git a/y2018_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2018_bot3/control_loops/drivetrain/drivetrain_base.cc
index 305b7c2..9501a8a 100644
--- a/y2018_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2018_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -1,5 +1,7 @@
#include "y2018_bot3/control_loops/drivetrain/drivetrain_base.h"
+#include <chrono>
+
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
@@ -8,6 +10,8 @@
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+namespace chrono = ::std::chrono;
+
namespace y2018_bot3 {
namespace control_loops {
namespace drivetrain {
@@ -28,8 +32,9 @@
::y2018_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2018_bot3::control_loops::drivetrain::MakeKFDrivetrainLoop,
- drivetrain::kDt, drivetrain::kRobotRadius, drivetrain::kWheelRadius,
- drivetrain::kV,
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
drivetrain::kHighGearRatio, drivetrain::kHighGearRatio,
kThreeStateDriveShifter, kThreeStateDriveShifter,