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
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 5871a29..773b6cb 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -303,6 +303,7 @@
],
visibility = ["//visibility:public"],
deps = [
+ "//aos/time",
"//third_party/eigen",
],
)
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);
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 7617dc2..5df1666 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -111,8 +111,7 @@
drivetrain_plant_.mutable_X(1, 0) = 0.0;
drivetrain_plant_.mutable_X(2, 0) = 0.0;
drivetrain_plant_.mutable_X(3, 0) = 0.0;
- drivetrain_plant_.mutable_Y() =
- drivetrain_plant_.C() * drivetrain_plant_.X();
+ drivetrain_plant_.mutable_Y() = drivetrain_plant_.C() * drivetrain_plant_.X();
last_left_position_ = drivetrain_plant_.Y(0, 0);
last_right_position_ = drivetrain_plant_.Y(1, 0);
}
@@ -134,8 +133,8 @@
{
::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
gyro_reading_.MakeMessage();
- gyro->angle = (right_encoder - left_encoder) /
- (dt_config_.robot_radius * 2.0);
+ gyro->angle =
+ (right_encoder - left_encoder) / (dt_config_.robot_radius * 2.0);
gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
(dt_config_.robot_radius * 2.0);
gyro.Send();
@@ -177,10 +176,7 @@
U(0, 0) += drivetrain_plant_.left_voltage_offset();
U(1, 0) += drivetrain_plant_.right_voltage_offset();
drivetrain_plant_.Update(U);
- double dt_float =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count();
+ double dt_float = ::aos::time::DurationInSeconds(dt_config_.dt);
state_ = RungeKuttaU(
[this](const ::Eigen::Matrix<double, 5, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 5cecce7..68f67f6 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -3,12 +3,12 @@
#include <chrono>
+#include "Eigen/Dense"
#include "aos/containers/priority_queue.h"
#include "aos/util/math.h"
#include "frc971/control_loops/c2d.h"
-#include "frc971/control_loops/runge_kutta.h"
-#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/runge_kutta.h"
namespace y2019 {
namespace control_loops {
@@ -50,7 +50,7 @@
kRightEncoder = 5,
kRightVelocity = 6,
kLeftVoltageError = 7,
- kRightVoltageError = 8 ,
+ kRightVoltageError = 8,
kAngularError = 9,
};
static constexpr int kNStates = 10;
@@ -140,10 +140,12 @@
void(const State &, const StateSquare &,
::std::function<Output(const State &, const Input &)> *,
::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ const State &)> *)>
+ make_h,
::std::function<Output(const State &, const Input &)> h,
::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t);
// A utility function for specifically updating with encoder and gyro
@@ -177,9 +179,10 @@
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
- Correct(z, &U, {}, [this](const State &X, const Input &) {
- return H_encoders_and_gyro_ * X;
- },
+ Correct(z, &U, {},
+ [this](const State &X, const Input &) {
+ return H_encoders_and_gyro_ * X;
+ },
[this](const State &) { return H_encoders_and_gyro_; }, R, t);
}
@@ -210,11 +213,12 @@
// estimate. This is used by the camera to make it so that we only have to
// match targets once.
// Only called if h and dhdx are empty.
- ::std::function<
- void(const State &, const StateSquare &,
- ::std::function<Output(const State &, const Input &)> *,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h;
+ ::std::function<void(
+ const State &, const StateSquare &,
+ ::std::function<Output(const State &, const Input &)> *,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)>
+ make_h;
// A function to calculate the expected output at a given state/input.
// TODO(james): For encoders/gyro, it is linear and the function call may
// be expensive. Potential source of optimization.
@@ -226,14 +230,14 @@
// recalculate it to be strictly correct, but I was both too lazy to do
// so and it seemed unnecessary). This is a potential source for future
// optimizations if function calls are being expensive.
- ::std::function<
- Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> dhdx;
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx;
// The measurement noise matrix.
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
// In order to sort the observations in the PriorityQueue object, we
// need a comparison function.
- friend bool operator <(const Observation &l, const Observation &r) {
+ friend bool operator<(const Observation &l, const Observation &r) {
return l.t < r.t;
}
};
@@ -278,11 +282,8 @@
controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);
*state = RungeKuttaU(
- [this](const State &X,
- const Input &U) { return DiffEq(X, U); },
- *state, U,
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
- .count());
+ [this](const State &X, const Input &U) { return DiffEq(X, U); }, *state,
+ U, ::aos::time::DurationInSeconds(dt));
StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
*P = Ptemp;
@@ -342,11 +343,13 @@
::std::function<
void(const State &, const StateSquare &,
::std::function<Output(const State &, const Input &)> *,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ ::std::function<
+ Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> *)>
+ make_h,
::std::function<Output(const State &, const Input &)> h,
::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
@@ -362,10 +365,9 @@
LOG(DEBUG,
"Camera dropped off of end with time of %fs; earliest observation in "
"queue has time of %fs.\n",
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- t.time_since_epoch()).count(),
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- observations_.begin()->t.time_since_epoch()).count());
+ ::aos::time::DurationInSeconds(t.time_since_epoch()),
+ ::aos::time::DurationInSeconds(
+ observations_.begin()->t.time_since_epoch()));
return;
}
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 80ce95c..fa6acc5 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -4,8 +4,8 @@
#include "aos/testing/random_seed.h"
#include "aos/testing/test_shm.h"
-#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
#include "gtest/gtest.h"
namespace frc971 {
@@ -15,7 +15,6 @@
typedef HybridEkf<>::StateIdx StateIdx;
-
class HybridEkfTest : public ::testing::Test {
public:
typedef HybridEkf<>::State State;
@@ -37,8 +36,7 @@
return RungeKuttaU(
::std::bind(&HybridEkfTest::DiffEq, this, ::std::placeholders::_1,
::std::placeholders::_2),
- X, U, ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt).count());
+ X, U, ::aos::time::DurationInSeconds(dt_config_.dt));
}
void CheckDiffEq(const State &X, const Input &U) {
// Re-implement dynamics as a sanity check:
@@ -69,15 +67,11 @@
// Dynamics don't expect error terms to change:
EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
}
- State DiffEq(const State &X, const Input &U) {
- return ekf_.DiffEq(X, U);
- }
+ State DiffEq(const State &X, const Input &U) { return ekf_.DiffEq(X, U); }
// Returns a random value sampled from a normal distribution with a standard
// deviation of std and a mean of zero.
- double Normal(double std) {
- return normal_(gen_) * std;
- }
+ double Normal(double std) { return normal_(gen_) * std; }
DrivetrainConfig<double> dt_config_;
HybridEkf<> ekf_;
@@ -93,19 +87,23 @@
CheckDiffEq(State::Zero(), Input::Zero());
CheckDiffEq(State::Zero(), {-5.0, 5.0});
CheckDiffEq(State::Zero(), {12.0, -3.0});
- CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
- {5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
- {5.0, 6.0});
- CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
- {5.0, 6.0});
+ CheckDiffEq(
+ (State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
+ .finished(),
+ {5.0, 6.0});
+ CheckDiffEq(
+ (State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
+ .finished(),
+ {5.0, 6.0});
+ CheckDiffEq(
+ (State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
+ .finished(),
+ {5.0, 6.0});
// And check that a theta outisde of [-M_PI, M_PI] works.
- CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
- 0.3).finished(),
- {5.0, 6.0});
+ CheckDiffEq(
+ (State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
+ .finished(),
+ {5.0, 6.0});
}
// Tests that if we provide a bunch of observations of the position
@@ -232,7 +230,7 @@
expected_X_hat(1, 0) = Z(1, 0) + modeled_X_hat(0, 0);
expected_X_hat(2, 0) = Z(2, 0);
EXPECT_LT((expected_X_hat.topRows<7>() - ekf_.X_hat().topRows<7>()).norm(),
- 1e-3)
+ 1e-3)
<< "X_hat: " << ekf_.X_hat() << " expected " << expected_X_hat;
// The covariance after the predictions but before the corrections should
// be higher than after the corrections are made.
@@ -299,7 +297,9 @@
U, t0_ + (ii + 1) * dt_config_.dt);
EXPECT_NEAR((true_X - ekf_.X_hat()).squaredNorm(), 0.0, 1e-25)
<< "Expected only floating point precision errors in update step. "
- "Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
+ "Estimated X_hat:\n"
+ << ekf_.X_hat() << "\ntrue X:\n"
+ << true_X;
}
}
@@ -373,14 +373,17 @@
true_X(StateIdx::kRightEncoder, 0) + Normal(1e-3),
(true_X(StateIdx::kRightVelocity, 0) -
true_X(StateIdx::kLeftVelocity, 0)) /
- dt_config_.robot_radius / 2.0 + Normal(1e-4),
+ dt_config_.robot_radius / 2.0 +
+ Normal(1e-4),
U, t0_ + (ii + 1) * dt_config_.dt);
}
EXPECT_NEAR(
(true_X.bottomRows<9>() - ekf_.X_hat().bottomRows<9>()).squaredNorm(),
0.0, 2e-3)
<< "Expected non-x/y estimates to converge to correct. "
- "Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
+ "Estimated X_hat:\n"
+ << ekf_.X_hat() << "\ntrue X:\n"
+ << true_X;
}
class HybridEkfDeathTest : public HybridEkfTest {};
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 414ec81..7d401d6 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -63,7 +63,6 @@
EXPECT_EQ(0.0, output.right_voltage);
}
-
EXPECT_LE(::std::abs(output.left_voltage), 12.0 + 1e-6);
EXPECT_LE(::std::abs(output.right_voltage), 12.0 + 1e-6);
@@ -75,12 +74,10 @@
return ContinuousDynamics(velocity_drivetrain_->plant(), Tlr_to_la_,
X, U);
},
- state_, U,
- chrono::duration_cast<chrono::duration<double>>(config_.dt).count());
+ state_, U, ::aos::time::DurationInSeconds(config_.dt));
t_ += config_.dt;
- time_.push_back(chrono::duration_cast<chrono::duration<double>>(
- t_.time_since_epoch()).count());
+ time_.push_back(::aos::time::DurationInSeconds(t_.time_since_epoch()));
simulation_ul_.push_back(U(0, 0));
simulation_ur_.push_back(U(1, 0));
simulation_x_.push_back(state_.x());
@@ -350,16 +347,15 @@
.finished(),
(::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 0.0, 0.5, -0.5)
.finished()),
- ::testing::Values(
- [](const ::Eigen::Matrix<double, 5, 1> &state) {
- return -1.0 * state.x();
- },
- [](const ::Eigen::Matrix<double, 5, 1> &state) {
- return 1.0 * state.x();
- },
- [](const ::Eigen::Matrix<double, 5, 1> &state) {
- return -0.25 * ::std::abs(state.x()) - 0.125 * state.x() * state.x();
- })));
+ ::testing::Values([](const ::Eigen::Matrix<double, 5, 1>
+ &state) { return -1.0 * state.x(); },
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return 1.0 * state.x();
+ },
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return -0.25 * ::std::abs(state.x()) -
+ 0.125 * state.x() * state.x();
+ })));
} // namespace testing
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index c9582e6..ae51cb1 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -4,8 +4,8 @@
#include "Eigen/Dense"
#include "aos/logging/matrix_logging.h"
-#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/hybrid_state_feedback_loop.h"
@@ -259,7 +259,6 @@
::Eigen::Matrix<double, 5, 5> Trajectory::ALinearizedContinuous(
const ::Eigen::Matrix<double, 5, 1> &state) const {
-
const double sintheta = ::std::sin(state(2));
const double costheta = ::std::cos(state(2));
const ::Eigen::Matrix<double, 2, 1> linear_angular =
@@ -268,7 +267,7 @@
// When stopped, just roll with a min velocity.
double linear_velocity = 0.0;
constexpr double kMinVelocity = 0.1;
- if (::std::abs(linear_angular(0)) < kMinVelocity / 100.0) {
+ if (::std::abs(linear_angular(0)) < kMinVelocity / 100.0) {
linear_velocity = 0.1;
} else if (::std::abs(linear_angular(0)) > kMinVelocity) {
linear_velocity = linear_angular(0);
@@ -312,8 +311,7 @@
BLinearizedContinuous();
// Now, convert it to discrete.
- controls::C2D(A_linearized_continuous, B_linearized_continuous,
- dt, A, B);
+ controls::C2D(A_linearized_continuous, B_linearized_continuous, dt, A, B);
}
::Eigen::Matrix<double, 2, 5> Trajectory::KForState(
@@ -355,24 +353,25 @@
result.block<2, 1>(0, 0) = spline_->XY(distance);
result(2, 0) = spline_->Theta(distance);
- result.block<2, 1>(3, 0) = Tla_to_lr_ *
- (::Eigen::Matrix<double, 2, 1>() << velocity,
- spline_->DThetaDt(distance, velocity))
- .finished();
+ result.block<2, 1>(3, 0) =
+ Tla_to_lr_ * (::Eigen::Matrix<double, 2, 1>() << velocity,
+ spline_->DThetaDt(distance, velocity))
+ .finished();
return result;
}
::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
- double dt_float =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
+ double dt_float = ::aos::time::DurationInSeconds(dt);
// TODO(austin): This feels like something that should be pulled out into
// a library for re-use.
- *state = RungeKutta([this](const ::Eigen::Matrix<double, 2, 1> x) {
- ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
- return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
- }, *state, dt_float);
+ *state = RungeKutta(
+ [this](const ::Eigen::Matrix<double, 2, 1> x) {
+ ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+ return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+ },
+ *state, dt_float);
::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
(*state)(1) = result(1);
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 8508099..d2dd2c8 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -15,7 +15,8 @@
// Basic ideas from spline following are from Jared Russell and
// http://msc.fe.uni-lj.si/Papers/Chapter10_MobileRobotsNewResearch_Lepetic2005.pdf
//
-// For the future, I'd like to use the following to measure distance to the path.
+// For the future, I'd like to use the following to measure distance to the
+// path.
// http://home.eps.hw.ac.uk/~ab226/papers/dist.pdf
//
// LQR controller was inspired by
@@ -109,9 +110,7 @@
::std::vector<double> length_plan_vl;
::std::vector<double> length_plan_vr;
const chrono::nanoseconds kDt = chrono::microseconds(5050);
- const double kDtDouble =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(kDt)
- .count();
+ const double kDtDouble = ::aos::time::DurationInSeconds(kDt);
{
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva =
trajectory.PlanXVA(kDt);
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index e54175b..d69f7db 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -3,10 +3,10 @@
#include <assert.h>
+#include <chrono>
#include <memory>
#include <utility>
#include <vector>
-#include <chrono>
#include "Eigen/Dense"
#include "unsupported/Eigen/MatrixFunctions"
@@ -198,7 +198,6 @@
Eigen::Matrix<Scalar, number_of_states, number_of_states> A_;
Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_;
-
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
coefficients_;
@@ -208,7 +207,6 @@
DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
};
-
// A container for all the observer coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs,
typename Scalar = double>
@@ -216,8 +214,10 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous;
- const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous;
- const Eigen::Matrix<Scalar, number_of_states, number_of_states> P_steady_state;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
+ R_continuous;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states>
+ P_steady_state;
HybridKalmanCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
@@ -239,7 +239,8 @@
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
- number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+ *observers)
: coefficients_(::std::move(*observers)) {}
HybridKalman(HybridKalman &&other)
@@ -345,9 +346,7 @@
coefficients().R_continuous.transpose()) /
static_cast<Scalar>(2.0);
- R_ = Rtemp /
- ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
- .count();
+ R_ = Rtemp / ::aos::time::TypedDurationInSeconds<Scalar>(dt);
}
// Internal state estimate.
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index ef5ffde..53c729d 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -265,9 +265,7 @@
status->voltage_error = this->X_hat(2, 0);
status->calculated_velocity =
(position() - last_position_) /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
status->estimator_state = this->EstimatorState(0);
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 1553a59..de9c22b 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -224,9 +224,8 @@
RunIteration(enabled, null_goal);
- const double loop_time = chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time)
- .count();
+ const double loop_time = ::aos::time::DurationInSeconds(
+ monotonic_clock::now() - loop_start_time);
const double subsystem_acceleration =
(subsystem_plant_.subsystem_velocity() - begin_subsystem_velocity) /
loop_time;