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_);