Move VisionStatus to an event loop
Convert the simulation over to using SimulatedEventLoop as well.
Change-Id: I05d837530f83df5432f056ca5ef6515fbab5f897
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index e7e6f73..5a00bfe 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -26,6 +26,7 @@
":superstructure_queue",
":vision_distance_average",
"//aos/controls:control_loop",
+ "//aos/events:event-loop",
"//y2017:constants",
"//y2017/control_loops/superstructure/column",
"//y2017/control_loops/superstructure/hood",
@@ -93,8 +94,9 @@
],
deps = [
":vision_time_adjuster",
+ "//aos/events:simulated_event_loop",
"//aos/testing:googletest",
- "//aos/testing:test_shm",
+ "//aos/testing:test_logging",
"//aos/time",
],
)
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 85af86b..b4762a7 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -364,8 +364,9 @@
status->stuck_voltage = IndexerStuckVoltage();
}
-Column::Column()
- : profiled_subsystem_(
+Column::Column(::aos::EventLoop *event_loop)
+ : vision_time_adjuster_(event_loop),
+ profiled_subsystem_(
::std::unique_ptr<
::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>>(
new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 4a4dfbd..0caa66e 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -159,7 +159,7 @@
class Column {
public:
- Column();
+ Column(::aos::EventLoop *event_loop);
double goal(int row, int col) const {
return profiled_subsystem_.goal(row, col);
}
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 730cd24..9b0297e 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,11 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name) {
+ name),
+ vision_status_fetcher_(
+ event_loop->MakeFetcher<::y2017::vision::VisionStatus>(
+ ".y2017.vision.vision_status")),
+ column_(event_loop) {
shot_interpolation_table_ =
::frc971::shooter_interpolation::InterpolationTable<ShotParams>({
// { distance_to_target, { shot_angle, shot_power, indexer_velocity }},
@@ -57,8 +61,8 @@
}
const vision::VisionStatus *vision_status = nullptr;
- if (vision::vision_status.FetchLatest()) {
- vision_status = vision::vision_status.get();
+ if (vision_status_fetcher_.Fetch()) {
+ vision_status = vision_status_fetcher_.get();
}
// Create a copy of the goals so that we can modify them.
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 580b6a5..e387cf2 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include <memory>
#include "aos/controls/control_loop.h"
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
@@ -41,6 +42,8 @@
control_loops::SuperstructureQueue::Status *status) override;
private:
+ ::aos::Fetcher<::y2017::vision::VisionStatus> vision_status_fetcher_;
+
hood::Hood hood_;
intake::Intake intake_;
shooter::Shooter shooter_;
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index 1e9c94f..4952f6d 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -102,7 +102,11 @@
} // namespace
-VisionTimeAdjuster::VisionTimeAdjuster() {}
+VisionTimeAdjuster::VisionTimeAdjuster(::aos::EventLoop *event_loop)
+ : drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")) {}
void VisionTimeAdjuster::Tick(monotonic_clock::time_point monotonic_now,
double turret_position,
@@ -111,8 +115,8 @@
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;
+ if (drivetrain_status_fetcher_.Fetch()) {
+ const auto &position = drivetrain_status_fetcher_.get();
DrivetrainAngle new_position{.time = position->sent_time,
.left = position->estimated_left_position,
.right = position->estimated_right_position};
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index b475a99..b45f827 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -4,7 +4,9 @@
#include <stdint.h>
#include "aos/containers/ring_buffer.h"
+#include "aos/events/event-loop.h"
#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/vision/vision.q.h"
namespace y2017 {
@@ -13,7 +15,7 @@
class VisionTimeAdjuster {
public:
- VisionTimeAdjuster();
+ VisionTimeAdjuster(::aos::EventLoop *event_loop);
// This needs to be called at the same interval as the control loops so that
// it can attempt to make accurate goal recommendations.
@@ -48,6 +50,10 @@
}
private:
+ // Fetcher to grab the latest drivetrain message.
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+
// Buffer space to store the most recent drivetrain and turret messages from
// the last second.
::aos::RingBuffer<ColumnAngle, 200> column_data_;
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
index 8dc3998..4f341de 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -2,8 +2,9 @@
#include "gtest/gtest.h"
+#include "aos/events/simulated-event-loop.h"
+#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
-#include "aos/testing/test_shm.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2017/vision/vision.q.h"
@@ -14,22 +15,31 @@
class VisionTimeAdjusterTest : public ::testing::Test {
public:
- VisionTimeAdjusterTest() : ::testing::Test() {
+ VisionTimeAdjusterTest()
+ : ::testing::Test(),
+ simulation_event_loop_(simulated_event_loop_factory_.MakeEventLoop()),
+ drivetrain_status_sender_(
+ simulation_event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
+ vision_status_sender_(
+ simulation_event_loop_->MakeSender<::y2017::vision::VisionStatus>(
+ ".y2017.vision.vision_status")),
+ vision_time_adjuster_event_loop_(
+ simulated_event_loop_factory_.MakeEventLoop()),
+ vision_status_fetcher_(vision_time_adjuster_event_loop_
+ ->MakeFetcher<::y2017::vision::VisionStatus>(
+ ".y2017.vision.vision_status")),
+ adjuster_(vision_time_adjuster_event_loop_.get()) {
+ ::aos::testing::EnableTestLogging();
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();
- vision::vision_status.Clear();
}
- ~VisionTimeAdjusterTest() {
- ::frc971::control_loops::drivetrain_queue.status.Clear();
- vision::vision_status.Clear();
- ::aos::time::DisableMockTime();
- }
+ ~VisionTimeAdjusterTest() {}
protected:
static constexpr ::std::chrono::milliseconds kTimeTick{5};
@@ -47,20 +57,14 @@
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();
const vision::VisionStatus *vision_status = nullptr;
- if (vision::vision_status.FetchLatest()) {
- vision_status = vision::vision_status.get();
+ if (vision_status_fetcher_.Fetch()) {
+ vision_status = vision_status_fetcher_.get();
}
- adjuster_.Tick(::aos::monotonic_clock::now(), turret_angle_, vision_status);
+ adjuster_.Tick(vision_time_adjuster_event_loop_->monotonic_now(),
+ turret_angle_, vision_status);
TickTime();
}
@@ -68,14 +72,15 @@
return kVisionDelay / kTimeTick;
}
- VisionTimeAdjuster adjuster_;
+ VisionTimeAdjuster *adjuster() { return &adjuster_; }
private:
- void TickTime() { ::aos::time::SetMockTime(current_time_ += kTimeTick); }
+ void TickTime() { simulated_event_loop_factory_.RunFor(kTimeTick); }
void SendMessages() {
SendDrivetrainPosition();
- if (::aos::monotonic_clock::now().time_since_epoch() % kVisionTick ==
+ if (simulated_event_loop_factory_.monotonic_now().time_since_epoch() %
+ kVisionTick ==
kVisionDelay) {
SendVisionTarget();
}
@@ -84,18 +89,18 @@
}
void SendDrivetrainPosition() {
- auto message =
- ::frc971::control_loops::drivetrain_queue.status.MakeMessage();
+ auto message = drivetrain_status_sender_.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();
+ auto message = vision_status_sender_.MakeMessage();
+ message->target_time =
+ (simulation_event_loop_->monotonic_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());
@@ -110,7 +115,16 @@
(drivetrain::kRobotRadius * 2.0);
}
- ::aos::testing::TestSharedMemory my_shm_;
+ ::aos::SimulatedEventLoopFactory simulated_event_loop_factory_;
+ ::std::unique_ptr<::aos::EventLoop> simulation_event_loop_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_sender_;
+ ::aos::Sender<::y2017::vision::VisionStatus> vision_status_sender_;
+
+ ::std::unique_ptr<::aos::EventLoop> vision_time_adjuster_event_loop_;
+ ::aos::Fetcher<::y2017::vision::VisionStatus> vision_status_fetcher_;
+
+ VisionTimeAdjuster adjuster_;
::aos::monotonic_clock::time_point current_time_ =
::aos::monotonic_clock::epoch();
@@ -144,10 +158,10 @@
ASSERT_NO_FATAL_FAILURE(RunIteration());
if (i < GetVisionDelayCount()) {
- EXPECT_FALSE(adjuster_.valid());
+ EXPECT_FALSE(adjuster()->valid());
} else {
- ASSERT_TRUE(adjuster_.valid());
- EXPECT_NEAR(M_PI / 5, adjuster_.goal(), 0.00001);
+ ASSERT_TRUE(adjuster()->valid());
+ EXPECT_NEAR(M_PI / 5, adjuster()->goal(), 0.00001);
}
}
}
@@ -164,10 +178,10 @@
ASSERT_NO_FATAL_FAILURE(RunIteration());
if (i < GetVisionDelayCount()) {
- EXPECT_FALSE(adjuster_.valid());
+ EXPECT_FALSE(adjuster()->valid());
} else {
- ASSERT_TRUE(adjuster_.valid());
- EXPECT_NEAR(2 * angle, adjuster_.goal(), 0.00001);
+ ASSERT_TRUE(adjuster()->valid());
+ EXPECT_NEAR(2 * angle, adjuster()->goal(), 0.00001);
}
}
}
@@ -186,10 +200,11 @@
ASSERT_NO_FATAL_FAILURE(RunIteration());
if (i < GetVisionDelayCount()) {
- EXPECT_FALSE(adjuster_.valid());
+ EXPECT_FALSE(adjuster()->valid());
} else {
- ASSERT_TRUE(adjuster_.valid());
- EXPECT_NEAR(-M_PI / 6 - 2 * drivetrain_angle, adjuster_.goal(), 0.00001);
+ ASSERT_TRUE(adjuster()->valid());
+ EXPECT_NEAR(-M_PI / 6 - 2 * drivetrain_angle, adjuster()->goal(),
+ 0.00001);
}
}
}