Add preliminary vision time adjuster
Change-Id: I3d920f0a689fb47ac238f7d845d7596b088e2d5d
diff --git a/aos/common/ring_buffer.h b/aos/common/ring_buffer.h
index 293c817..331991a 100644
--- a/aos/common/ring_buffer.h
+++ b/aos/common/ring_buffer.h
@@ -11,6 +11,8 @@
template <typename Data, size_t buffer_size>
class RingBuffer {
public:
+ static constexpr size_t kBufferSize = buffer_size;
+
RingBuffer() {}
// Add an item to the RingBuffer, overwriting the oldest element if necessary
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 63e06f1..bac8655 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -66,3 +66,32 @@
':superstructure_queue',
],
)
+
+cc_library(
+ name = 'vision_time_adjuster',
+ hdrs = [
+ 'vision_time_adjuster.h',
+ ],
+ srcs = [
+ 'vision_time_adjuster.cc',
+ ],
+ deps = [
+ ':superstructure_queue',
+ '//aos/common:ring_buffer',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2017/vision:vision_queue',
+ ],
+)
+
+cc_test(
+ name = 'vision_time_adjuster_test',
+ srcs = [
+ 'vision_time_adjuster_test.cc',
+ ],
+ deps = [
+ ':vision_time_adjuster',
+ '//aos/common:time',
+ '//aos/testing:googletest',
+ '//aos/testing:test_shm',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
new file mode 100644
index 0000000..b93e1d5
--- /dev/null
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -0,0 +1,171 @@
+#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/vision/vision.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
+namespace {
+
+// Finds the two data points in the ring buffer between which the specified
+// time falls. The results are stored in "before" and "after. If the specified
+// time happens to co-incide perfectly with a data point, then "before" and
+// "after" will both point to that data point.
+template <typename Data, size_t buffer_size>
+bool FindBeforeAndAfter(const aos::RingBuffer<Data, buffer_size> &data,
+ monotonic_clock::time_point time, Data *before,
+ Data *after) {
+ // With no data, there's nothing we can do.
+ if (data.size() == 0) return false;
+
+ // If we only have one data point, we pretend that it's that data point.
+ if (data.size() == 1) {
+ *before = data[0];
+ *after = data[0];
+ return true;
+ }
+
+ // Find the two data points that match the two robots.
+ for (size_t i = 0; i < data.size(); ++i) {
+ if (data[i].time > time) {
+ if (i == 0) {
+ // If this if the first data point and it's already past the time stamp
+ // we're looking for then we just use the first data point for both.
+ *before = data[i];
+ } else {
+ *before = data[i - 1];
+ }
+ *after = data[i];
+ return true;
+ }
+ }
+
+ // We've hit the end of our data. Just fill both of them as the last data
+ // point we have.
+ *before = data[data.size() - 1];
+ *after = data[data.size() - 1];
+ return true;
+}
+
+double Interpolate(monotonic_clock::time_point before_time, double before_data,
+ 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();
+ return before_data * (1 - age_ratio) + after_data * age_ratio;
+}
+
+double ComputeColumnPosition(const VisionTimeAdjuster::ColumnAngle &position) {
+ return position.turret;
+}
+
+double ComputeDrivetrainPosition(
+ const VisionTimeAdjuster::DrivetrainAngle &position) {
+ return (position.right - position.left) /
+ (::y2017::control_loops::drivetrain::kRobotRadius * 2.0);
+}
+
+template <typename Data, size_t buffer_size>
+bool ComputeAngle(const ::aos::RingBuffer<Data, buffer_size> &data,
+ monotonic_clock::time_point time,
+ ::std::function<double(const Data &)> compute_position,
+ double *result) {
+ Data before;
+ Data after;
+ bool offset_is_valid = false;
+
+ if (FindBeforeAndAfter(data, time, &before, &after)) {
+ double position;
+ double before_position = compute_position(before);
+ double after_position = compute_position(after);
+ if (before.time == after.time) {
+ position = before_position;
+ } else {
+ position = Interpolate(before.time, before_position, after.time,
+ after_position, time);
+ }
+ offset_is_valid = true;
+ *result = position;
+ }
+
+ return offset_is_valid;
+}
+
+} // namespace
+
+VisionTimeAdjuster::VisionTimeAdjuster() {}
+
+void VisionTimeAdjuster::Tick(monotonic_clock::time_point monotonic_now,
+ double turret_position) {
+ // We have new column data, store it.
+ column_data_.Push({.time = monotonic_now, .turret = turret_position});
+
+ // If we have new drivetrain data, we store it.
+ if (::frc971::control_loops::drivetrain_queue.status.FetchLatest()) {
+ const auto &position = ::frc971::control_loops::drivetrain_queue.status;
+ DrivetrainAngle new_position{.time = position->sent_time,
+ .left = position->estimated_left_position,
+ .right = position->estimated_right_position};
+ drivetrain_data_.Push(new_position);
+ most_recent_drivetrain_angle_ = ComputeDrivetrainPosition(new_position);
+ }
+
+ // If we have new vision data, compute the newest absolute angle at which the
+ // target is.
+ if (vision::vision_status.FetchLatest() &&
+ vision::vision_status->image_valid) {
+ monotonic_clock::time_point last_target_time(
+ monotonic_clock::duration(vision::vision_status->target_time));
+
+ double column_angle = 0;
+ double drivetrain_angle = 0;
+
+ bool column_angle_is_valid = ComputeAngle<ColumnAngle>(
+ column_data_, last_target_time, ComputeColumnPosition, &column_angle);
+ bool drivetrain_angle_is_valid = ComputeAngle<DrivetrainAngle>(
+ drivetrain_data_, last_target_time, ComputeDrivetrainPosition,
+ &drivetrain_angle);
+
+ 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());
+ most_recent_vision_reading_ = vision::vision_status->angle;
+ most_recent_vision_angle_ =
+ vision::vision_status->angle + column_angle + drivetrain_angle;
+ most_recent_vision_time_ = monotonic_now;
+ }
+ }
+
+ goal_ = most_recent_vision_angle_ - most_recent_drivetrain_angle_;
+ LOG(INFO, "Vision angle %f drivetrain %f\n", most_recent_vision_angle_,
+ most_recent_drivetrain_angle_);
+
+ // Now, update the vision valid flag to tell us if we have a valid vision
+ // angle within the last two seconds.
+ if (monotonic_now < most_recent_vision_time_ + chrono::seconds(2)) {
+ valid_ = true;
+ } else {
+ valid_ = false;
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
new file mode 100644
index 0000000..19307e3
--- /dev/null
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -0,0 +1,70 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_TIME_ADJUSTER_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_TIME_ADJUSTER_H_
+
+#include <stdint.h>
+
+#include "aos/common/ring_buffer.h"
+#include "aos/common/time.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+class VisionTimeAdjuster {
+ public:
+ VisionTimeAdjuster();
+
+ // This needs to be called at the same interval as the control loops so that
+ // it can attempt to make accurate goal recommendations.
+ void Tick(::aos::monotonic_clock::time_point monotonic_now,
+ double turret_position);
+
+ // Returns true if we have enough data to recommend a goal for the turret.
+ bool valid() const { return valid_; }
+
+ // Returns the goal that we are recommending for the turret. This value is
+ // only valid if valid() returns true.
+ double goal() const { return goal_; }
+
+ double most_recent_vision_angle() const { return most_recent_vision_angle_; }
+ double most_recent_vision_reading() const {
+ return most_recent_vision_reading_;
+ }
+
+ struct ColumnAngle {
+ ::aos::monotonic_clock::time_point time;
+ double turret;
+ };
+
+ struct DrivetrainAngle {
+ ::aos::monotonic_clock::time_point time;
+ double left;
+ double right;
+ };
+
+ private:
+ // Buffer space to store the most recent drivetrain and turret messages from
+ // the last second.
+ ::aos::RingBuffer<ColumnAngle, 200> column_data_;
+ ::aos::RingBuffer<DrivetrainAngle, 200> drivetrain_data_;
+
+ // The most recently computed goal angle of the turret. This does not yet
+ // include the most recent drivetrain angle. Subtract the most recent
+ // drivetrain angle from this to get the recommended turret goal.
+ double most_recent_vision_angle_ = 0.0;
+ ::aos::monotonic_clock::time_point most_recent_vision_time_ =
+ ::aos::monotonic_clock::min_time;
+
+ // The most recent angle of the drivetrain.
+ double most_recent_drivetrain_angle_ = 0.0;
+ double most_recent_vision_reading_ = 0.0;
+
+ double goal_ = 0.0;
+ bool valid_ = false;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_TIME_ADJUSTER_H_
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
new file mode 100644
index 0000000..3031121
--- /dev/null
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -0,0 +1,202 @@
+#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/common/time.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/vision/vision.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+class VisionTimeAdjusterTest : public ::testing::Test {
+ public:
+ VisionTimeAdjusterTest() : ::testing::Test() {
+ static_assert(kVisionDelay % kTimeTick == ::std::chrono::milliseconds(0),
+ "kVisionDelay must be a multiple of kTimeTick.");
+ static_assert(
+ kVisionDelay / kTimeTick == decltype(turret_history_)::kBufferSize,
+ "We need a different amount of space to hold the turret data.");
+ ::aos::time::EnableMockTime(current_time_);
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
+ superstructure_queue.status.Clear();
+ vision::vision_status.Clear();
+ }
+
+ ~VisionTimeAdjusterTest() {
+ superstructure_queue.status.Clear();
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
+ vision::vision_status.Clear();
+ ::aos::time::DisableMockTime();
+ }
+
+ protected:
+ static constexpr ::std::chrono::milliseconds kTimeTick{5};
+
+ // We get frames at 20 Hz and we assume a 15 ms delay.
+ static constexpr ::std::chrono::milliseconds kVisionTick{50};
+ static constexpr ::std::chrono::milliseconds kVisionDelay{15};
+
+ void SetTurretPosition(double turret) { turret_angle_ = turret; }
+
+ void SetDrivetrainPosition(double left, double right) {
+ drivetrain_left_ = left;
+ drivetrain_right_ = right;
+ }
+
+ void SetVisionAngle(double angle) { vision_angle_ = angle; }
+
+ void RunForTime(::aos::monotonic_clock::duration run_for) {
+ const auto start_time = ::aos::monotonic_clock::now();
+ while (::aos::monotonic_clock::now() < start_time + run_for) {
+ RunIteration();
+ }
+ }
+
+ void RunIteration() {
+ SendMessages();
+ adjuster_.Tick();
+ TickTime();
+ }
+
+ static constexpr size_t GetVisionDelayCount() {
+ return kVisionDelay / kTimeTick;
+ }
+
+ VisionTimeAdjuster adjuster_;
+
+ private:
+ void TickTime() { ::aos::time::SetMockTime(current_time_ += kTimeTick); }
+
+ void SendMessages() {
+ SendTurretPosition();
+ SendDrivetrainPosition();
+ if (::aos::monotonic_clock::now().time_since_epoch() % kVisionTick ==
+ kVisionDelay) {
+ SendVisionTarget();
+ }
+ turret_history_.Push(turret_angle_);
+ drivetrain_history_.Push(GetDriveTrainAngle());
+ }
+
+ void SendTurretPosition() {
+ auto message = superstructure_queue.status.MakeMessage();
+ message->turret.position = turret_angle_;
+ ASSERT_TRUE(message.Send());
+ }
+
+ void SendDrivetrainPosition() {
+ auto message =
+ ::frc971::control_loops::drivetrain_queue.status.MakeMessage();
+ message->estimated_left_position = drivetrain_left_;
+ message->estimated_right_position = drivetrain_right_;
+ ASSERT_TRUE(message.Send());
+ }
+
+ void SendVisionTarget() {
+ auto message = vision::vision_status.MakeMessage();
+ message->target_time = (::aos::monotonic_clock::now() - kVisionDelay)
+ .time_since_epoch()
+ .count();
+ message->distance = vision_distance_;
+ ASSERT_EQ(turret_history_.capacity(), turret_history_.size());
+ ASSERT_EQ(drivetrain_history_.capacity(), drivetrain_history_.size());
+ message->angle =
+ vision_angle_ - turret_history_[0] - drivetrain_history_[0];
+ ASSERT_TRUE(message.Send());
+ }
+
+ double GetDriveTrainAngle() const {
+ // TODO(user): Implement this.
+ static constexpr double kDriveTrainRadius = 1.0;
+ return -(drivetrain_left_ - drivetrain_right_) / kDriveTrainRadius;
+ }
+
+ ::aos::testing::TestSharedMemory my_shm_;
+
+ ::aos::monotonic_clock::time_point current_time_ =
+ ::aos::monotonic_clock::epoch();
+
+ ::aos::RingBuffer<double, 3> turret_history_;
+ ::aos::RingBuffer<double, 3> drivetrain_history_;
+
+ // The turret angle with respect to the robot base.
+ double turret_angle_ = 0.0;
+
+ // Absolute angle of the robot with respect to the ground.
+ double drivetrain_left_ = 0.0;
+ double drivetrain_right_ = 0.0;
+
+ // The absolute angle for the target (i.e. with respect to the ground),
+ // irrespective of the turret and robot angle and such.
+ double vision_distance_ = 0.0;
+ double vision_angle_ = 0.0;
+};
+
+constexpr ::std::chrono::milliseconds VisionTimeAdjusterTest::kTimeTick;
+constexpr ::std::chrono::milliseconds VisionTimeAdjusterTest::kVisionTick;
+constexpr ::std::chrono::milliseconds VisionTimeAdjusterTest::kVisionDelay;
+
+// Test that moving only the turret around results in the correct turret goal.
+TEST_F(VisionTimeAdjusterTest, TurretRotationOnly) {
+ SetDrivetrainPosition(0, 0);
+ SetVisionAngle(M_PI / 5);
+ for (size_t i = 0; i < 200; ++i) {
+ SetTurretPosition(static_cast<int>(i) * 0.01);
+ ASSERT_NO_FATAL_FAILURE(RunIteration());
+
+ if (i < GetVisionDelayCount()) {
+ EXPECT_FALSE(adjuster_.valid());
+ } else {
+ ASSERT_TRUE(adjuster_.valid());
+ EXPECT_NEAR(M_PI / 5, adjuster_.goal(), 0.00001);
+ }
+ }
+}
+
+// Tests that moving only the drivetrain around results in the correct turret
+// goal.
+TEST_F(VisionTimeAdjusterTest, DrivetrainRotationOnly) {
+ SetTurretPosition(0.0);
+ SetVisionAngle(0.0);
+ for (size_t i = 0; i < 200; ++i) {
+ double angle = static_cast<double>(i) * 0.01;
+ SetDrivetrainPosition(angle, -angle);
+ ASSERT_NO_FATAL_FAILURE(RunIteration());
+
+ if (i < GetVisionDelayCount()) {
+ EXPECT_FALSE(adjuster_.valid());
+ } else {
+ ASSERT_TRUE(adjuster_.valid());
+ EXPECT_NEAR(2 * angle, adjuster_.goal(), 0.00001);
+ }
+ }
+}
+
+// Tests that moving both the drivetrain and the turret results in the correct
+// turret angle.
+TEST_F(VisionTimeAdjusterTest, DrivetrainAndTurretTogether) {
+ SetVisionAngle(-M_PI / 6);
+
+ for (size_t i = 0; i < 200; ++i) {
+ double drivetrain_angle = static_cast<double>(i) * 0.01;
+ double turret_angle = (100.0 - static_cast<double>(i)) * 0.005;
+ SetDrivetrainPosition(-drivetrain_angle, drivetrain_angle);
+ SetTurretPosition(turret_angle);
+ ASSERT_NO_FATAL_FAILURE(RunIteration());
+
+ if (i < GetVisionDelayCount()) {
+ EXPECT_FALSE(adjuster_.valid());
+ } else {
+ ASSERT_TRUE(adjuster_.valid());
+ EXPECT_NEAR(-M_PI / 6 - 2 * drivetrain_angle, adjuster_.goal(), 0.00001);
+ }
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017