Merge "Fix simulation sent_time and add a test"
diff --git a/aos/ipc_lib/ipc_comparison.cc b/aos/ipc_lib/ipc_comparison.cc
index cff2699..2c73397 100644
--- a/aos/ipc_lib/ipc_comparison.cc
+++ b/aos/ipc_lib/ipc_comparison.cc
@@ -23,12 +23,12 @@
#include "aos/condition.h"
#include "aos/event.h"
+#include "aos/init.h"
+#include "aos/ipc_lib/queue.h"
#include "aos/logging/implementations.h"
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
-#include "aos/init.h"
-#include "aos/ipc_lib/queue.h"
DEFINE_string(method, "", "Which IPC method to use");
DEFINE_int32(messages, 1000000, "How many messages to send back and forth");
@@ -427,7 +427,6 @@
const ::std::unique_ptr<SemaphoreInterface> ping_, pong_;
};
-
class AOSMutexPingPonger : public ConditionVariablePingPonger {
public:
AOSMutexPingPonger()
@@ -457,10 +456,8 @@
public:
AOSEventPingPonger()
: SemaphorePingPonger(
- ::std::unique_ptr<SemaphoreInterface>(
- new AOSEventSemaphore()),
- ::std::unique_ptr<SemaphoreInterface>(
- new AOSEventSemaphore())) {}
+ ::std::unique_ptr<SemaphoreInterface>(new AOSEventSemaphore()),
+ ::std::unique_ptr<SemaphoreInterface>(new AOSEventSemaphore())) {}
private:
class AOSEventSemaphore : public SemaphoreInterface {
@@ -572,8 +569,7 @@
private:
class SysvSemaphore : public SemaphoreInterface {
public:
- SysvSemaphore()
- : sem_id_(PCHECK(semget(IPC_PRIVATE, 1, 0600))) {}
+ SysvSemaphore() : sem_id_(PCHECK(semget(IPC_PRIVATE, 1, 0600))) {}
private:
void Get() override {
@@ -606,8 +602,7 @@
private:
class PosixSemaphore : public SemaphoreInterface {
public:
- PosixSemaphore(sem_t *sem)
- : sem_(sem) {}
+ PosixSemaphore(sem_t *sem) : sem_(sem) {}
private:
void Get() override { PCHECK(sem_wait(sem_)); }
@@ -909,8 +904,7 @@
server.join();
LOG(INFO, "Took %f seconds to send %" PRId32 " messages\n",
- chrono::duration_cast<chrono::duration<double>>(end - start).count(),
- FLAGS_messages);
+ ::aos::time::DurationInSeconds(end - start), FLAGS_messages);
const chrono::nanoseconds per_message = (end - start) / FLAGS_messages;
if (per_message >= chrono::seconds(1)) {
LOG(INFO, "More than 1 second per message ?!?\n");
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index 49bb321..60c561b 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -32,6 +32,7 @@
deps = [
":binary_log_file",
":logging",
+ ":queue_logging",
"//aos:queues",
"//aos/events:event-loop",
"//aos/ipc_lib:queue",
diff --git a/aos/logging/replay.h b/aos/logging/replay.h
index 679b760..45a4cd2 100644
--- a/aos/logging/replay.h
+++ b/aos/logging/replay.h
@@ -10,6 +10,7 @@
#include "aos/ipc_lib/queue.h"
#include "aos/logging/binary_log_file.h"
#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
#include "aos/macros.h"
#include "aos/queue.h"
#include "aos/queue_types.h"
diff --git a/aos/time/time.h b/aos/time/time.h
index 0333c5c..469534f 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -2,17 +2,16 @@
#define AOS_TIME_H_
#include <stdint.h>
-#include <time.h>
#include <sys/time.h>
-#include <stdint.h>
+#include <time.h>
-#include <type_traits>
#include <chrono>
-#include <thread>
#include <ostream>
+#include <thread>
+#include <type_traits>
-#include "aos/type_traits/type_traits.h"
#include "aos/macros.h"
+#include "aos/type_traits/type_traits.h"
namespace aos {
@@ -98,6 +97,16 @@
hertz;
}
+template <typename Scalar>
+constexpr Scalar TypedDurationInSeconds(monotonic_clock::duration dt) {
+ return ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
+ .count();
+}
+
+constexpr double DurationInSeconds(monotonic_clock::duration dt) {
+ return TypedDurationInSeconds<double>(dt);
+}
+
// RAII class that freezes monotonic_clock::now() (to avoid making large numbers
// of syscalls to find the real time).
class TimeFreezer {
diff --git a/aos/util/log_interval.h b/aos/util/log_interval.h
index 5e76166..69170e4 100644
--- a/aos/util/log_interval.h
+++ b/aos/util/log_interval.h
@@ -1,8 +1,8 @@
#ifndef AOS_UTIL_LOG_INTERVAL_H_
#define AOS_UTIL_LOG_INTERVAL_H_
-#include "aos/time/time.h"
#include "aos/logging/logging.h"
+#include "aos/time/time.h"
#include <string>
@@ -78,8 +78,7 @@
log_do(level_, "%s: %.*s %d times over %f sec\n", context_,
static_cast<int>(message_.size()), message_.data(),
interval_.Count(),
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- interval_.interval()).count());
+ ::aos::time::DurationInSeconds(interval_.interval()));
context_ = NULL;
}
}
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index e58324f..a1e8abb 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -12,21 +12,17 @@
output_.setZero();
}
-void TrapezoidProfile::UpdateVals(double acceleration,
- double delta_time) {
- output_(0) += output_(1) * delta_time +
- 0.5 * acceleration * delta_time * delta_time;
+void TrapezoidProfile::UpdateVals(double acceleration, double delta_time) {
+ output_(0) +=
+ output_(1) * delta_time + 0.5 * acceleration * delta_time * delta_time;
output_(1) += acceleration * delta_time;
}
-const Matrix<double, 2, 1> &TrapezoidProfile::Update(
- double goal_position,
- double goal_velocity) {
+const Matrix<double, 2, 1> &TrapezoidProfile::Update(double goal_position,
+ double goal_velocity) {
CalculateTimes(goal_position - output_(0), goal_velocity);
- double next_timestep =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(timestep_)
- .count();
+ double next_timestep = ::aos::time::DurationInSeconds(timestep_);
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
@@ -93,26 +89,23 @@
}
// We now know the top velocity we can get to.
- double top_velocity = sqrt((distance_to_target +
- (output_(1) * output_(1)) /
- (2.0 * acceleration_) +
- (goal_velocity * goal_velocity) /
- (2.0 * deceleration_)) /
- (-1.0 / (2.0 * deceleration_) +
- 1.0 / (2.0 * acceleration_)));
+ double top_velocity = sqrt(
+ (distance_to_target + (output_(1) * output_(1)) / (2.0 * acceleration_) +
+ (goal_velocity * goal_velocity) / (2.0 * deceleration_)) /
+ (-1.0 / (2.0 * deceleration_) + 1.0 / (2.0 * acceleration_)));
// If it can go too fast, we now know how long we get to accelerate for and
// how long to go at constant velocity.
if (top_velocity > maximum_velocity_) {
- acceleration_time_ = (maximum_velocity_ - output_(1)) /
- maximum_acceleration_;
- constant_time_ = (distance_to_target +
- (goal_velocity * goal_velocity -
- maximum_velocity_ * maximum_velocity_) /
- (2.0 * maximum_acceleration_)) / maximum_velocity_;
+ acceleration_time_ =
+ (maximum_velocity_ - output_(1)) / maximum_acceleration_;
+ constant_time_ =
+ (distance_to_target + (goal_velocity * goal_velocity -
+ maximum_velocity_ * maximum_velocity_) /
+ (2.0 * maximum_acceleration_)) /
+ maximum_velocity_;
} else {
- acceleration_time_ = (top_velocity - output_(1)) /
- acceleration_;
+ acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
}
CHECK_GT(top_velocity, -maximum_velocity_);
@@ -122,8 +115,7 @@
acceleration_time_ = 0;
}
- deceleration_time_ = (goal_velocity - top_velocity) /
- deceleration_;
+ deceleration_time_ = (goal_velocity - top_velocity) / deceleration_;
}
} // namespace util
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;
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index e902691..aec4d4f 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -7,10 +7,10 @@
#include <math.h>
#include <chrono>
+#include "aos/init.h"
#include "aos/logging/queue_logging.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
-#include "aos/init.h"
#include "frc971/wpilib/imu.q.h"
#include "frc971/zeroing/averager.h"
@@ -214,9 +214,8 @@
// interrupt delay among other things), but it will catch the code
// constantly falling behind, which seems like the most likely failure
// scenario.
- if (!dio1_->Get() ||
- dio1_->WaitForInterrupt(0, false) !=
- frc::InterruptableSensorBase::kTimeout) {
+ if (!dio1_->Get() || dio1_->WaitForInterrupt(0, false) !=
+ frc::InterruptableSensorBase::kTimeout) {
LOG(ERROR, "IMU read took too long\n");
continue;
}
@@ -243,10 +242,8 @@
}
auto message = imu_values.MakeMessage();
- message->fpga_timestamp =
- chrono::duration_cast<chrono::duration<double>>(
- dio1_->ReadRisingTimestamp().time_since_epoch())
- .count();
+ message->fpga_timestamp = ::aos::time::DurationInSeconds(
+ dio1_->ReadRisingTimestamp().time_since_epoch());
message->monotonic_timestamp_ns =
chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
.count();
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index 42fb86b..eb55664 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -7,6 +7,7 @@
#include "aos/macros.h"
#include "aos/mutex/mutex.h"
+#include "aos/time/time.h"
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Counter.h"
@@ -236,9 +237,7 @@
static constexpr ::std::chrono::nanoseconds kNominalPeriod =
::std::chrono::microseconds(4096);
static constexpr double kMaxPeriod =
- (::std::chrono::duration_cast<::std::chrono::duration<double>>(
- kNominalPeriod) *
- 2).count();
+ ::aos::time::DurationInSeconds(kNominalPeriod * 2);
::std::unique_ptr<::frc::Counter> high_counter_, period_length_counter_;
::std::unique_ptr<::frc::DigitalInput> input_;
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index ef59ba4..18e490e 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -3,12 +3,12 @@
#include <chrono>
#include <memory>
-#include "aos/util/phased_loop.h"
-#include "aos/time/time.h"
-#include "aos/util/trapezoid_profile.h"
+#include "aos/actions/actions.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
-#include "aos/actions/actions.h"
+#include "aos/time/time.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/trapezoid_profile.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -29,15 +29,6 @@
namespace this_thread = ::std::this_thread;
using ::aos::monotonic_clock;
-namespace {
-
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
-} // namespace
-
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
@@ -104,7 +95,8 @@
right_initial_position = right_goal;
}
-void PositionClawVertically(double intake_power = 0.0, double centering_power = 0.0) {
+void PositionClawVertically(double intake_power = 0.0,
+ double centering_power = 0.0) {
if (!control_loops::claw_queue.goal.MakeWithBuilder()
.bottom_angle(0.0)
.separation_angle(0.0)
@@ -166,12 +158,12 @@
while (true) {
if (ShouldExitAuto()) return;
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
- double left_error = ::std::abs(
- left_initial_position -
- ::frc971::control_loops::drivetrain_queue.status->estimated_left_position);
- double right_error = ::std::abs(
- right_initial_position -
- ::frc971::control_loops::drivetrain_queue.status->estimated_right_position);
+ double left_error = ::std::abs(left_initial_position -
+ ::frc971::control_loops::drivetrain_queue
+ .status->estimated_left_position);
+ double right_error = ::std::abs(right_initial_position -
+ ::frc971::control_loops::drivetrain_queue
+ .status->estimated_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
@@ -220,8 +212,8 @@
::frc971::control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
::frc971::control_loops::drivetrain_queue.status->estimated_left_position;
- right_initial_position =
- ::frc971::control_loops::drivetrain_queue.status->estimated_right_position;
+ right_initial_position = ::frc971::control_loops::drivetrain_queue.status
+ ->estimated_right_position;
}
void WaitUntilClawDone() {
@@ -241,14 +233,11 @@
}
bool ans =
control_loops::claw_queue.status->zeroed &&
- (::std::abs(control_loops::claw_queue.status->bottom_velocity) <
- 1.0) &&
+ (::std::abs(control_loops::claw_queue.status->bottom_velocity) < 1.0) &&
(::std::abs(control_loops::claw_queue.status->bottom -
- control_loops::claw_queue.goal->bottom_angle) <
- 0.10) &&
+ control_loops::claw_queue.goal->bottom_angle) < 0.10) &&
(::std::abs(control_loops::claw_queue.status->separation -
- control_loops::claw_queue.goal->separation_angle) <
- 0.4);
+ control_loops::claw_queue.goal->separation_angle) < 0.4);
if (ans) {
return;
}
@@ -258,9 +247,7 @@
class HotGoalDecoder {
public:
- HotGoalDecoder() {
- ResetCounts();
- }
+ HotGoalDecoder() { ResetCounts(); }
void ResetCounts() {
hot_goal.FetchLatest();
@@ -393,7 +380,7 @@
// Turn the claw on, keep it straight up until the ball has been grabbed.
LOG(INFO, "Claw going up at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
PositionClawVertically(12.0, 4.0);
SetShotPower(115.0);
@@ -401,7 +388,7 @@
this_thread::sleep_for(chrono::milliseconds(250));
if (ShouldExitAuto()) return;
LOG(INFO, "Readying claw for shot at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
{
if (ShouldExitAuto()) return;
@@ -448,7 +435,7 @@
// Shoot.
LOG(INFO, "Shooting at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Shoot();
this_thread::sleep_for(chrono::milliseconds(50));
@@ -460,7 +447,7 @@
if (ShouldExitAuto()) return;
} else if (auto_version == AutoVersion::kSingleHot) {
LOG(INFO, "auto done at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
PositionClawVertically(0.0, 0.0);
return;
}
@@ -469,7 +456,7 @@
if (ShouldExitAuto()) return;
// Intake the new ball.
LOG(INFO, "Claw ready for intake at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
PositionClawBackIntake();
auto drivetrain_action =
SetDriveGoal(kShootDistance + kPickupDistance, drive_params);
@@ -477,7 +464,7 @@
WaitUntilDoneOrCanceled(::std::move(drivetrain_action));
if (ShouldExitAuto()) return;
LOG(INFO, "Wait for the claw at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitUntilClawDone();
if (ShouldExitAuto()) return;
}
@@ -485,7 +472,7 @@
// Drive back.
{
LOG(INFO, "Driving back at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
auto drivetrain_action =
SetDriveGoal(-(kShootDistance + kPickupDistance), drive_params);
this_thread::sleep_for(chrono::milliseconds(300));
@@ -525,7 +512,7 @@
}
LOG(INFO, "Shooting at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
// Shoot
Shoot();
if (ShouldExitAuto()) return;
diff --git a/y2014/autonomous/auto_main.cc b/y2014/autonomous/auto_main.cc
index d38e1e6..f125cc2 100644
--- a/y2014/autonomous/auto_main.cc
+++ b/y2014/autonomous/auto_main.cc
@@ -1,12 +1,12 @@
#include <stdio.h>
-#include "aos/time/time.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
+#include "aos/time/time.h"
#include "frc971/autonomous/auto.q.h"
#include "y2014/autonomous/auto.h"
-int main(int /*argc*/, char * /*argv*/[]) {
+int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
LOG(INFO, "Auto main started\n");
@@ -28,9 +28,7 @@
auto elapsed_time = ::aos::monotonic_clock::now() - start_time;
LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- elapsed_time)
- .count());
+ ::aos::time::DurationInSeconds(elapsed_time));
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
@@ -40,4 +38,3 @@
::aos::Cleanup();
return 0;
}
-
diff --git a/y2014_bot3/actors/autonomous_actor.cc b/y2014_bot3/actors/autonomous_actor.cc
index 31f0175..67a2955 100644
--- a/y2014_bot3/actors/autonomous_actor.cc
+++ b/y2014_bot3/actors/autonomous_actor.cc
@@ -19,11 +19,6 @@
namespace {
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
const ProfileParameters kDrive = {5.0, 2.5};
const ProfileParameters kTurn = {8.0, 3.0};
@@ -44,7 +39,8 @@
StartDrive(1.0, 0.0, kDrive, kTurn);
if (!WaitForDriveDone()) return true;
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 5f95072..4f68f20 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -5,8 +5,8 @@
#include <chrono>
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
@@ -17,8 +17,8 @@
namespace y2016 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
@@ -43,10 +43,6 @@
const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};
const ProfileParameters kTwoBallBallPickupAccel = {2.0, 2.5};
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
} // namespace
AutonomousActor::AutonomousActor(
@@ -594,7 +590,7 @@
WaitForIntake();
LOG(INFO, "Intake done at %f seconds, starting to drive\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
const double kDriveDistance = 5.05;
StartDrive(-kDriveDistance, 0.0, kTwoBallLowDrive, kSlowTurn);
@@ -609,12 +605,12 @@
const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
first_ball_there = ball_detected;
LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
false, 0.0);
LOG(INFO, "Shutting off rollers at %f seconds, starting to straighten out\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
false, 0.0);
@@ -634,7 +630,7 @@
if (!WaitForDriveDone()) return;
LOG(INFO, "First shot done driving at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructureProfile();
@@ -657,7 +653,7 @@
}
LOG(INFO, "First shot at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
SetShooterSpeed(0.0);
@@ -677,7 +673,7 @@
if (!WaitForDriveNear(0.06, kDoNotTurnCare)) return;
LOG(INFO, "At Low Bar %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
OpenShooter();
constexpr double kSecondBallAfterBarDrive = 2.10;
@@ -695,7 +691,7 @@
false, 12.0);
LOG(INFO, "Done backing up %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
constexpr double kDriveBackDistance = 5.15 - 0.4;
StartDrive(-kDriveBackDistance, 0.0, kTwoBallLowDrive, kFinishTurn);
@@ -704,7 +700,7 @@
StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
LOG(INFO, "Straightening up at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
CloseIfBall();
if (!WaitForDriveNear(kDriveBackDistance - 2.3, kDoNotTurnCare)) return;
@@ -715,7 +711,7 @@
if (!ball_detected) {
if (!WaitForDriveDone()) return;
LOG(INFO, "Aborting, no ball %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
return;
}
}
@@ -732,7 +728,7 @@
if (!WaitForDriveDone()) return;
LOG(INFO, "Second shot done driving at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructure();
AlignWithVisionGoal();
if (ShouldCancel()) return;
@@ -743,7 +739,7 @@
// 2.2 with 0.4 of vision.
// 1.8 without any vision.
LOG(INFO, "Going to vision align at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForAlignedWithVision(
(start_time + chrono::milliseconds(13500) + kVisionExtra * 2) -
monotonic_clock::now());
@@ -751,21 +747,23 @@
WaitForSuperstructureProfile();
if (ShouldCancel()) return;
LOG(INFO, "Shoot at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Shoot();
LOG(INFO, "Second shot at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
SetShooterSpeed(0.0);
LOG(INFO, "Folding superstructure back down\n");
TuckArm(true, false);
- LOG(INFO, "Shot %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Shot %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructureLow();
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
void AutonomousActor::StealAndMoveOverBy(double distance) {
@@ -932,7 +930,8 @@
StartDrive(0.5, 0.0, kMoatDrive, kFastTurn);
if (!WaitForDriveDone()) return true;
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 6ae7f0e..08ab1be 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -8,7 +8,6 @@
#include "y2016/control_loops/shooter/shooter_plant.h"
-
namespace y2016 {
namespace control_loops {
namespace shooter {
@@ -37,9 +36,7 @@
history_position_ = (history_position_ + 1) % kHistoryLength;
}
-double ShooterSide::voltage() const {
- return loop_->U(0, 0);
-}
+double ShooterSide::voltage() const { return loop_->U(0, 0); }
void ShooterSide::Update(bool disabled) {
loop_->mutable_R() = loop_->next_R();
@@ -60,8 +57,7 @@
// Compute the distance moved over that time period.
status->avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
- (chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency).count() *
+ (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
status->angular_velocity = loop_->X_hat(1, 0);
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index d0c4362..e7983f3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -88,11 +88,12 @@
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
- superstructure_queue_(".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status") {
+ superstructure_queue_(
+ ".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status") {
InitializeIntakePosition(0.0);
InitializeShoulderPosition(0.0);
InitializeRelativeWristPosition(0.0);
@@ -246,11 +247,12 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : superstructure_queue_(".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status"),
+ : superstructure_queue_(
+ ".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status"),
superstructure_(&event_loop_),
superstructure_plant_() {}
@@ -300,9 +302,8 @@
double begin_wrist_velocity =
superstructure_plant_.wrist_angular_velocity();
RunIteration(enabled);
- 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 shoulder_acceleration =
(superstructure_plant_.shoulder_angular_velocity() -
begin_shoulder_velocity) /
@@ -921,8 +922,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -941,8 +942,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.5)
.max_angular_velocity_intake(1)
.max_angular_acceleration_intake(1)
@@ -967,7 +968,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -986,7 +987,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.5)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
.angle_wrist(0.0)
.max_angular_velocity_intake(4.5)
.max_angular_acceleration_intake(800)
@@ -1051,8 +1052,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -1071,8 +1072,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(1.3)
.max_angular_velocity_intake(1.0)
.max_angular_acceleration_intake(1.0)
@@ -1139,7 +1140,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.upper) // stowed
.angle_shoulder(M_PI / 2.0) // in the collision area
- .angle_wrist(M_PI) // forward
+ .angle_wrist(M_PI) // forward
.Send());
RunForTime(chrono::seconds(5));
@@ -1341,4 +1342,4 @@
} // namespace testing
} // namespace superstructure
} // namespace control_loops
-} // namespace frc971
+} // namespace y2016
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 04e91f6..84af420 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -20,9 +20,9 @@
#include "frc971/autonomous/auto.q.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
+#include "y2016/vision/vision.q.h"
namespace chrono = ::std::chrono;
@@ -128,7 +128,7 @@
AddPoint("big indicator", big_indicator);
AddPoint("superstructure state indicator", superstructure_state_indicator);
- if(auto_mode_indicator != 15) {
+ if (auto_mode_indicator != 15) {
AddPoint("auto mode indicator", auto_mode_indicator);
}
#endif
@@ -198,11 +198,10 @@
if (static_cast<size_t>(adjusted_index) <
sample_items_.at(0).datapoints.size()) {
message << cur_sample << "%"
- << chrono::duration_cast<chrono::duration<double>>(
+ << ::aos::time::DurationInSeconds(
sample_items_.at(0)
.datapoints.at(adjusted_index)
.time.time_since_epoch())
- .count()
<< "%"; // Send time.
// Add comma-separated list of data points.
for (size_t cur_measure = 0; cur_measure < sample_items_.size();
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 3409671..4b95034 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -10,11 +10,11 @@
#include <thread>
#include <vector>
+#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
-#include "aos/init.h"
#include "aos/vision/events/udp.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -179,12 +179,11 @@
double angle, double last_angle,
::aos::vision::Vector<2> *interpolated_result,
double *interpolated_angle) {
- const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- older.capture_time() - newer.last_capture_time())
- .count() /
- chrono::duration_cast<chrono::duration<double>>(
- newer.capture_time() - newer.last_capture_time())
- .count();
+ const double age_ratio =
+ ::aos::time::DurationInSeconds(older.capture_time() -
+ newer.last_capture_time()) /
+ ::aos::time::DurationInSeconds(newer.capture_time() -
+ newer.last_capture_time());
interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
@@ -226,12 +225,9 @@
status->drivetrain_left_position = before.left;
status->drivetrain_right_position = before.right;
} else {
- const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- capture_time - before.time)
- .count() /
- chrono::duration_cast<chrono::duration<double>>(
- after.time - before.time)
- .count();
+ const double age_ratio =
+ ::aos::time::DurationInSeconds(capture_time - before.time) /
+ ::aos::time::DurationInSeconds(after.time - before.time);
status->drivetrain_left_position =
before.left * (1 - age_ratio) + after.left * age_ratio;
status->drivetrain_right_position =
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 6070a71..ee17f49 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -5,26 +5,21 @@
#include <chrono>
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
namespace y2017 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
const ProfileParameters kGearBallBackDrive = {3.0, 3.5};
const ProfileParameters kGearDrive = {1.5, 2.0};
const ProfileParameters kGearFastDrive = {2.0, 2.5};
@@ -163,11 +158,11 @@
SendSuperstructureGoal();
// Shoot from the peg.
- //set_indexer_angular_velocity(-2.1 * M_PI);
- //SendSuperstructureGoal();
- //this_thread::sleep_for(chrono::milliseconds(1750));
+ // set_indexer_angular_velocity(-2.1 * M_PI);
+ // SendSuperstructureGoal();
+ // this_thread::sleep_for(chrono::milliseconds(1750));
- //this_thread::sleep_for(chrono::milliseconds(500));
+ // this_thread::sleep_for(chrono::milliseconds(500));
this_thread::sleep_for(chrono::milliseconds(750));
set_indexer_angular_velocity(0.0);
set_vision_track(false);
@@ -180,7 +175,7 @@
SendSuperstructureGoal();
LOG(INFO, "Starting drive back %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(-2.75, kDriveDirection * 1.24, kSlowDrive,
kFirstGearStartTurn);
@@ -192,8 +187,7 @@
if (!WaitForDriveNear(0.2, 0.0)) return true;
this_thread::sleep_for(chrono::milliseconds(200));
// Trip the hopper
- StartDrive(0.0, kDriveDirection * 1.10, kSlowDrive,
- kSmashTurn);
+ StartDrive(0.0, kDriveDirection * 1.10, kSlowDrive, kSmashTurn);
if (!WaitForDriveNear(0.2, 0.35)) return true;
set_vision_track(true);
@@ -201,11 +195,10 @@
SendSuperstructureGoal();
if (!WaitForDriveNear(0.2, 0.2)) return true;
- StartDrive(0.0, -kDriveDirection * 0.15, kSlowDrive,
- kSmashTurn);
+ StartDrive(0.0, -kDriveDirection * 0.15, kSlowDrive, kSmashTurn);
LOG(INFO, "Starting second shot %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_indexer_angular_velocity(-2.15 * M_PI);
SendSuperstructureGoal();
if (!WaitForDriveNear(0.2, 0.1)) return true;
@@ -235,7 +228,8 @@
set_intake_goal(0.07);
SendSuperstructureGoal();
- StartDrive(-3.42, kDriveDirection * (M_PI / 10 - 0.057) , kSlowDrive, kFirstTurn);
+ StartDrive(-3.42, kDriveDirection * (M_PI / 10 - 0.057), kSlowDrive,
+ kFirstTurn);
if (!WaitForDriveNear(3.30, 0.0)) return true;
LOG(INFO, "Turn ended: %f left to go\n", DriveDistanceLeft());
// We can go to 2.50 before we hit the previous profile.
@@ -281,7 +275,7 @@
SendSuperstructureGoal();
LOG(INFO, "Started shooting at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
this_thread::sleep_for(start_time + chrono::seconds(9) -
monotonic_clock::now());
@@ -302,7 +296,8 @@
} break;
}
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 8a3644e..85af86b 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -127,9 +127,7 @@
indexer_dt_velocity_ =
(new_position.indexer.encoder - indexer_last_position_) /
- chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
indexer_last_position_ = new_position.indexer.encoder;
stuck_indexer_detector_->Correct(Y_);
@@ -144,9 +142,7 @@
indexer_average_angular_velocity_ =
(indexer_history_[indexer_oldest_history_position] -
indexer_history_[indexer_history_position_]) /
- (chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count() *
+ (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
// Ready if average angular velocity is close to the goal.
@@ -255,9 +251,8 @@
::Eigen::Matrix<double, 2, 1> goal_state =
profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
- constexpr double kDt = chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ constexpr double kDt =
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
loop_->mutable_next_R(0, 0) = 0.0;
// TODO(austin): This might not handle saturation right, but I'm not sure I
@@ -297,7 +292,8 @@
bool ColumnProfiledSubsystem::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
- if (turret_position() > range_.upper_hard || turret_position() < range_.lower_hard) {
+ if (turret_position() > range_.upper_hard ||
+ turret_position() < range_.lower_hard) {
LOG(ERROR,
"ColumnProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
turret_position(), range_.lower_hard, range_.upper_hard);
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 73da9b9..4a4dfbd 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -148,9 +148,7 @@
status->voltage_error = X_hat(5, 0);
status->calculated_velocity =
(turret_position() - turret_last_position_) /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
status->estimator_state = EstimatorState(0);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index fee9124..5023e6f 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -98,8 +98,7 @@
X_hat_current_ = loop_->X_hat();
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
- dt_velocity_ = dt_position_ /
- chrono::duration_cast<chrono::duration<double>>(dt).count();
+ dt_velocity_ = dt_position_ / ::aos::time::DurationInSeconds(dt);
fixed_dt_velocity_ = dt_position_ / 0.00505;
loop_->Update(disabled, dt);
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index c8f55ff..46a3af6 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -256,9 +256,7 @@
freeze_indexer_ = freeze_indexer;
}
// Triggers the turret to freeze relative to the indexer.
- void set_freeze_turret(bool freeze_turret) {
- freeze_turret_ = freeze_turret;
- }
+ void set_freeze_turret(bool freeze_turret) { freeze_turret_ = freeze_turret; }
// Simulates the superstructure for a single timestep.
void Simulate() {
@@ -511,9 +509,8 @@
RunIteration(enabled);
- 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 intake_acceleration =
(superstructure_plant_.intake_velocity() - begin_intake_velocity) /
loop_time;
@@ -958,7 +955,6 @@
superstructure_plant_.set_hood_voltage_offset(0.0);
RunForTime(chrono::seconds(5), false);
-
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
@@ -986,7 +982,6 @@
ASSERT_TRUE(goal.Send());
}
-
RunForTime(chrono::seconds(5));
VerifyNearGoal();
EXPECT_TRUE(superstructure_queue_.status->shooter.ready);
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index 57f3228..1e9c94f 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -59,12 +59,8 @@
monotonic_clock::time_point after_time, double after_data,
monotonic_clock::time_point current_time) {
const double age_ratio =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- current_time - before_time)
- .count() /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(after_time -
- before_time)
- .count();
+ ::aos::time::DurationInSeconds(current_time - before_time) /
+ ::aos::time::DurationInSeconds(after_time - before_time);
return before_data * (1 - age_ratio) + after_data * age_ratio;
}
@@ -142,8 +138,7 @@
if (column_angle_is_valid && drivetrain_angle_is_valid) {
LOG(INFO, "Accepting Vision angle of %f, age %f\n",
most_recent_vision_angle_,
- chrono::duration_cast<chrono::duration<double>>(
- monotonic_now - last_target_time).count());
+ ::aos::time::DurationInSeconds(monotonic_now - last_target_time));
most_recent_vision_reading_ = vision_status->angle;
most_recent_vision_angle_ =
vision_status->angle + column_angle + drivetrain_angle;
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index ed91158..4b0fce1 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -5,8 +5,8 @@
#include <chrono>
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
@@ -15,18 +15,13 @@
namespace actors {
using ::frc971::ProfileParameters;
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
const ProfileParameters kDrive = {5.0, 2.5};
const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
@@ -109,7 +104,8 @@
}
*/
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
@@ -138,7 +134,8 @@
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
- StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive, kSweepingTurn);
+ StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive,
+ kSweepingTurn);
if (!WaitForTurnProfileDone()) return true;
// Now, close so let's move the arm up.
@@ -165,7 +162,7 @@
if (!WaitForArmTrajectoryClose(0.001)) return true;
LOG(INFO, "Arm close at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(1000));
@@ -236,7 +233,8 @@
return true;
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
- if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
+ return true;
StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
set_arm_goal_position(arm::BackHighBoxIndex());
SendSuperstructureGoal();
@@ -251,7 +249,7 @@
if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance)))
return true;
LOG(INFO, "Final turn at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(0.0, M_PI / 2.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
if (!WaitForDriveProfileNear(0.15)) return true;
@@ -259,13 +257,13 @@
StartDrive(0.0, 0.3, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
LOG(INFO, "Dropping at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_open_claw(true);
SendSuperstructureGoal();
::std::this_thread::sleep_for(chrono::milliseconds(1000));
LOG(INFO, "Backing up at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(1.5, -0.55, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
@@ -307,7 +305,7 @@
::std::this_thread::sleep_for(chrono::milliseconds(500));
LOG(INFO, "Dropping second box at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_open_claw(true);
SendSuperstructureGoal();
@@ -334,12 +332,13 @@
return true;
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
- if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
+ return true;
StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
set_arm_goal_position(arm::UpIndex());
SendSuperstructureGoal();
LOG(INFO, "Lifting arm at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForTurnProfileDone()) return true;
StartDrive(0.0, 0.0, kDrive, kTurn);
@@ -372,7 +371,7 @@
if (!WaitForDriveNear(0.2, 0.2)) return true;
set_max_drivetrain_voltage(6.0);
LOG(INFO, "Lowered drivetrain voltage %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(300));
set_open_claw(true);
@@ -424,7 +423,7 @@
SendSuperstructureGoal();
set_intake_angle(-0.60);
LOG(INFO, "Dropped first box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(700));
@@ -432,7 +431,7 @@
SendSuperstructureGoal();
LOG(INFO, "Starting second box drive %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
constexpr double kSecondBoxSwerveAngle = 0.35;
constexpr double kSecondBoxDrive = 1.38;
StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
@@ -459,7 +458,7 @@
SendSuperstructureGoal();
LOG(INFO, "Grabbing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(200));
StartDrive(-0.04, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
@@ -467,17 +466,17 @@
set_max_drivetrain_voltage(12.0);
LOG(INFO, "Got second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(500));
set_grab_box(false);
- //set_arm_goal_position(arm::UpIndex());
+ // set_arm_goal_position(arm::UpIndex());
set_arm_goal_position(arm::BackHighBoxIndex());
set_roller_voltage(0.0);
set_disable_box_correct(false);
SendSuperstructureGoal();
LOG(INFO, "Driving to place second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(-kSecondBoxDrive + 0.16, kSecondBoxSwerveAngle, kDrive, kFastTurn);
if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
@@ -487,19 +486,19 @@
kFastTurn);
LOG(INFO, "Starting throw %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
if (!WaitForArmTrajectoryClose(0.25)) return true;
SendSuperstructureGoal();
// Throw the box.
- //if (!WaitForArmTrajectoryClose(0.20)) return true;
+ // if (!WaitForArmTrajectoryClose(0.20)) return true;
if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
set_open_claw(true);
set_intake_angle(-M_PI / 4.0);
LOG(INFO, "Releasing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
SendSuperstructureGoal();
::std::this_thread::sleep_for(chrono::milliseconds(700));
@@ -507,7 +506,7 @@
SendSuperstructureGoal();
LOG(INFO, "Driving to third box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
kFastTurn);
if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
@@ -528,15 +527,15 @@
if (!WaitForDriveProfileDone()) return true;
LOG(INFO, "Waiting for third box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForBoxGrabed()) return true;
LOG(INFO, "Third box grabbed %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
const bool too_late =
monotonic_clock::now() > start_time + chrono::milliseconds(12500);
if (too_late) {
LOG(INFO, "Third box too long, going up. %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_grab_box(false);
set_arm_goal_position(arm::UpIndex());
set_roller_voltage(0.0);
@@ -566,7 +565,7 @@
SendSuperstructureGoal();
LOG(INFO, "Final open %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
if (!WaitForDriveProfileDone()) return true;
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 552086a..c3d9772 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -47,9 +47,7 @@
loop_;
constexpr static double kDt =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
// Sets the offset of the controller to be the zeroing estimator offset when
// possible otherwise zero.
@@ -93,9 +91,7 @@
State state() const { return state_; }
- bool clear_of_box() const {
- return controller_.output_position() < -0.1;
- }
+ bool clear_of_box() const { return controller_.output_position() < -0.1; }
double output_position() const { return controller_.output_position(); }
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 6ad8bff..0447629 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -15,19 +15,10 @@
namespace y2019 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
-namespace {
-
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
-} // namespace
-
AutonomousActor::AutonomousActor(
::aos::EventLoop *event_loop,
::frc971::autonomous::AutonomousActionQueueGroup *s)
@@ -260,7 +251,7 @@
set_suction_goal(false, 1);
SendSuperstructureGoal();
LOG(INFO, "Dropping disc 1 %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
@@ -286,7 +277,7 @@
// As soon as we pick up Panel 2 go score on the rocket
if (!WaitForGamePiece()) return true;
LOG(INFO, "Got gamepiece %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
LineFollowAtVelocity(-4.0);
SplineHandle spline3 =
PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
@@ -303,18 +294,18 @@
// Line follow in to the second disc.
LineFollowAtVelocity(-0.7, 3);
LOG(INFO, "Drawing in disc 2 %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForDriveYCloseToZero(1.2)) return true;
set_suction_goal(false, 1);
SendSuperstructureGoal();
LOG(INFO, "Dropping disc 2 %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
LOG(INFO, "Backing up %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
LineFollowAtVelocity(0.9, 3);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
} else {
@@ -355,7 +346,8 @@
if (!WaitForSuperstructureDone()) return true;
}
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
return true;
}
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 845575f..2e16e1d 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -31,6 +31,39 @@
],
)
+cc_binary(
+ name = "replay_localizer",
+ srcs = [
+ "replay_localizer.cc",
+ ],
+ defines =
+ cpu_select({
+ "amd64": [
+ "SUPPORT_PLOT=1",
+ ],
+ "arm": [],
+ }),
+ linkstatic = True,
+ deps = [
+ "//frc971/control_loops/drivetrain:localizer_queue",
+ ":localizer",
+ ":event_loop_localizer",
+ ":drivetrain_base",
+ "@com_github_gflags_gflags//:gflags",
+ "//y2019:constants",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos:init",
+ "//aos/controls:replay_control_loop",
+ "//frc971/queues:gyro",
+ "//frc971/wpilib:imu_queue",
+ ] + cpu_select({
+ "amd64": [
+ "//third_party/matplotlib-cpp",
+ ],
+ "arm": [],
+ }),
+)
+
cc_library(
name = "polydrivetrain_plants",
srcs = [
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index d008d54..1950468 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -1,19 +1,19 @@
#include "y2019/control_loops/drivetrain/localizer.h"
-#include <random>
#include <queue>
+#include <random>
#include "aos/testing/random_seed.h"
#include "aos/testing/test_shm.h"
-#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
#include "gflags/gflags.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
#include "gtest/gtest.h"
-#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(plot, false, "If true, plot");
@@ -27,7 +27,8 @@
constexpr size_t kNumTargetsPerFrame = 3;
typedef TypedLocalizer<kNumCameras, Field::kNumTargets, Field::kNumObstacles,
- kNumTargetsPerFrame, double> TestLocalizer;
+ kNumTargetsPerFrame, double>
+ TestLocalizer;
typedef typename TestLocalizer::Camera TestCamera;
typedef typename TestCamera::Pose Pose;
typedef typename TestCamera::LineSegment Obstacle;
@@ -47,7 +48,7 @@
const ::std::map<::std::string, ::std::string> &kwargs) {
::std::vector<double> x;
::std::vector<double> y;
- for (const Pose & p : poses) {
+ for (const Pose &p : poses) {
x.push_back(p.abs_pos().x());
y.push_back(p.abs_pos().y());
}
@@ -152,7 +153,7 @@
spline_drivetrain_.SetGoal(goal);
// Let the spline drivetrain compute the spline.
- ::frc971::control_loops::DrivetrainQueue::Status status;
+ ::frc971::control_loops::DrivetrainQueue::Status status;
do {
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
spline_drivetrain_.PopulateStatus(&status);
@@ -177,7 +178,7 @@
matplotlibcpp::plot(simulation_x_, simulation_y_, {{"label", "robot"}});
matplotlibcpp::plot(estimated_x_, estimated_y_,
{{"label", "estimation"}});
- for (const Target & target : targets_) {
+ for (const Target &target : targets_) {
PlotPlotPts(target.PlotPoints(), {{"c", "g"}});
}
for (const Obstacle &obstacle : true_obstacles_) {
@@ -341,7 +342,7 @@
// The period with which we should take frames from the cameras. Currently,
// we just trigger all the cameras at once, rather than offsetting them or
// anything.
- const int camera_period = 5; // cycles
+ const int camera_period = 5; // cycles
// The amount of time to delay the camera images from when they are taken, for
// each camera.
const ::std::array<milliseconds, 4> camera_latencies{
@@ -381,9 +382,7 @@
const ::Eigen::Matrix<double, 5, 1> goal_state =
spline_drivetrain_.CurrentGoalState();
simulation_t_.push_back(
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- t.time_since_epoch())
- .count());
+ ::aos::time::DurationInSeconds(t.time_since_epoch()));
spline_x_.push_back(goal_state(0, 0));
spline_y_.push_back(goal_state(1, 0));
simulation_x_.push_back(state(StateIdx::kX, 0));
@@ -401,10 +400,7 @@
state = ::frc971::control_loops::RungeKuttaU(
[this](const ::Eigen::Matrix<double, 10, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
- state, U,
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count());
+ state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
// Add arbitrary disturbances at some arbitrary interval. The main points of
// interest here are that we (a) stop adding disturbances at the very end of
diff --git a/y2019/control_loops/drivetrain/replay_localizer.cc b/y2019/control_loops/drivetrain/replay_localizer.cc
new file mode 100644
index 0000000..dd025f8
--- /dev/null
+++ b/y2019/control_loops/drivetrain/replay_localizer.cc
@@ -0,0 +1,415 @@
+#include <fcntl.h>
+
+#include "aos/init.h"
+#include "aos/logging/implementations.h"
+#include "aos/logging/replay.h"
+#include "aos/network/team_number.h"
+#include "aos/queue.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/wpilib/imu.q.h"
+#include "gflags/gflags.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
+
+DEFINE_string(logfile, "", "Path to the logfile to replay.");
+// TODO(james): Figure out how to reliably source team number from logfile.
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_int32(plot_duration, 30,
+ "Duration, in seconds, to plot after the start time.");
+DEFINE_int32(start_offset, 0,
+ "Time, in seconds, to start replay plot after the first enable.");
+
+namespace y2019 {
+namespace control_loops {
+namespace drivetrain {
+using ::y2019::constants::Field;
+
+typedef TypedLocalizer<
+ constants::Values::kNumCameras, Field::kNumTargets, Field::kNumObstacles,
+ EventLoopLocalizer::kMaxTargetsPerFrame, double> TestLocalizer;
+typedef typename TestLocalizer::Camera TestCamera;
+typedef typename TestCamera::Pose Pose;
+typedef typename TestCamera::LineSegment Obstacle;
+
+#if defined(SUPPORT_PLOT)
+// Plots a line from a vector of Pose's.
+void PlotPlotPts(const ::std::vector<Pose> &poses,
+ const ::std::map<::std::string, ::std::string> &kwargs) {
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ for (const Pose & p : poses) {
+ x.push_back(p.abs_pos().x());
+ y.push_back(p.abs_pos().y());
+ }
+ matplotlibcpp::plot(x, y, kwargs);
+}
+#endif
+
+class LocalizerReplayer {
+ // This class is for replaying logfiles from the 2019 robot and seeing how the
+ // localizer performs withi any new changes versus how it performed during the
+ // real match. This starts running replay on the first enable and then will
+ // actually plot a subset of the run based on the --plot_duration and
+ // --start_offset flags.
+ public:
+ typedef ::frc971::control_loops::DrivetrainQueue::Goal DrivetrainGoal;
+ typedef ::frc971::control_loops::DrivetrainQueue::Position DrivetrainPosition;
+ typedef ::frc971::control_loops::DrivetrainQueue::Status DrivetrainStatus;
+ typedef ::frc971::control_loops::DrivetrainQueue::Output DrivetrainOutput;
+ typedef ::frc971::IMUValues IMUValues;
+ typedef ::aos::JoystickState JoystickState;
+
+ LocalizerReplayer() : localizer_(GetDrivetrainConfig(), &event_loop_) {
+ replayer_.AddDirectQueueSender<CameraFrame>(
+ "wpilib_interface.stripped.IMU", "camera_frames",
+ ".y2019.control_loops.drivetrain.camera_frames");
+
+ const char *drivetrain = "drivetrain.stripped";
+
+ replayer_.AddHandler<DrivetrainPosition>(
+ drivetrain, "position", [this](const DrivetrainPosition &msg) {
+ if (has_been_enabled_) {
+ localizer_.Update(last_last_U_ * battery_voltage_ / 12.0,
+ msg.sent_time, msg.left_encoder,
+ msg.right_encoder, latest_gyro_, 0.0);
+ if (start_time_ <= msg.sent_time &&
+ start_time_ + plot_duration_ >= msg.sent_time) {
+ t_pos_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ new_x_.push_back(localizer_.x());
+ new_y_.push_back(localizer_.y());
+ new_theta_.push_back(localizer_.theta());
+ new_left_encoder_.push_back(localizer_.left_encoder());
+ new_right_encoder_.push_back(localizer_.right_encoder());
+ new_left_velocity_.push_back(localizer_.left_velocity());
+ new_right_velocity_.push_back(localizer_.right_velocity());
+
+ left_voltage_.push_back(last_last_U_(0, 0));
+ right_voltage_.push_back(last_last_U_(1, 0));
+ }
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainGoal>(
+ drivetrain, "goal", [this](const DrivetrainGoal &msg) {
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_goal_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ drivetrain_mode_.push_back(msg.controller_type);
+ throttle_.push_back(msg.throttle);
+ wheel_.push_back(msg.wheel);
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainStatus>(
+ drivetrain, "status", [this](const DrivetrainStatus &msg) {
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_status_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ original_x_.push_back(msg.x);
+ original_y_.push_back(msg.y);
+ original_theta_.push_back(msg.theta);
+ original_line_theta_.push_back(msg.line_follow_logging.rel_theta);
+ original_line_goal_theta_.push_back(
+ msg.line_follow_logging.goal_theta);
+ original_line_distance_.push_back(
+ msg.line_follow_logging.distance_to_target);
+ }
+ });
+
+ replayer_.AddHandler<DrivetrainOutput>(
+ drivetrain, "output", [this](const DrivetrainOutput &msg) {
+ last_last_U_ = last_U_;
+ last_U_ << msg.left_voltage, msg.right_voltage;
+ });
+
+ replayer_.AddHandler<frc971::control_loops::drivetrain::LocalizerControl>(
+ drivetrain, "localizer_control",
+ [this](const frc971::control_loops::drivetrain::LocalizerControl &msg) {
+ LOG_STRUCT(DEBUG, "localizer_control", msg);
+ localizer_.ResetPosition(msg.sent_time, msg.x, msg.y, msg.theta,
+ msg.theta_uncertainty,
+ !msg.keep_current_theta);
+ });
+
+ replayer_.AddHandler<JoystickState>(
+ "wpilib_interface.stripped.DSReader", "joystick_state",
+ [this](const JoystickState &msg) {
+ if (msg.enabled && !has_been_enabled_) {
+ has_been_enabled_ = true;
+ start_time_ =
+ msg.sent_time + ::std::chrono::seconds(FLAGS_start_offset);
+ }
+ });
+
+ replayer_.AddHandler<IMUValues>(
+ "wpilib_interface.stripped.IMU", "sending",
+ [this](const IMUValues &msg) {
+ latest_gyro_ = msg.gyro_z;
+ if (has_been_enabled_ && start_time_ <= msg.sent_time &&
+ (start_time_ + plot_duration_ >= msg.sent_time)) {
+ t_imu_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ msg.sent_time.time_since_epoch())
+ .count());
+ lat_accel_.push_back(msg.accelerometer_y);
+ long_accel_.push_back(msg.accelerometer_x);
+ z_accel_.push_back(msg.accelerometer_z);
+ }
+ });
+
+ replayer_.AddHandler<::aos::RobotState>(
+ "wpilib_interface.stripped.SensorReader", "robot_state",
+ [this](const ::aos::RobotState &msg) {
+ battery_voltage_ = msg.voltage_battery;
+ });
+ }
+
+ void ProcessFile(const char *filename) {
+ int fd;
+ if (strcmp(filename, "-") == 0) {
+ fd = STDIN_FILENO;
+ } else {
+ fd = open(filename, O_RDONLY);
+ }
+ if (fd == -1) {
+ PLOG(FATAL, "couldn't open file '%s' for reading", filename);
+ }
+
+ replayer_.OpenFile(fd);
+ ::aos::RawQueue *queue = ::aos::logging::GetLoggingQueue();
+ while (!replayer_.ProcessMessage()) {
+ const ::aos::logging::LogMessage *const msg =
+ static_cast<const ::aos::logging::LogMessage *>(
+ queue->ReadMessage(::aos::RawQueue::kNonBlock));
+ if (msg != NULL) {
+ ::aos::logging::internal::PrintMessage(stdout, *msg);
+
+ queue->FreeMessage(msg);
+ }
+ continue;
+ }
+ replayer_.CloseCurrentFile();
+
+ PCHECK(close(fd));
+
+ Plot();
+ }
+
+ void Plot() {
+#if defined(SUPPORT_PLOT)
+ int start_idx = 0;
+ for (const float t : t_pos_) {
+ if (t < ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ start_time_.time_since_epoch())
+ .count() +
+ 0.0) {
+ ++start_idx;
+ } else {
+ break;
+ }
+ }
+ ::std::vector<float> trunc_orig_x(original_x_.begin() + start_idx,
+ original_x_.end());
+ ::std::vector<float> trunc_orig_y(original_y_.begin() + start_idx,
+ original_y_.end());
+ ::std::vector<float> trunc_new_x(new_x_.begin() + start_idx, new_x_.end());
+ ::std::vector<float> trunc_new_y(new_y_.begin() + start_idx, new_y_.end());
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(trunc_orig_x, trunc_orig_y,
+ {{"label", "original"}, {"marker", "o"}});
+ matplotlibcpp::plot(trunc_new_x, trunc_new_y,
+ {{"label", "new"}, {"marker", "o"}});
+ matplotlibcpp::xlim(-0.1, 19.0);
+ matplotlibcpp::ylim(-5.0, 5.0);
+ Field field;
+ for (const Target &target : field.targets()) {
+ PlotPlotPts(target.PlotPoints(), {{"c", "g"}});
+ }
+ for (const Obstacle &obstacle : field.obstacles()) {
+ PlotPlotPts(obstacle.PlotPoints(), {{"c", "k"}});
+ }
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_status_, original_x_,
+ {{"label", "original x"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_y_,
+ {{"label", "original y"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_theta_,
+ {{"label", "original theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_x_, {{"label", "new x"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_y_, {{"label", "new y"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_theta_,
+ {{"label", "new theta"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_pos_, left_voltage_,
+ {{"label", "left voltage"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, right_voltage_,
+ {{"label", "right voltage"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_pos_, new_left_encoder_,
+ {{"label", "left encoder"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_right_encoder_,
+ {{"label", "right encoder"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_left_velocity_,
+ {{"label", "left velocity"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_pos_, new_right_velocity_,
+ {{"label", "right velocity"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_goal_, drivetrain_mode_,
+ {{"label", "mode"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_goal_, throttle_,
+ {{"label", "throttle"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_goal_, wheel_, {{"label", "wheel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_theta_,
+ {{"label", "relative theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_goal_theta_,
+ {{"label", "goal theta"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_status_, original_line_distance_,
+ {{"label", "dist to target"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ ::std::vector<double> filt_lat_accel_ = FilterValues(lat_accel_, 10);
+ ::std::vector<double> filt_long_accel_ = FilterValues(long_accel_, 10);
+ ::std::vector<double> filt_z_accel_ = FilterValues(z_accel_, 10);
+ matplotlibcpp::plot(t_imu_, filt_lat_accel_,
+ {{"label", "lateral accel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_imu_, filt_long_accel_,
+ {{"label", "longitudinal accel"}, {"marker", "o"}});
+ matplotlibcpp::plot(t_imu_, filt_z_accel_,
+ {{"label", "z accel"}, {"marker", "o"}});
+ matplotlibcpp::grid(true);
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+#endif
+ }
+
+ private:
+ // Do a simple moving average with a width of N over the given raw data and
+ // return the results. For calculating values with N/2 of either edge, we
+ // assume that the space past the edges is filled with zeroes.
+ ::std::vector<double> FilterValues(const ::std::vector<double> &raw, int N) {
+ ::std::vector<double> filt;
+ for (int ii = 0; ii < static_cast<int>(raw.size()); ++ii) {
+ double sum = 0;
+ for (int jj = ::std::max(0, ii - N / 2);
+ jj < ::std::min(ii + N / 2, static_cast<int>(raw.size()) - 1);
+ ++jj) {
+ sum += raw[jj];
+ }
+ filt.push_back(sum / static_cast<double>(N));
+ }
+ return filt;
+ }
+
+ // TODO(james): Move to some sort of virtual event loop.
+ ::aos::ShmEventLoop event_loop_;
+
+ EventLoopLocalizer localizer_;
+ // Whether the robot has been enabled yet.
+ bool has_been_enabled_ = false;
+ // Cache of last gyro value to forward to the localizer.
+ double latest_gyro_ = 0.0; // rad/sec
+ double battery_voltage_ = 12.0; // volts
+ ::Eigen::Matrix<double, 2, 1> last_U_{0, 0};
+ ::Eigen::Matrix<double, 2, 1> last_last_U_{0, 0};
+
+ ::aos::logging::linux_code::LogReplayer replayer_;
+
+ // Time at which to start the plot.
+ ::aos::monotonic_clock::time_point start_time_;
+ // Length of time to plot.
+ ::std::chrono::seconds plot_duration_{FLAGS_plot_duration};
+
+ // All the values to plot, organized in blocks by vectors of data that come in
+ // at the same time (e.g., status messages come in offset from position
+ // messages and so while they are all generally the same frequency, we can end
+ // receiving slightly different numbers of messages on different queues).
+
+ // TODO(james): Improve plotting support.
+ ::std::vector<double> t_status_;
+ ::std::vector<double> original_x_;
+ ::std::vector<double> original_y_;
+ ::std::vector<double> original_theta_;
+ ::std::vector<double> original_line_theta_;
+ ::std::vector<double> original_line_goal_theta_;
+ ::std::vector<double> original_line_distance_;
+
+ ::std::vector<double> t_pos_;
+ ::std::vector<double> new_x_;
+ ::std::vector<double> new_y_;
+ ::std::vector<double> new_left_encoder_;
+ ::std::vector<double> new_right_encoder_;
+ ::std::vector<double> new_left_velocity_;
+ ::std::vector<double> new_right_velocity_;
+ ::std::vector<double> new_theta_;
+ ::std::vector<double> left_voltage_;
+ ::std::vector<double> right_voltage_;
+
+ ::std::vector<double> t_goal_;
+ ::std::vector<double> drivetrain_mode_;
+ ::std::vector<double> throttle_;
+ ::std::vector<double> wheel_;
+
+ ::std::vector<double> t_imu_;
+ ::std::vector<double> lat_accel_;
+ ::std::vector<double> z_accel_;
+ ::std::vector<double> long_accel_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2019
+
+int main(int argc, char *argv[]) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ if (FLAGS_logfile.empty()) {
+ fprintf(stderr, "Need a file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::network::OverrideTeamNumber(FLAGS_team);
+
+ ::aos::InitCreate();
+
+ ::y2019::control_loops::drivetrain::LocalizerReplayer replay;
+ replay.ProcessFile(FLAGS_logfile.c_str());
+
+ // TODO(james): No clue if the Clear() functions are actually accomplishing
+ // things. Mostly just paranoia left over from earlier on.
+ ::frc971::imu_values.Clear();
+ ::frc971::control_loops::drivetrain_queue.goal.Clear();
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
+ ::frc971::control_loops::drivetrain_queue.position.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
+
+ ::aos::Cleanup();
+}
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index aa81380..036b210 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -28,8 +28,8 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::PositionSensorSimulator;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
@@ -352,9 +352,8 @@
CheckCollisions();
}
- 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 elevator_acceleration =
(superstructure_plant_.elevator_velocity() -
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 44a7c6f..baab2c2 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -32,9 +32,9 @@
class WebsocketHandler : public seasocks::WebSocket::Handler {
public:
WebsocketHandler();
- void onConnect(seasocks::WebSocket* connection) override;
- void onData(seasocks::WebSocket* connection, const char* data) override;
- void onDisconnect(seasocks::WebSocket* connection) override;
+ void onConnect(seasocks::WebSocket *connection) override;
+ void onData(seasocks::WebSocket *connection, const char *data) override;
+ void onDisconnect(seasocks::WebSocket *connection) override;
void SendData(const std::string &data);
@@ -42,8 +42,7 @@
std::set<seasocks::WebSocket *> connections_;
};
-WebsocketHandler::WebsocketHandler() {
-}
+WebsocketHandler::WebsocketHandler() {}
void WebsocketHandler::onConnect(seasocks::WebSocket *connection) {
connections_.insert(connection);
@@ -221,13 +220,9 @@
camera_pose.set_base(&robot_pose);
camera_debug->set_current_frame_age(
- chrono::duration_cast<chrono::duration<double>>(
- now - cur_frame.capture_time)
- .count());
+ ::aos::time::DurationInSeconds(now - cur_frame.capture_time));
camera_debug->set_time_since_last_target(
- chrono::duration_cast<chrono::duration<double>>(
- now - last_target_time[ii])
- .count());
+ ::aos::time::DurationInSeconds(now - last_target_time[ii]));
for (const auto &target : cur_frame.targets) {
frc971::control_loops::TypedPose<double> target_pose(
&camera_pose, {target.x, target.y, 0}, target.theta);