Convert control loop tests over to simulated event loop
This makes it so that we properly only use ShmEventLoop for running in
realtime on a robot. Very nice.
Change-Id: I46b770b336f59e08cfaf28511b3bd5689f72fff1
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index 30fe5c6..a612ecb 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -27,7 +27,7 @@
"control_loop_test.h",
],
deps = [
- "//aos/events:shm-event-loop",
+ "//aos/events:simulated_event_loop",
"//aos/logging:queue_logging",
"//aos/robot_state",
"//aos/testing:googletest",
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 2b57a0b..4f48737 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -23,26 +23,7 @@
}
template <class T>
-void ControlLoop<T>::Iterate() {
- if (!has_iterate_fetcher_) {
- iterate_position_fetcher_ =
- event_loop_->MakeFetcher<PositionType>(name_ + ".position");
- has_iterate_fetcher_ = true;
- }
- const bool did_fetch = iterate_position_fetcher_.Fetch();
- if (!did_fetch) {
- LOG(FATAL, "Failed to fetch from position queue\n");
- }
- IteratePosition(*iterate_position_fetcher_);
-}
-
-template <class T>
void ControlLoop<T>::IteratePosition(const PositionType &position) {
- // Since Exit() isn't async safe, we want to call Exit from the periodic
- // handler.
- if (!run_) {
- event_loop_->Exit();
- }
no_goal_.Print();
no_sensor_state_.Print();
motors_off_log_.Print();
@@ -119,28 +100,5 @@
status.Send();
}
-template <class T>
-void ControlLoop<T>::Run() {
- struct sigaction action;
- action.sa_handler = &ControlLoop<T>::Quit;
- sigemptyset(&action.sa_mask);
- action.sa_flags = SA_RESETHAND;
-
- PCHECK(sigaction(SIGTERM, &action, nullptr));
- PCHECK(sigaction(SIGQUIT, &action, nullptr));
- PCHECK(sigaction(SIGINT, &action, nullptr));
-
- event_loop_->MakeWatcher(name_ + ".position",
- [this](const PositionType &position) {
- this->IteratePosition(position);
- });
-
- event_loop_->Run();
- LOG(INFO, "Shutting down\n");
-}
-
-template <class T>
-::std::atomic<bool> ControlLoop<T>::run_{true};
-
} // namespace controls
} // namespace aos
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 56353c1..fe72022 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -15,16 +15,6 @@
namespace aos {
namespace controls {
-// Interface to describe runnable jobs.
-class Runnable {
- public:
- virtual ~Runnable() {}
- // Runs forever.
- virtual void Run() = 0;
- // Does one quick piece of work and return. Does _not_ block.
- virtual void Iterate() = 0;
-};
-
// Control loops run this often, "starting" at time 0.
constexpr ::std::chrono::nanoseconds kLoopFrequency =
::std::chrono::milliseconds(5);
@@ -35,7 +25,7 @@
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
template <class T>
-class ControlLoop : public Runnable {
+class ControlLoop {
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
// and Output structures.
@@ -58,6 +48,11 @@
event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
joystick_state_fetcher_ =
event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+
+ event_loop_->MakeWatcher(name_ + ".position",
+ [this](const PositionType &position) {
+ this->IteratePosition(position);
+ });
}
const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
@@ -87,22 +82,12 @@
// subsystem.
virtual void Zero(OutputType *output) { output->Zero(); }
- // Runs the loop forever.
- // TODO(austin): This should move to the event loop once it gets hoisted out.
- void Run() override;
-
- // Runs one cycle of the loop.
- // TODO(austin): This should go away when all the tests use event loops
- // directly.
- void Iterate() override;
-
protected:
+ // Runs one cycle of the loop.
void IteratePosition(const PositionType &position);
EventLoop *event_loop() { return event_loop_; }
- static void Quit(int /*signum*/) { run_ = false; }
-
// Runs an iteration of the control loop.
// goal is the last goal that was sent. It might be any number of cycles old
// or nullptr if we haven't ever received a goal.
@@ -126,7 +111,6 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
- ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
EventLoop *event_loop_;
::std::string name_;
@@ -154,8 +138,6 @@
SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
SimpleLogInterval no_goal_ =
SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
-
- static ::std::atomic<bool> run_;
};
} // namespace controls
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index 95f1c7d..85d610e 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -5,7 +5,7 @@
#include "gtest/gtest.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/simulated-event-loop.h"
#include "aos/logging/queue_logging.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/testing/test_shm.h"
@@ -22,68 +22,43 @@
template <typename TestBaseClass>
class ControlLoopTestTemplated : public TestBaseClass {
public:
- ControlLoopTestTemplated() {
+ ControlLoopTestTemplated(::std::chrono::nanoseconds dt = kTimeTick)
+ : dt_(dt), robot_status_event_loop_(MakeEventLoop()) {
robot_state_sender_ =
- test_event_loop_.MakeSender<::aos::RobotState>(".aos.robot_state");
+ robot_status_event_loop_->MakeSender<::aos::RobotState>(
+ ".aos.robot_state");
joystick_state_sender_ =
- test_event_loop_.MakeSender<::aos::JoystickState>(".aos.joystick_state");
+ robot_status_event_loop_->MakeSender<::aos::JoystickState>(
+ ".aos.joystick_state");
- ::aos::time::EnableMockTime(current_time_);
+ // Schedule the robot status send 1 nanosecond before the loop runs.
+ send_robot_state_phased_loop_ = robot_status_event_loop_->AddPhasedLoop(
+ [this](int) { SendRobotState(); }, dt_,
+ dt - ::std::chrono::nanoseconds(1));
- SendMessages(false);
+ send_joystick_state_timer_ =
+ robot_status_event_loop_->AddTimer([this]() { SendJoystickState(); });
+
+ robot_status_event_loop_->OnRun([this]() {
+ send_joystick_state_timer_->Setup(
+ robot_status_event_loop_->monotonic_now(), dt_);
+ });
}
- virtual ~ControlLoopTestTemplated() {
- ::aos::time::DisableMockTime();
- }
+ virtual ~ControlLoopTestTemplated() {}
void set_team_id(uint16_t team_id) { team_id_ = team_id; }
uint16_t team_id() const { return team_id_; }
- // Sends out all of the required queue messages.
- void SendMessages(bool enabled) {
- if (current_time_ >= kDSPacketTime + last_ds_time_ ||
- last_enabled_ != enabled) {
- last_ds_time_ = current_time_;
- auto new_state = joystick_state_sender_.MakeMessage();
- new_state->fake = true;
-
- new_state->enabled = enabled;
- new_state->autonomous = false;
- new_state->team_id = team_id_;
-
- new_state.Send();
- last_enabled_ = enabled;
+ // Sets the enabled/disabled bit and (potentially) rebroadcasts the robot
+ // state messages.
+ void SetEnabled(bool enabled) {
+ if (enabled_ != enabled) {
+ enabled_ = enabled;
+ SendJoystickState();
+ SendRobotState();
+ send_joystick_state_timer_->Setup(
+ robot_status_event_loop_->monotonic_now(), dt_);
}
-
- {
- auto new_state = robot_state_sender_.MakeMessage();
-
- new_state->reader_pid = reader_pid_;
- new_state->outputs_enabled = enabled;
- new_state->browned_out = false;
-
- new_state->is_3v3_active = true;
- new_state->is_5v_active = true;
- new_state->voltage_3v3 = 3.3;
- new_state->voltage_5v = 5.0;
-
- new_state->voltage_roborio_in = battery_voltage_;
- new_state->voltage_battery = battery_voltage_;
-
- LOG_STRUCT(INFO, "robot_state", *new_state);
- new_state.Send();
- }
- }
- // Ticks time for a single control loop cycle.
- void TickTime(::std::chrono::nanoseconds dt = kTimeTick) {
- ::aos::time::SetMockTime(current_time_ += dt);
- }
-
- // Simulates everything that happens during 1 loop time step.
- void SimulateTimestep(bool enabled,
- ::std::chrono::nanoseconds dt = kTimeTick) {
- SendMessages(enabled);
- TickTime(dt);
}
// Simulate a reset of the process reading sensors, which tells loops that all
@@ -97,33 +72,91 @@
battery_voltage_ = battery_voltage;
}
+ ::std::unique_ptr<::aos::EventLoop> MakeEventLoop() {
+ return event_loop_factory_.MakeEventLoop();
+ }
+
+ void RunFor(monotonic_clock::duration duration) {
+ event_loop_factory_.RunFor(duration);
+ }
+
+ ::aos::monotonic_clock::time_point monotonic_now() {
+ return event_loop_factory_.monotonic_now();
+ }
+
+ ::std::chrono::nanoseconds dt() const { return dt_; }
+
private:
- static constexpr ::std::chrono::milliseconds kTimeTick{5};
+ // Sends out all of the required queue messages.
+ void SendJoystickState() {
+ if (monotonic_now() >= kDSPacketTime + last_ds_time_ ||
+ last_enabled_ != enabled_) {
+ auto new_state = joystick_state_sender_.MakeMessage();
+ new_state->fake = true;
+
+ new_state->enabled = enabled_;
+ new_state->autonomous = false;
+ new_state->team_id = team_id_;
+
+ LOG_STRUCT(INFO, "joystick_state", *new_state);
+ new_state.Send();
+
+ last_ds_time_ = monotonic_now();
+ last_enabled_ = enabled_;
+ }
+ }
+
+ bool last_enabled_ = false;
+
+ void SendRobotState() {
+ auto new_state = robot_state_sender_.MakeMessage();
+
+ new_state->reader_pid = reader_pid_;
+ new_state->outputs_enabled = enabled_;
+ new_state->browned_out = false;
+
+ new_state->is_3v3_active = true;
+ new_state->is_5v_active = true;
+ new_state->voltage_3v3 = 3.3;
+ new_state->voltage_5v = 5.0;
+
+ new_state->voltage_roborio_in = battery_voltage_;
+ new_state->voltage_battery = battery_voltage_;
+
+ LOG_STRUCT(INFO, "robot_state", *new_state);
+ new_state.Send();
+ }
+
+ static constexpr ::std::chrono::microseconds kTimeTick{5000};
static constexpr ::std::chrono::milliseconds kDSPacketTime{20};
+ const ::std::chrono::nanoseconds dt_;
+
+ SimulatedEventLoopFactory event_loop_factory_;
+
uint16_t team_id_ = 971;
int32_t reader_pid_ = 1;
double battery_voltage_ = 12.4;
::aos::monotonic_clock::time_point last_ds_time_ =
- ::aos::monotonic_clock::epoch();
- ::aos::monotonic_clock::time_point current_time_ =
- ::aos::monotonic_clock::epoch();
+ ::aos::monotonic_clock::min_time;
- ::aos::testing::TestSharedMemory my_shm_;
+ bool enabled_ = false;
- bool last_enabled_ = false;
-
- ::aos::ShmEventLoop test_event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> robot_status_event_loop_;
::aos::Sender<::aos::RobotState> robot_state_sender_;
::aos::Sender<::aos::JoystickState> joystick_state_sender_;
+
+ ::aos::PhasedLoopHandler *send_robot_state_phased_loop_ = nullptr;
+ ::aos::TimerHandler *send_joystick_state_timer_ = nullptr;
};
typedef ControlLoopTestTemplated<::testing::Test> ControlLoopTest;
template <typename TestBaseClass>
-constexpr ::std::chrono::milliseconds ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
+constexpr ::std::chrono::microseconds
+ ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
template <typename TestBaseClass>
constexpr ::std::chrono::milliseconds ControlLoopTestTemplated<TestBaseClass>::kDSPacketTime;
diff --git a/aos/events/BUILD b/aos/events/BUILD
index aaa1c86..330ad40 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -64,7 +64,8 @@
srcs = ["event-loop_param_test.cc"],
hdrs = ["event-loop_param_test.h"],
deps = [
- "event-loop",
+ ":event-loop",
+ "//aos/logging:queue_logging",
"//aos/testing:googletest",
],
)
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index 1435723..4d6fda5 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -131,9 +131,6 @@
// TODO(austin): OnExit
- // Stops receiving events
- virtual void Exit() = 0;
-
// Sets the scheduler priority to run the event loop at. This may not be
// called after we go into "real-time-mode".
virtual void SetRuntimeRealtimePriority(int priority) = 0;
diff --git a/aos/events/event-loop_param_test.cc b/aos/events/event-loop_param_test.cc
index d6f41ab..1dcc258 100644
--- a/aos/events/event-loop_param_test.cc
+++ b/aos/events/event-loop_param_test.cc
@@ -5,6 +5,8 @@
#include "gmock/gmock.h"
#include "gtest/gtest.h"
+#include "aos/logging/queue_logging.h"
+
namespace aos {
namespace testing {
namespace {
@@ -24,13 +26,6 @@
TestMessage() { Zero(); }
};
-// Ends the given event loop at the given time from now.
-void EndEventLoop(EventLoop *loop, ::std::chrono::milliseconds duration) {
- auto end_timer = loop->AddTimer([loop]() { loop->Exit(); });
- end_timer->Setup(loop->monotonic_now() +
- ::std::chrono::milliseconds(duration));
-}
-
// Tests that watcher can receive messages from a sender.
// Also tests that OnRun() works.
TEST_P(AbstractEventLoopTest, Basic) {
@@ -51,7 +46,7 @@
loop2->MakeWatcher("/test", [&](const TestMessage &message) {
EXPECT_EQ(message.msg_value, 200);
- loop2->Exit();
+ this->Exit();
});
EXPECT_FALSE(happened);
@@ -94,7 +89,7 @@
loop2->MakeWatcher("/test", [&](const TestMessage &message) {
values.push_back(message.msg_value);
if (values.size() == 2) {
- loop2->Exit();
+ this->Exit();
}
});
@@ -149,7 +144,7 @@
});
// Add a timer to actually quit.
- auto test_timer = loop2->AddTimer([&loop2]() { loop2->Exit(); });
+ auto test_timer = loop2->AddTimer([this]() { this->Exit(); });
loop2->OnRun([&test_timer, &loop2]() {
test_timer->Setup(loop2->monotonic_now(), ::std::chrono::milliseconds(100));
});
@@ -180,11 +175,11 @@
}
// Add a timer to actually quit.
- auto test_timer = loop2->AddTimer([&loop2, &fetcher, &values]() {
+ auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
while (fetcher.FetchNext()) {
values.push_back(fetcher->msg_value);
}
- loop2->Exit();
+ this->Exit();
});
loop2->OnRun([&test_timer, &loop2]() {
@@ -218,11 +213,11 @@
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
// Add a timer to actually quit.
- auto test_timer = loop2->AddTimer([&loop2, &fetcher, &values]() {
+ auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
while (fetcher.FetchNext()) {
values.push_back(fetcher->msg_value);
}
- loop2->Exit();
+ this->Exit();
});
loop2->OnRun([&test_timer, &loop2]() {
@@ -257,7 +252,7 @@
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
// Add a timer to actually quit.
- auto test_timer = loop2->AddTimer([&loop2, &fetcher, &values]() {
+ auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
if (fetcher.Fetch()) {
values.push_back(fetcher->msg_value);
}
@@ -265,7 +260,7 @@
if (fetcher.Fetch()) {
values.push_back(fetcher->msg_value);
}
- loop2->Exit();
+ this->Exit();
});
loop2->OnRun([&test_timer, &loop2]() {
@@ -299,7 +294,7 @@
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
// Add a timer to actually quit.
- auto test_timer = loop2->AddTimer([&loop2, &fetcher, &values, &sender]() {
+ auto test_timer = loop2->AddTimer([&fetcher, &values, &sender, this]() {
if (fetcher.Fetch()) {
values.push_back(fetcher->msg_value);
}
@@ -328,7 +323,7 @@
values.push_back(fetcher->msg_value);
}
- loop2->Exit();
+ this->Exit();
});
loop2->OnRun([&test_timer, &loop2]() {
@@ -444,7 +439,7 @@
loop2->MakeWatcher("/test1", [&](const TestMessage &) {});
loop2->MakeWatcher("/test2", [&](const TestMessage &message) {
EXPECT_EQ(message.msg_value, 200);
- loop2->Exit();
+ this->Exit();
});
auto sender = loop1->MakeSender<TestMessage>("/test2");
@@ -566,12 +561,12 @@
// Run kCount iterations.
loop1->AddPhasedLoop(
- [×, &loop1](int count) {
+ [×, &loop1, this](int count) {
EXPECT_EQ(count, 1);
times.push_back(loop1->monotonic_now());
LOG(INFO, "%zu\n", times.size());
if (times.size() == kCount) {
- loop1->Exit();
+ this->Exit();
}
},
chrono::seconds(1), kOffset);
@@ -627,7 +622,7 @@
auto sender = loop1->MakeSender<TestMessage>("/test");
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
- auto test_timer = loop1->AddTimer([&sender, &fetcher, &loop1]() {
+ auto test_timer = loop1->AddTimer([&sender, &fetcher, this]() {
for (int i = 0; i < 100000; ++i) {
auto msg = sender.MakeMessage();
msg->msg_value = i;
@@ -643,7 +638,7 @@
++last;
}
- loop1->Exit();
+ this->Exit();
});
loop1->OnRun([&test_timer, &loop1]() {
diff --git a/aos/events/event-loop_param_test.h b/aos/events/event-loop_param_test.h
index 26d869d..83f0b37 100644
--- a/aos/events/event-loop_param_test.h
+++ b/aos/events/event-loop_param_test.h
@@ -22,6 +22,9 @@
// Runs the loops until they quit.
virtual void Run() = 0;
+ // Quits the loops.
+ virtual void Exit() = 0;
+
// Advances time by sleeping. Can't be called from inside a loop.
virtual void SleepFor(::std::chrono::nanoseconds duration) = 0;
};
@@ -36,9 +39,19 @@
void Run() { return factory_->Run(); }
+ void Exit() { return factory_->Exit(); }
+
void SleepFor(::std::chrono::nanoseconds duration) {
return factory_->SleepFor(duration);
}
+
+ // Ends the given event loop at the given time from now.
+ void EndEventLoop(EventLoop *loop, ::std::chrono::milliseconds duration) {
+ auto end_timer = loop->AddTimer([this]() { this->Exit(); });
+ end_timer->Setup(loop->monotonic_now() +
+ ::std::chrono::milliseconds(duration));
+ }
+
// You can implement all the usual fixture class members here.
// To access the test parameter, call GetParam() from class
// TestWithParam<T>.
diff --git a/aos/events/raw-event-loop.h b/aos/events/raw-event-loop.h
index 7ab2810..d221a6e 100644
--- a/aos/events/raw-event-loop.h
+++ b/aos/events/raw-event-loop.h
@@ -156,12 +156,6 @@
const monotonic_clock::duration interval,
const monotonic_clock::duration offset = ::std::chrono::seconds(0)) = 0;
- // Stops receiving events.
- virtual void Exit() = 0;
-
- // TODO(austin): This shouldn't belong here.
- virtual void Run() = 0;
-
protected:
friend class EventScheduler;
void set_is_running(bool value) { is_running_.store(value); }
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
index eead9f5..e44a54b 100644
--- a/aos/events/shm-event-loop.cc
+++ b/aos/events/shm-event-loop.cc
@@ -408,7 +408,10 @@
::aos::SetCurrentThreadName(thread_state_.name());
- // Now, all the threads are up. Go RT.
+ // Now, all the threads are up. Lock everything into memory and go RT.
+ if (thread_state_.priority_ != -1) {
+ ::aos::InitRT();
+ }
thread_state_.MaybeSetCurrentThreadRealtimePriority();
set_is_running(true);
diff --git a/aos/events/shm-event-loop.h b/aos/events/shm-event-loop.h
index 24cd909..5db8319 100644
--- a/aos/events/shm-event-loop.h
+++ b/aos/events/shm-event-loop.h
@@ -50,8 +50,8 @@
::std::chrono::seconds(0)) override;
void OnRun(::std::function<void()> on_run) override;
- void Run() override;
- void Exit() override;
+ void Run();
+ void Exit();
// TODO(austin): Add a function to register control-C call.
diff --git a/aos/events/shm-event-loop_test.cc b/aos/events/shm-event-loop_test.cc
index 8d0f552..8af00f0 100644
--- a/aos/events/shm-event-loop_test.cc
+++ b/aos/events/shm-event-loop_test.cc
@@ -24,6 +24,8 @@
void Run() override { CHECK_NOTNULL(primary_event_loop_)->Run(); }
+ void Exit() override { CHECK_NOTNULL(primary_event_loop_)->Exit(); }
+
void SleepFor(::std::chrono::nanoseconds duration) override {
::std::this_thread::sleep_for(duration);
}
@@ -80,10 +82,10 @@
bool did_timer = false;
bool did_watcher = false;
- auto timer = loop->AddTimer([&did_timer, &loop]() {
+ auto timer = loop->AddTimer([&did_timer, &loop, &factory]() {
EXPECT_TRUE(IsRealtime());
did_timer = true;
- loop->Exit();
+ factory.Exit();
});
loop->MakeWatcher("/test", [&did_watcher](const TestMessage &) {
@@ -118,7 +120,7 @@
constexpr chrono::milliseconds kOffset = chrono::milliseconds(400);
loop1->AddPhasedLoop(
- [×, &loop1, &kOffset](int count) {
+ [×, &loop1, &kOffset, &factory](int count) {
const ::aos::monotonic_clock::time_point monotonic_now =
loop1->monotonic_now();
@@ -143,7 +145,7 @@
times.push_back(loop1->monotonic_now());
if (times.size() == 2) {
- loop1->Exit();
+ factory.Exit();
}
// Now, add a large delay. This should push us up to 3 cycles.
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
index 71b0c79..a3edd9b 100644
--- a/aos/events/simulated-event-loop.cc
+++ b/aos/events/simulated-event-loop.cc
@@ -231,13 +231,6 @@
void OnRun(::std::function<void()> on_run) override {
scheduler_->Schedule(scheduler_->monotonic_now(), on_run);
}
- void Run() override {
- LOG(FATAL, "Run from the factory instead\n");
- scheduler_->Run();
- }
- void Exit() override {
- scheduler_->Exit();
- }
void set_name(const char *name) override { name_ = name; }
diff --git a/aos/events/simulated-event-loop.h b/aos/events/simulated-event-loop.h
index 81b181f..0828382 100644
--- a/aos/events/simulated-event-loop.h
+++ b/aos/events/simulated-event-loop.h
@@ -189,11 +189,17 @@
public:
::std::unique_ptr<EventLoop> MakeEventLoop();
+ // Starts executing the event loops unconditionally.
void Run() { scheduler_.Run(); }
+ // Executes the event loops for a duration.
void RunFor(monotonic_clock::duration duration) {
scheduler_.RunFor(duration);
}
+ // Stops executing all event loops. Meant to be called from within an event
+ // loop handler.
+ void Exit() { scheduler_.Exit(); }
+
monotonic_clock::time_point monotonic_now() const {
return scheduler_.monotonic_now();
}
diff --git a/aos/events/simulated-event-loop_test.cc b/aos/events/simulated-event-loop_test.cc
index be52243..e987a11 100644
--- a/aos/events/simulated-event-loop_test.cc
+++ b/aos/events/simulated-event-loop_test.cc
@@ -17,6 +17,7 @@
}
void Run() override { event_loop_factory_.Run(); }
+ void Exit() override { event_loop_factory_.Exit(); }
// TODO(austin): Implement this. It's used currently for a phased loop test.
// I'm not sure how much that matters.
diff --git a/aos/init.cc b/aos/init.cc
index aa609ec..8ce70ea 100644
--- a/aos/init.cc
+++ b/aos/init.cc
@@ -120,15 +120,19 @@
GoRT(relative_priority);
}
+void InitRT() {
+ LockAllMemory();
+
+ // Only let rt processes run for 3 seconds straight.
+ SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
+
+ // Allow rt processes up to priority 40.
+ SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
+}
+
void GoRT(int relative_priority) {
if (ShouldBeRealtime()) {
- LockAllMemory();
-
- // Only let rt processes run for 3 seconds straight.
- SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
-
- // Allow rt processes up to priority 40.
- SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
+ InitRT();
// Set our process to the appropriate priority.
struct sched_param param;
@@ -137,9 +141,10 @@
PDie("%s-init: setting SCHED_FIFO failed", program_invocation_short_name);
}
} else {
- fprintf(stderr, "%s not doing realtime initialization because environment"
- " variable %s is set\n", program_invocation_short_name,
- kNoRealtimeEnvironmentVariable);
+ fprintf(stderr,
+ "%s not doing realtime initialization because environment"
+ " variable %s is set\n",
+ program_invocation_short_name, kNoRealtimeEnvironmentVariable);
printf("no realtime for %s. see stderr\n", program_invocation_short_name);
}
diff --git a/aos/init.h b/aos/init.h
index 4489c5d..be99a24 100644
--- a/aos/init.h
+++ b/aos/init.h
@@ -23,6 +23,11 @@
// exit gracefully).
void Cleanup();
+// Locks everything into memory and sets the limits. This plus InitNRT are
+// everything you need to do before SetCurrentThreadRealtimePriority will make
+// your thread RT. Called as part of ShmEventLoop::Run()
+void InitRT();
+
// Performs the realtime parts of initialization after InitNRT(true) has been called.
void GoRT(int relative_priority = 0);
diff --git a/aos/input/joystick_input.cc b/aos/input/joystick_input.cc
index a208f1e..0bbba41 100644
--- a/aos/input/joystick_input.cc
+++ b/aos/input/joystick_input.cc
@@ -10,10 +10,6 @@
namespace aos {
namespace input {
-::std::atomic<bool> JoystickInput::run_;
-
-void JoystickInput::Quit(int /*signum*/) { run_ = false; }
-
void JoystickInput::HandleData(const ::aos::JoystickState &joystick_state) {
data_.Update(joystick_state);
@@ -63,27 +59,6 @@
}
RunIteration(data_);
-
- if (!run_) {
- event_loop_->Exit();
- }
-}
-
-void JoystickInput::Run() {
- // TODO(austin): We need a better sigint story for event loops in general.
- run_ = true;
- struct sigaction action;
- action.sa_handler = &JoystickInput::Quit;
- sigemptyset(&action.sa_mask);
- action.sa_flags = SA_RESETHAND;
-
- PCHECK(sigaction(SIGTERM, &action, nullptr));
- PCHECK(sigaction(SIGQUIT, &action, nullptr));
- PCHECK(sigaction(SIGINT, &action, nullptr));
-
- event_loop_->Run();
-
- LOG(INFO, "Shutting down\n");
}
} // namespace input
diff --git a/aos/input/joystick_input.h b/aos/input/joystick_input.h
index 98a89b1..ed72e78 100644
--- a/aos/input/joystick_input.h
+++ b/aos/input/joystick_input.h
@@ -23,10 +23,9 @@
[this](const ::aos::JoystickState &joystick_state) {
this->HandleData(joystick_state);
});
+ event_loop->SetRuntimeRealtimePriority(29);
}
- void Run();
-
protected:
int mode() const { return mode_; }
@@ -36,10 +35,6 @@
// Subclasses should do whatever they want with data here.
virtual void RunIteration(const driver_station::Data &data) = 0;
- static void Quit(int /*signum*/);
-
- static ::std::atomic<bool> run_;
-
EventLoop *event_loop_;
driver_station::Data data_;
diff --git a/aos/util/log_interval.h b/aos/util/log_interval.h
index 69170e4..acd32e8 100644
--- a/aos/util/log_interval.h
+++ b/aos/util/log_interval.h
@@ -28,6 +28,7 @@
void WantToLog() {
if (count_ == 0) {
+ // TODO(austin): event loops!
last_done_ = ::aos::monotonic_clock::now();
}
++count_;
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index 2dc8dea..91059f6 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -16,21 +16,36 @@
namespace codelab {
namespace testing {
+namespace chrono = ::std::chrono;
+using aos::monotonic_clock;
+
// Class which simulates stuff and sends out queue messages with the
// position.
class BasicSimulation {
public:
- BasicSimulation()
- : basic_queue_(".frc971.codelab.basic_queue",
- ".frc971.codelab.basic_queue.goal",
- ".frc971.codelab.basic_queue.position",
- ".frc971.codelab.basic_queue.output",
- ".frc971.codelab.basic_queue.status") {}
+ BasicSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ position_sender_(event_loop_->MakeSender<BasicQueue::Position>(
+ ".frc971.codelab.basic_queue.position")),
+ status_fetcher_(event_loop_->MakeFetcher<BasicQueue::Status>(
+ ".frc971.codelab.basic_queue.status")),
+ output_fetcher_(event_loop_->MakeFetcher<BasicQueue::Output>(
+ ".frc971.codelab.basic_queue.output")) {
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
// Sends a queue message with the position data.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<BasicQueue::Position> position =
- basic_queue_.position.MakeMessage();
+ auto position = position_sender_.MakeMessage();
position->limit_sensor = limit_sensor_;
@@ -38,68 +53,81 @@
}
void VerifyResults(double voltage, bool status) {
- basic_queue_.output.FetchLatest();
- basic_queue_.status.FetchLatest();
+ output_fetcher_.Fetch();
+ status_fetcher_.Fetch();
- ASSERT_TRUE(basic_queue_.output.get() != nullptr);
- ASSERT_TRUE(basic_queue_.status.get() != nullptr);
+ ASSERT_TRUE(output_fetcher_.get() != nullptr);
+ ASSERT_TRUE(status_fetcher_.get() != nullptr);
- EXPECT_EQ(basic_queue_.output->intake_voltage, voltage);
- EXPECT_EQ(basic_queue_.status->intake_complete, status);
+ EXPECT_EQ(output_fetcher_->intake_voltage, voltage);
+ EXPECT_EQ(status_fetcher_->intake_complete, status);
}
void set_limit_sensor(bool value) { limit_sensor_ = value; }
// Simulates basic control loop for a single timestep.
- void Simulate() { EXPECT_TRUE(basic_queue_.output.FetchLatest()); }
+ void Simulate() { EXPECT_TRUE(output_fetcher_.Fetch()); }
private:
- BasicQueue basic_queue_;
+ ::aos::EventLoop *event_loop_;
+
+ ::aos::Sender<BasicQueue::Position> position_sender_;
+ ::aos::Fetcher<BasicQueue::Status> status_fetcher_;
+ ::aos::Fetcher<BasicQueue::Output> output_fetcher_;
+
bool limit_sensor_ = false;
+
+ bool first_ = true;
};
class BasicControlLoopTest : public ::aos::testing::ControlLoopTest {
public:
BasicControlLoopTest()
- : basic_queue_(".frc971.codelab.basic_queue",
- ".frc971.codelab.basic_queue.goal",
- ".frc971.codelab.basic_queue.position",
- ".frc971.codelab.basic_queue.output",
- ".frc971.codelab.basic_queue.status"),
- basic_loop_(&event_loop_, ".frc971.codelab.basic_queue") {
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ goal_sender_(test_event_loop_->MakeSender<BasicQueue::Goal>(
+ ".frc971.codelab.basic_queue.goal")),
+
+ basic_event_loop_(MakeEventLoop()),
+ basic_(basic_event_loop_.get(), ".frc971.codelab.basic_queue"),
+
+ basic_simulation_event_loop_(MakeEventLoop()),
+ basic_simulation_(basic_simulation_event_loop_.get(), dt()) {
set_team_id(control_loops::testing::kTeamNumber);
}
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<BasicQueue::Goal> goal_sender_;
- basic_simulation_.SendPositionMessage();
- basic_loop_.Iterate();
- basic_simulation_.Simulate();
+ ::std::unique_ptr<::aos::EventLoop> basic_event_loop_;
+ Basic basic_;
- TickTime();
- }
-
- BasicQueue basic_queue_;
- ::aos::ShmEventLoop event_loop_;
- Basic basic_loop_;
+ ::std::unique_ptr<::aos::EventLoop> basic_simulation_event_loop_;
BasicSimulation basic_simulation_;
};
// Tests that when the motor has finished intaking it stops.
TEST_F(BasicControlLoopTest, IntakeLimitTransitionsToTrue) {
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = true;
+ ASSERT_TRUE(message.Send());
+ }
+
basic_simulation_.set_limit_sensor(false);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(12.0, false);
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = true;
+ ASSERT_TRUE(message.Send());
+ }
basic_simulation_.set_limit_sensor(true);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, true);
}
@@ -107,10 +135,14 @@
// Tests that the intake goes on if the sensor is not pressed
// and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitNotSet) {
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = true;
+ ASSERT_TRUE(message.Send());
+ }
basic_simulation_.set_limit_sensor(false);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(12.0, false);
}
@@ -118,10 +150,14 @@
// Tests that the intake is off if no intake is requested,
// even if the limit sensor is off.
TEST_F(BasicControlLoopTest, NoIntakeLimitNotSet) {
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = false;
+ ASSERT_TRUE(message.Send());
+ }
basic_simulation_.set_limit_sensor(false);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, false);
}
@@ -129,20 +165,28 @@
// Tests that the intake is off even if the limit sensor
// is pressed and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitSet) {
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = true;
+ ASSERT_TRUE(message.Send());
+ }
basic_simulation_.set_limit_sensor(true);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, true);
}
// Tests that the intake is off if no intake is requested,
TEST_F(BasicControlLoopTest, NoIntakeLimitSet) {
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(false).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = false;
+ ASSERT_TRUE(message.Send());
+ }
basic_simulation_.set_limit_sensor(true);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, false);
}
@@ -151,7 +195,7 @@
TEST_F(BasicControlLoopTest, NoGoalSet) {
basic_simulation_.set_limit_sensor(true);
- RunIteration();
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, false);
}
@@ -160,9 +204,14 @@
TEST_F(BasicControlLoopTest, Disabled) {
basic_simulation_.set_limit_sensor(true);
- ASSERT_TRUE(basic_queue_.goal.MakeWithBuilder().intake(true).Send());
+ {
+ auto message = goal_sender_.MakeMessage();
+ message->intake = true;
+ ASSERT_TRUE(message.Send());
+ }
- RunIteration(false);
+ SetEnabled(false);
+ RunFor(dt() * 2);
basic_simulation_.VerifyResults(0.0, false);
}
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 773b6cb..3d6c930 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -243,6 +243,17 @@
],
)
+queue_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.q",
+ ],
+ deps = [
+ ":profiled_subsystem_queue",
+ ":queues",
+ ],
+)
+
cc_library(
name = "profiled_subsystem",
srcs = [
@@ -407,6 +418,7 @@
":position_sensor_sim",
":static_zeroing_single_dof_profiled_subsystem",
":static_zeroing_single_dof_profiled_subsystem_test_plants",
+ ":static_zeroing_single_dof_profiled_subsystem_test_queue",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b45f016..b9d2bc4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -93,7 +93,8 @@
const ::frc971::control_loops::DrivetrainQueue::Position *position,
::frc971::control_loops::DrivetrainQueue::Output *output,
::frc971::control_loops::DrivetrainQueue::Status *status) {
- monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ const monotonic_clock::time_point monotonic_now =
+ event_loop()->monotonic_now();
if (!has_been_enabled_ && output) {
has_been_enabled_ = true;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 3f20554..e4de9bb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,9 +10,6 @@
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
-#if defined(SUPPORT_PLOT)
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-#endif
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -22,8 +19,6 @@
#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "frc971/queues/gyro.q.h"
-DEFINE_bool(plot, false, "If true, plot");
-
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -34,92 +29,59 @@
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- const DrivetrainConfig<double> dt_config_;
- DeadReckonEkf localizer_;
- // Create a loop and simulation plant.
- DrivetrainLoop drivetrain_motor_;
- ::aos::ShmEventLoop simulation_event_loop_;
- DrivetrainSimulation drivetrain_motor_plant_;
-
DrivetrainTest()
- : my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
+ : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+ test_event_loop_(MakeEventLoop()),
+ drivetrain_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_goal_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_status_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
+ drivetrain_output_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ localizer_control_sender_(
+ test_event_loop_->MakeSender<LocalizerControl>(
+ ".frc971.control_loops.drivetrain.localizer_control")),
+ drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTestDrivetrainConfig()),
- localizer_(dt_config_),
- drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
- drivetrain_motor_plant_(&simulation_event_loop_, dt_config_) {
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ drivetrain_plant_event_loop_(MakeEventLoop()),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_) {
set_battery_voltage(12.0);
}
+ virtual ~DrivetrainTest() {}
- void RunIteration() {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true, dt_config_.dt);
- if (FLAGS_plot) {
- my_drivetrain_queue_.status.FetchLatest();
-
- ::Eigen::Matrix<double, 2, 1> actual_position =
- drivetrain_motor_plant_.GetPosition();
- actual_x_.push_back(actual_position(0));
- actual_y_.push_back(actual_position(1));
-
- trajectory_x_.push_back(my_drivetrain_queue_.status->trajectory_logging.x);
- trajectory_y_.push_back(my_drivetrain_queue_.status->trajectory_logging.y);
- }
- }
-
- void RunForTime(monotonic_clock::duration run_for) {
- const auto end_time = monotonic_clock::now() + run_for;
- while (monotonic_clock::now() < end_time) {
- RunIteration();
- }
- }
-
- void TearDown() {
-#if defined(SUPPORT_PLOT)
- if (FLAGS_plot) {
- std::cout << "plotting.\n";
- matplotlibcpp::figure();
- matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
- matplotlibcpp::plot(trajectory_x_, trajectory_y_,
- {{"label", "trajectory position"}});
- matplotlibcpp::legend();
- matplotlibcpp::show();
- }
-#endif
- }
+ void TearDown() { drivetrain_plant_.MaybePlot(); }
void VerifyNearGoal() {
- my_drivetrain_queue_.goal.FetchLatest();
- my_drivetrain_queue_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
- drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
- drivetrain_motor_plant_.GetRightPosition(), 1e-3);
+ drivetrain_goal_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ drivetrain_plant_.GetLeftPosition(), 1e-3);
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ drivetrain_plant_.GetRightPosition(), 1e-3);
}
void VerifyNearPosition(double x, double y) {
- my_drivetrain_queue_.status.FetchLatest();
- auto actual = drivetrain_motor_plant_.GetPosition();
+ auto actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), x, 1e-2);
EXPECT_NEAR(actual(1), y, 1e-2);
}
void VerifyNearSplineGoal() {
- my_drivetrain_queue_.status.FetchLatest();
- double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
- double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
- auto actual = drivetrain_motor_plant_.GetPosition();
+ drivetrain_status_fetcher_.Fetch();
+ const double expected_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double expected_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), expected_x, 2e-2);
EXPECT_NEAR(actual(1), expected_y, 2e-2);
}
@@ -128,104 +90,117 @@
do {
// Run for fewer iterations while the worker thread computes.
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- } while (my_drivetrain_queue_.status->trajectory_logging.planning_state !=
- (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
+ RunFor(dt());
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+ (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
}
void WaitForTrajectoryExecution() {
do {
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- } while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
+ RunFor(dt());
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
}
- virtual ~DrivetrainTest() {}
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_output_fetcher_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
- private:
- ::std::vector<double> actual_x_;
- ::std::vector<double> actual_y_;
- ::std::vector<double> trajectory_x_;
- ::std::vector<double> trajectory_y_;
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
+ const DrivetrainConfig<double> dt_config_;
+ DeadReckonEkf localizer_;
+ // Create a loop and simulation plant.
+ DrivetrainLoop drivetrain_;
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
+ DrivetrainSimulation drivetrain_plant_;
};
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(2));
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(2));
VerifyNearGoal();
}
// Tests that the drivetrain converges on a goal when under the effect of a
// voltage offset/disturbance.
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- drivetrain_motor_plant_.set_left_voltage_offset(1.0);
- drivetrain_motor_plant_.set_right_voltage_offset(1.0);
- RunForTime(chrono::milliseconds(1500));
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ drivetrain_plant_.set_left_voltage_offset(1.0);
+ drivetrain_plant_.set_right_voltage_offset(1.0);
+ RunFor(chrono::milliseconds(1500));
VerifyNearGoal();
}
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
for (int i = 0; i < 500; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
if (i > 20 && i < 200) {
- SimulateTimestep(false, dt_config_.dt);
+ SetEnabled(false);
} else {
- SimulateTimestep(true, dt_config_.dt);
+ SetEnabled(true);
}
+ RunFor(dt());
}
VerifyNearGoal();
}
-// Tests that never having a goal doesn't break.
-TEST_F(DrivetrainTest, NoGoalStart) {
- for (int i = 0; i < 20; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- }
-}
-
// Tests that never having a goal, but having driver's station messages, doesn't
// break.
TEST_F(DrivetrainTest, NoGoalWithRobotState) {
- RunForTime(chrono::milliseconds(100));
+ SetEnabled(true);
+ RunFor(chrono::milliseconds(100));
}
// Tests that the robot successfully drives straight forward.
// This used to not work due to a U-capping bug.
TEST_F(DrivetrainTest, DriveStraightForward) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(4.0)
- .right_goal(4.0)
- .Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 4.0;
+ EXPECT_TRUE(message.Send());
+ }
+
for (int i = 0; i < 500; ++i) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
}
VerifyNearGoal();
}
@@ -233,16 +208,19 @@
// Tests that the robot successfully drives close to straight.
// This used to fail in simulation due to libcdd issues with U-capping.
TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(4.0)
- .right_goal(3.9)
- .Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 3.9;
+ EXPECT_TRUE(message.Send());
+ }
for (int i = 0; i < 500; ++i) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
}
VerifyNearGoal();
}
@@ -274,81 +252,78 @@
// Tests that a linear motion profile succeeds.
TEST_F(DrivetrainTest, ProfileStraightForward) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 4.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 3.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 3.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 3.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 3.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(6))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that an angular motion profile succeeds.
TEST_F(DrivetrainTest, ProfileTurn) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = -1.0;
- goal->right_goal = 1.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 3.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 3.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 3.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 3.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(6))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- -my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ -drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that a mixed turn drive saturated profile succeeds.
TEST_F(DrivetrainTest, SaturatedTurnDrive) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 5.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 6.0;
- goal->linear.max_acceleration = 4.0;
- goal->angular.max_velocity = 2.0;
- goal->angular.max_acceleration = 4.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 5.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 6.0;
+ message->linear.max_acceleration = 4.0;
+ message->angular.max_velocity = 2.0;
+ message->angular.max_acceleration = 4.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(3))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
}
VerifyNearGoal();
}
@@ -356,111 +331,121 @@
// Tests that being in teleop drive for a bit and then transitioning to closed
// drive profiles nicely.
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(1.0)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(1));
-
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(-0.3)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(1));
-
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(0.0)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(10));
-
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 5.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 2.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 2.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = 1.0;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
}
- const auto end_time = monotonic_clock::now() + chrono::seconds(4);
- while (monotonic_clock::now() < end_time) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true, dt_config_.dt);
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ RunFor(chrono::seconds(1));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = -0.3;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(chrono::seconds(1));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = 0.0;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(chrono::seconds(10));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 5.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 2.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 2.0;
+ EXPECT_TRUE(message.Send());
+ }
+
+ const auto end_time = monotonic_now() + chrono::seconds(4);
+ while (monotonic_now() < end_time) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that simple spline converges on a goal.
TEST_F(DrivetrainTest, SplineSimple) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ // Send the start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that we can drive a spline backwards.
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.drive_spline_backwards = true;
- goal->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
- goal->spline.spline_y = {{0.0, 0.0, -0.25 ,-0.75, -1.0, -1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.drive_spline_backwards = true;
+ message->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
+ message->spline.spline_y = {{0.0, 0.0, -0.25, -0.75, -1.0, -1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
// Check that we are right on the spline at the start (otherwise the feedback
// will tend to correct for going the wrong direction).
for (int ii = 0; ii < 10; ++ii) {
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
VerifyNearSplineGoal();
}
@@ -468,249 +453,282 @@
VerifyNearSplineGoal();
// Check that we are pointed the right direction:
- my_drivetrain_queue_.status.FetchLatest();
- auto actual = drivetrain_motor_plant_.state();
+ drivetrain_status_fetcher_.Fetch();
+ auto actual = drivetrain_plant_.state();
const double expected_theta =
- my_drivetrain_queue_.status->trajectory_logging.theta;
+ drivetrain_status_fetcher_->trajectory_logging.theta;
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta),
- 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
}
// Tests that simple spline with a single goal message.
TEST_F(DrivetrainTest, SplineSingleGoal) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a trajectory can be stopped in the middle.
TEST_F(DrivetrainTest, SplineStop) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(500));
- my_drivetrain_queue_.status.FetchLatest();
- const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
- const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+ RunFor(chrono::milliseconds(500));
+ drivetrain_status_fetcher_.Fetch();
+ const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Now stop.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped.
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
}
// Tests that a spline can't be restarted.
TEST_F(DrivetrainTest, SplineRestart) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(500));
- my_drivetrain_queue_.status.FetchLatest();
- const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
- const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+ RunFor(chrono::milliseconds(500));
+ drivetrain_status_fetcher_.Fetch();
+ const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(500));
+ // Send a stop goal.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(500));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> restart_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- restart_goal->controller_type = 2;
- restart_goal->spline_handle = 1;
- restart_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Send a restart.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped and restarted.
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
}
// Tests that simple spline converges when it doesn't start where it thinks.
TEST_F(DrivetrainTest, SplineOffset) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.2, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that simple spline converges when it starts to the side of where it
// thinks.
TEST_F(DrivetrainTest, SplineSideOffset) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.5, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a multispline converges on a goal.
TEST_F(DrivetrainTest, MultiSpline) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 2;
- goal->spline.spline_x = {
- {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- goal->spline.spline_y = {
- {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 2;
+ message->spline.spline_x = {
+ {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {
+ {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(4000));
+ RunFor(chrono::milliseconds(4000));
VerifyNearSplineGoal();
}
// Tests that several splines converges on a goal.
TEST_F(DrivetrainTest, SequentialSplines) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
VerifyNearSplineGoal();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal.Send();
- RunIteration();
+ // Second spline.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_start_goal->controller_type = 2;
- second_start_goal->spline_handle = 2;
- second_start_goal.Send();
+ // And then start it.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a second spline will run if the first is stopped.
TEST_F(DrivetrainTest, SplineStopFirst) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(1000));
+ RunFor(chrono::milliseconds(1000));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(500));
+ // Stop goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(500));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal->spline_handle = 2;
- second_spline_goal.Send();
+ // Second spline goal.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
@@ -720,37 +738,42 @@
// Tests that we can run a second spline after having planned but never executed
// the first spline.
TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- // Don't start running the splane.
- goal->spline_handle = 0;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ // Don't start running the splane.
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(1000));
+ RunFor(chrono::milliseconds(1000));
// Plan another spline, but don't start it yet:
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal->spline_handle = 0;
- second_spline_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- execute_goal = my_drivetrain_queue_.goal.MakeMessage();
- execute_goal->controller_type = 2;
- execute_goal->spline_handle = 2;
- execute_goal.Send();
+ // Now execute it.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryExecution();
VerifyNearSplineGoal();
@@ -759,77 +782,90 @@
// Tests that splines can excecute and plan at the same time.
TEST_F(DrivetrainTest, ParallelSplines) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->spline_handle = 1;
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal.Send();
+ // Second spline goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->spline_handle = 1;
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryExecution();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_start_goal->controller_type = 2;
- second_start_goal->spline_handle = 2;
- second_start_goal.Send();
+ // Second start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::milliseconds(4000));
+ RunFor(chrono::milliseconds(4000));
VerifyNearSplineGoal();
}
-//Tests that a trajectory never told to execute will not replan.
+// Tests that a trajectory never told to execute will not replan.
TEST_F(DrivetrainTest, OnlyPlanSpline) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
for (int i = 0; i < 100; ++i) {
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_EQ(my_drivetrain_queue_.status->trajectory_logging.planning_state, 3);
+ RunFor(dt());
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+ 3);
::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
}
VerifyNearSplineGoal();
}
-//Tests that a trajectory can be executed after it is planned.
+// Tests that a trajectory can be executed after it is planned.
TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
VerifyNearPosition(1.0, 1.0);
}
@@ -837,58 +873,66 @@
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
+ SetEnabled(true);
localizer_.target_selector()->set_has_target(true);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 3;
- goal->throttle = 0.5;
- goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 3;
+ message->throttle = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.frozen);
- EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.have_target);
- EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.x);
- EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.y);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.frozen);
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.have_target);
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.x);
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.y);
EXPECT_FLOAT_EQ(M_PI_4,
- my_drivetrain_queue_.status->line_follow_logging.theta);
+ drivetrain_status_fetcher_->line_follow_logging.theta);
// Should have run off the end of the target, running along the y=x line.
- EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
- EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
- drivetrain_motor_plant_.GetPosition().y(), 0.1);
+ EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+ EXPECT_NEAR(drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.1);
}
// Tests that the line follower will not run and defer to regular open-loop
// driving when there is no target yet:
TEST_F(DrivetrainTest, LineFollowDefersToOpenLoop) {
+ SetEnabled(true);
localizer_.target_selector()->set_has_target(false);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 3;
- goal->throttle = 0.5;
- goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 3;
+ message->throttle = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
// Should have run straight (because we just set throttle, with wheel = 0)
// along X-axis.
- EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
- EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+ EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+ EXPECT_NEAR(0.0, drivetrain_plant_.GetPosition().y(), 1e-4);
}
// Tests that we can reset the localizer to a new position.
TEST_F(DrivetrainTest, ResetLocalizer) {
+ SetEnabled(true);
EXPECT_EQ(0.0, localizer_.x());
EXPECT_EQ(0.0, localizer_.y());
EXPECT_EQ(0.0, localizer_.theta());
- ::aos::Queue<LocalizerControl> localizer_queue(
- ".frc971.control_loops.drivetrain.localizer_control");
- ASSERT_TRUE(
- localizer_queue.MakeWithBuilder().x(9.0).y(7.0).theta(1.0).Send());
- RunIteration();
+ {
+ auto message = localizer_control_sender_.MakeMessage();
+ message->x = 9.0;
+ message->y = 7.0;
+ message->theta = 1.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(dt());
EXPECT_EQ(9.0, localizer_.x());
EXPECT_EQ(7.0, localizer_.y());
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index a5cf6af..34c9907 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -6,12 +6,18 @@
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "gflags/gflags.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+DEFINE_bool(plot, false, "If true, plot");
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -86,16 +92,23 @@
: event_loop_(event_loop),
robot_state_fetcher_(
event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
- dt_config_(dt_config),
- drivetrain_plant_(MakePlantFromConfig(dt_config_)),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
+ drivetrain_position_sender_(
+ event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")),
+ drivetrain_output_fetcher_(
+ event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ drivetrain_status_fetcher_(
+ event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
gyro_reading_sender_(
event_loop->MakeSender<::frc971::sensors::GyroReading>(
".frc971.sensors.gyro_reading")),
+ dt_config_(dt_config),
+ drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
StateFeedbackHybridPlant<2, 2, 2>,
@@ -106,6 +119,29 @@
dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
Reinitialize();
last_U_.setZero();
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ if (FLAGS_plot) {
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+
+ ::Eigen::Matrix<double, 2, 1> actual_position = GetPosition();
+ actual_x_.push_back(actual_position(0));
+ actual_y_.push_back(actual_position(1));
+
+ trajectory_x_.push_back(
+ drivetrain_status_fetcher_->trajectory_logging.x);
+ trajectory_y_.push_back(
+ drivetrain_status_fetcher_->trajectory_logging.y);
+ }
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt_config_.dt);
}
void DrivetrainSimulation::Reinitialize() {
@@ -123,8 +159,8 @@
const double right_encoder = GetRightPosition();
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
- position = my_drivetrain_queue_.position.MakeMessage();
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>::Message
+ position = drivetrain_position_sender_.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
@@ -146,10 +182,10 @@
void DrivetrainSimulation::Simulate() {
last_left_position_ = drivetrain_plant_.Y(0, 0);
last_right_position_ = drivetrain_plant_.Y(1, 0);
- EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage;
+ last_U_ << drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage;
{
robot_state_fetcher_.Fetch();
const double scalar = robot_state_fetcher_.get()
@@ -157,8 +193,8 @@
: 1.0;
last_U_ *= scalar;
}
- left_gear_high_ = my_drivetrain_queue_.output->left_high;
- right_gear_high_ = my_drivetrain_queue_.output->right_high;
+ left_gear_high_ = drivetrain_output_fetcher_->left_high;
+ right_gear_high_ = drivetrain_output_fetcher_->right_high;
if (left_gear_high_) {
if (right_gear_high_) {
@@ -187,6 +223,20 @@
state_, U, dt_float);
}
+void DrivetrainSimulation::MaybePlot() {
+#if defined(SUPPORT_PLOT)
+ if (FLAGS_plot) {
+ std::cout << "Plotting." << ::std::endl;
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
+ matplotlibcpp::plot(trajectory_x_, trajectory_y_,
+ {{"label", "trajectory position"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+ }
+#endif
+}
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index cdde176..9a4f177 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -52,12 +52,6 @@
double GetLeftPosition() const { return drivetrain_plant_.Y(0, 0); }
double GetRightPosition() const { return drivetrain_plant_.Y(1, 0); }
- // Sends out the position queue messages.
- void SendPositionMessage();
-
- // Simulates the drivetrain moving for one timestep.
- void Simulate();
-
void set_left_voltage_offset(double left_voltage_offset) {
drivetrain_plant_.set_left_voltage_offset(left_voltage_offset);
}
@@ -73,17 +67,30 @@
return state_.block<2,1>(0,0);
}
+ void MaybePlot();
+
private:
+ // Sends out the position queue messages.
+ void SendPositionMessage();
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate();
+
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_output_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
+
DrivetrainConfig<double> dt_config_;
DrivetrainPlant drivetrain_plant_;
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
- ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
-
// This state is [x, y, theta, left_velocity, right_velocity].
::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
::std::unique_ptr<
@@ -96,6 +103,12 @@
bool left_gear_high_ = false;
bool right_gear_high_ = false;
+ bool first_ = true;
+
+ ::std::vector<double> actual_x_;
+ ::std::vector<double> actual_y_;
+ ::std::vector<double> trajectory_x_;
+ ::std::vector<double> trajectory_y_;
};
} // namespace testing
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 68f67f6..16b0b61 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -488,6 +488,9 @@
// tuning things. I haven't yet tried messing with these values again.
encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
+
+ X_hat_.setZero();
+ P_.setZero();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 7d401d6..f4d718e 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -9,7 +9,7 @@
#include "gtest/gtest.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-DEFINE_bool(plot, false, "If true, plot");
+DECLARE_bool(plot);
namespace chrono = ::std::chrono;
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 23a978c..7b2ceae 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/pose.h"
@@ -94,18 +95,22 @@
class DeadReckonEkf : public LocalizerInterface {
typedef HybridEkf<double> Ekf;
typedef typename Ekf::StateIdx StateIdx;
+
public:
- DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
- ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
- ekf_.P());
+ DeadReckonEkf(::aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &dt_config)
+ : ekf_(dt_config) {
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+ ekf_.P());
+ });
target_selector_.set_has_target(false);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
- ::aos::monotonic_clock::time_point now,
- double left_encoder, double right_encoder,
- double gyro_rate,
- double /*longitudinal_accelerometer*/) override {
+ ::aos::monotonic_clock::time_point now, double left_encoder,
+ double right_encoder, double gyro_rate,
+ double /*longitudinal_accelerometer*/) override {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
}
@@ -115,7 +120,8 @@
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
- right_encoder, 0, 0, 0, 0).finished(),
+ right_encoder, 0, 0, 0,
+ 0).finished(),
ekf_.P());
};
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 1b6b965..ac23907 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -195,4 +195,4 @@
struct StaticZeroingSingleDOFProfiledSubsystemGoal {
double unsafe_goal;
.frc971.ProfileParameters profile_params;
-};
\ No newline at end of file
+};
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 de9c22b..58096f3 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
@@ -1,9 +1,11 @@
#include "gtest/gtest.h"
+#include "aos/controls/control_loop.h"
#include "aos/controls/control_loop_test.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
@@ -25,7 +27,14 @@
zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
-typedef ::testing::Types<SZSDPS_AbsEncoder, SZSDPS_PotAndAbsEncoder> TestTypes;
+typedef ::testing::Types<
+ ::std::pair<
+ SZSDPS_AbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
+ ::std::pair<
+ SZSDPS_PotAndAbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+ TestTypes;
constexpr ::frc971::constants::Range kRange{
.lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
@@ -69,170 +78,78 @@
return params;
}
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
-struct TestIntakeSystemData {
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
-
- ProfiledJointStatus status;
-
- typename ZeroingEstimator::Position position;
-
- double output;
-};
-
} // namespace
-template <typename SZSDPS>
+template <typename SZSDPS, typename QueueGroup>
class TestIntakeSystemSimulation {
public:
- TestIntakeSystemSimulation()
- : subsystem_plant_(new CappedTestPlant(
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
+ chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ subsystem_position_sender_(
+ event_loop_->MakeSender<PositionType>(".position")),
+ subsystem_status_fetcher_(
+ event_loop_->MakeFetcher<StatusType>(".status")),
+ subsystem_output_fetcher_(
+ event_loop_->MakeFetcher<OutputType>(".output")),
+ subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
subsystem_sensor_sim_(kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ this->Simulate();
+ }
+ first_ = false;
+ this->SendPositionMessage();
+ },
+ dt);
}
void InitializeSubsystemPosition(double start_pos) {
- subsystem_plant_->mutable_X(0, 0) = start_pos;
- subsystem_plant_->mutable_X(1, 0) = 0.0;
+ this->subsystem_plant_->mutable_X(0, 0) = start_pos;
+ this->subsystem_plant_->mutable_X(1, 0) = 0.0;
- InitializeSensorSim(start_pos);
+ this->InitializeSensorSim(start_pos);
}
void InitializeSensorSim(double start_pos);
- // Updates the position message with the position of the subsystem.
- void UpdatePositionMessage(
- typename SZSDPS::ZeroingEstimator::Position *position) {
- subsystem_sensor_sim_.GetSensorValues(position);
- }
-
- double subsystem_position() const { return subsystem_plant_->X(0, 0); }
- double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
+ double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_subsystem_voltage_offset(double voltage_offset) {
- subsystem_plant_->set_voltage_offset(voltage_offset);
+ this->subsystem_plant_->set_voltage_offset(voltage_offset);
}
- // Simulates the subsystem for a single timestep.
- void Simulate(const double output_voltage, const int32_t state) {
- const double voltage_check_subsystem =
- (static_cast<typename SZSDPS::State>(state) == SZSDPS::State::RUNNING)
- ? kOperatingVoltage
- : kZeroingVoltage;
+ // Sends a queue message with the position.
+ void SendPositionMessage() {
+ typename ::aos::Sender<PositionType>::Message position =
+ subsystem_position_sender_.MakeMessage();
- EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
+ this->subsystem_sensor_sim_.GetSensorValues(&position->position);
- ::Eigen::Matrix<double, 1, 1> subsystem_U;
- subsystem_U << output_voltage + subsystem_plant_->voltage_offset();
- subsystem_plant_->Update(subsystem_U);
-
- const double position_subsystem = subsystem_plant_->Y(0, 0);
-
- subsystem_sensor_sim_.MoveTo(position_subsystem);
-
- EXPECT_GE(position_subsystem, kRange.lower_hard);
- EXPECT_LE(position_subsystem, kRange.upper_hard);
- }
-
- private:
- ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
- PositionSensorSimulator subsystem_sensor_sim_;
-};
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_PotAndAbsEncoder>::InitializeSensorSim(
- double start_pos) {
- subsystem_sensor_sim_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- TestIntakeSystemValues<
- typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
- .measured_absolute_position);
-}
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_AbsEncoder>::InitializeSensorSim(
- double start_pos) {
- subsystem_sensor_sim_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- TestIntakeSystemValues<
- typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
- .measured_absolute_position);
-}
-
-template <typename TSZSDPS>
-class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
- protected:
- using SZSDPS = TSZSDPS;
- using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
- using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
-
- IntakeSystemTest()
- : robot_state_fetcher_(
- simulation_event_loop_.MakeFetcher<::aos::RobotState>(
- ".aos.robot_state")),
- subsystem_(TestIntakeSystemValues<
- typename SZSDPS::ZeroingEstimator>::make_params()),
- subsystem_plant_() {}
-
- void VerifyNearGoal() {
- EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
- subsystem_data_.status.position, 0.001);
- EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
- subsystem_plant_.subsystem_position(), 0.001);
- EXPECT_NEAR(subsystem_data_.status.velocity, 0, 0.001);
- }
-
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true, bool null_goal = false) {
- SendMessages(enabled);
- subsystem_plant_.UpdatePositionMessage(&subsystem_data_.position);
-
- // Checks if the robot was reset and resets the subsystem.
- // Required since there is no ControlLoop to reset it (ie. a superstructure)
- robot_state_fetcher_.Fetch();
- if (robot_state_fetcher_.get()) {
- if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
- LOG(ERROR, "WPILib reset, restarting\n");
- subsystem_.Reset();
- }
- sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
- }
- subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
- subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
-
- subsystem_.Iterate(null_goal ? nullptr : &subsystem_goal_,
- &subsystem_data_.position, &subsystem_data_.output,
- &subsystem_data_.status);
-
- subsystem_plant_.Simulate(subsystem_data_.output,
- subsystem_data_.status.state);
-
- TickTime(::std::chrono::microseconds(5050));
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
- bool null_goal = false) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- const auto loop_start_time = monotonic_clock::now();
- double begin_subsystem_velocity = subsystem_plant_.subsystem_velocity();
-
- RunIteration(enabled, null_goal);
-
- 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;
- EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
- EXPECT_NEAR(subsystem_plant_.subsystem_velocity(), 0.0,
- peak_subsystem_velocity_);
- }
+ position.Send();
}
void set_peak_subsystem_acceleration(double value) {
@@ -242,53 +159,233 @@
peak_subsystem_velocity_ = value;
}
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory
- // that is no longer valid.
- // TestIntakeSystemData subsystem_data_;
- ::aos::ShmEventLoop simulation_event_loop_;
- ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ // Simulates the subsystem for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(subsystem_output_fetcher_.Fetch());
+ EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
- // Create a control loop and simulation.
- SZSDPS subsystem_;
- TestIntakeSystemSimulation<SZSDPS> subsystem_plant_;
+ const double begin_subsystem_velocity = subsystem_velocity();
- StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
+ const double voltage_check_subsystem =
+ (static_cast<typename SZSDPS::State>(
+ subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+ ? kOperatingVoltage
+ : kZeroingVoltage;
- TestIntakeSystemData<typename SZSDPS::ZeroingEstimator,
- typename SZSDPS::ProfiledJointStatus>
- subsystem_data_;
+ EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+ voltage_check_subsystem);
+
+ ::Eigen::Matrix<double, 1, 1> subsystem_U;
+ subsystem_U << subsystem_output_fetcher_->output +
+ subsystem_plant_->voltage_offset();
+ subsystem_plant_->Update(subsystem_U);
+
+ const double position_subsystem = subsystem_plant_->Y(0, 0);
+
+ subsystem_sensor_sim_.MoveTo(position_subsystem);
+
+ EXPECT_GE(position_subsystem, kRange.lower_hard);
+ EXPECT_LE(position_subsystem, kRange.upper_hard);
+
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+ const double subsystem_acceleration =
+ (subsystem_velocity() - begin_subsystem_velocity) / loop_time;
+ EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
+ EXPECT_NEAR(subsystem_velocity(), 0.0, peak_subsystem_velocity_);
+ }
private:
+ ::aos::EventLoop *event_loop_;
+ chrono::nanoseconds dt_;
+
+ bool first_ = true;
+
+ typename ::aos::Sender<PositionType> subsystem_position_sender_;
+ typename ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+ typename ::aos::Fetcher<OutputType> subsystem_output_fetcher_;
+
+ ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
+ PositionSensorSimulator subsystem_sensor_sim_;
+
// The acceleration limits to check for while moving.
double peak_subsystem_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_subsystem_velocity_ = 1e10;
+};
- int32_t sensor_reader_pid_ = 0;
+template <>
+void TestIntakeSystemSimulation<
+ SZSDPS_PotAndAbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
+ InitializeSensorSim(double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <>
+void TestIntakeSystemSimulation<
+ SZSDPS_AbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+ InitializeSensorSim(double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+// Class to represent a module using a subsystem. This lets us use event loops
+// to wrap it.
+template <typename QueueGroup, typename SZSDPS>
+class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+ public:
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+ subsystem_(TestIntakeSystemValues<
+ typename SZSDPS::ZeroingEstimator>::make_params()) {}
+
+ void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
+ OutputType *output, StatusType *status) {
+ if (this->WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ subsystem_.Reset();
+ }
+
+ // Convert one goal type to another...
+ StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+ if (unsafe_goal != nullptr ) {
+ goal.unsafe_goal = unsafe_goal->unsafe_goal;
+ goal.profile_params.max_velocity =
+ unsafe_goal->profile_params.max_velocity;
+ goal.profile_params.max_acceleration =
+ unsafe_goal->profile_params.max_acceleration;
+ }
+
+ subsystem_.Iterate(
+ unsafe_goal == nullptr ? nullptr : &goal, &position->position,
+ output == nullptr ? nullptr : &output->output, &status->status);
+ }
+
+ SZSDPS *subsystem() { return &subsystem_; }
+
+ private:
+ SZSDPS subsystem_;
+};
+
+template <typename TSZSDPS>
+class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ using SZSDPS = typename TSZSDPS::first_type;
+ using QueueGroup = typename TSZSDPS::second_type;
+ using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
+ using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
+
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ IntakeSystemTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+ subsystem_goal_fetcher_(
+ test_event_loop_->MakeFetcher<GoalType>(".goal")),
+ subsystem_status_fetcher_(
+ test_event_loop_->MakeFetcher<StatusType>(".status")),
+ subsystem_event_loop_(MakeEventLoop()),
+ subsystem_(subsystem_event_loop_.get(), ""),
+ subsystem_plant_event_loop_(MakeEventLoop()),
+ subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
+
+ void VerifyNearGoal() {
+ subsystem_goal_fetcher_.Fetch();
+ EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ subsystem_status_fetcher_->status.position, 0.001);
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+ }
+
+ SZSDPS *subsystem() { return subsystem_.subsystem(); }
+
+ void set_peak_subsystem_acceleration(double value) {
+ set_peak_subsystem_acceleration(value);
+ }
+ void set_peak_subsystem_velocity(double value) {
+ set_peak_subsystem_velocity(value);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<GoalType> subsystem_goal_sender_;
+ ::aos::Fetcher<GoalType> subsystem_goal_fetcher_;
+ ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> subsystem_event_loop_;
+ Subsystem<QueueGroup, SZSDPS> subsystem_;
+
+ ::std::unique_ptr<::aos::EventLoop> subsystem_plant_event_loop_;
+ TestIntakeSystemSimulation<SZSDPS, QueueGroup> subsystem_plant_;
};
TYPED_TEST_CASE_P(IntakeSystemTest);
// Tests that the subsystem does nothing when the goal is zero.
TYPED_TEST_P(IntakeSystemTest, DoesNothing) {
+ this->SetEnabled(true);
// Intake system uses 0.05 to test for 0.
- this->subsystem_data_.goal.unsafe_goal = 0.05;
- this->RunForTime(chrono::seconds(5));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 0.05;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(5));
this->VerifyNearGoal();
}
// Tests that the subsystem loop can reach a goal.
TYPED_TEST_P(IntakeSystemTest, ReachesGoal) {
+ this->SetEnabled(true);
// Set a reasonable goal.
- auto &goal = this->subsystem_data_.goal;
- goal.unsafe_goal = 0.1;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 0.1;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
// Give it a lot of time to get there.
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
}
@@ -296,35 +393,43 @@
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
+ this->SetEnabled(true);
// Zero it before we move.
- auto &goal = this->subsystem_data_.goal;
- goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(8));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- goal.unsafe_goal = kRange.lower;
- goal.profile_params.max_velocity = 20.0;
- goal.profile_params.max_acceleration = 0.1;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower;
+ message->profile_params.max_velocity = 20.0;
+ message->profile_params.max_acceleration = 0.1;
+ EXPECT_TRUE(message.Send());
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- goal.unsafe_goal = kRange.upper;
- goal.profile_params.max_velocity = 0.1;
- goal.profile_params.max_acceleration = 100;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ message->profile_params.max_velocity = 0.1;
+ message->profile_params.max_acceleration = 100;
+ EXPECT_TRUE(message.Send());
}
this->set_peak_subsystem_velocity(0.2);
this->set_peak_subsystem_acceleration(103);
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
}
@@ -332,184 +437,244 @@
// Tests that the subsystem loop doesn't try and go beyond it's physical range
// of the mechanisms.
TYPED_TEST_P(IntakeSystemTest, RespectsRange) {
- auto &goal = this->subsystem_data_.goal;
+ this->SetEnabled(true);
+
// Set some ridiculous goals to test upper limits.
{
- goal.unsafe_goal = 100.0;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 100.0;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(kRange.upper, this->subsystem_data_.status.position, 0.001);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set some ridiculous goals to test lower limits.
{
- goal.unsafe_goal = -100.0;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = -100.0;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(kRange.lower, this->subsystem_data_.status.position, 0.001);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that the subsystem loop zeroes when run for a while.
TYPED_TEST_P(IntakeSystemTest, ZeroTest) {
- auto &goal = this->subsystem_data_.goal;
+ this->SetEnabled(true);
+
{
- goal.unsafe_goal = kRange.upper;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TYPED_TEST_P(IntakeSystemTest, ZeroNoGoal) {
- this->RunForTime(chrono::seconds(5));
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(5));
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
}
TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+ this->SetEnabled(true);
this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(10));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+ this->SetEnabled(true);
+
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(10));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TYPED_TEST_P(IntakeSystemTest, ResetTest) {
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper - 0.1;
- this->RunForTime(chrono::seconds(10));
+ this->SetEnabled(true);
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper - 0.1;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
+
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
this->VerifyNearGoal();
this->SimulateSensorReset();
- this->RunForTime(chrono::milliseconds(100));
+ this->RunFor(chrono::milliseconds(100));
EXPECT_EQ(TestFixture::SZSDPS::State::UNINITIALIZED,
- this->subsystem_.state());
+ this->subsystem()->state());
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
this->VerifyNearGoal();
}
// Tests that the internal goals don't change while disabled.
TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.03;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower + 0.03;
+ EXPECT_TRUE(message.Send());
+ }
// Checks that the subsystem has not moved from its starting position at 0
- this->RunForTime(chrono::milliseconds(100), false);
- EXPECT_EQ(0.0, this->subsystem_.goal(0));
+ this->RunFor(chrono::milliseconds(100));
+ EXPECT_EQ(0.0, this->subsystem()->goal(0));
// Now make sure they move correctly
- this->RunForTime(chrono::seconds(4), true);
- EXPECT_NE(0.0, this->subsystem_.goal(0));
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(4));
+ EXPECT_NE(0.0, this->subsystem()->goal(0));
}
// Tests that zeroing while disabled works.
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower;
+ EXPECT_TRUE(message.Send());
+ }
// Run disabled for 2 seconds
- this->RunForTime(chrono::seconds(2), false);
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
- this->RunForTime(chrono::seconds(4), true);
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(4));
this->VerifyNearGoal();
}
// Tests that set_min_position limits range properly
TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower_hard;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower_hard;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(2));
// Check that kRange.lower is used as the default min position
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set min position and check that the subsystem increases to that position
- this->subsystem_.set_min_position(kRange.lower + 0.05);
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower + 0.05);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower + 0.05,
+ this->subsystem()->set_min_position(kRange.lower + 0.05);
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
0.001);
// Clear min position and check that the subsystem returns to kRange.lower
- this->subsystem_.clear_min_position();
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+ this->subsystem()->clear_min_position();
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that set_max_position limits range properly
TYPED_TEST_P(IntakeSystemTest, MaxPositionTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.upper_hard;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper_hard;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(2));
// Check that kRange.upper is used as the default max position
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set max position and check that the subsystem lowers to that position
- this->subsystem_.set_max_position(kRange.upper - 0.05);
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper - 0.05);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper - 0.05,
+ this->subsystem()->set_max_position(kRange.upper - 0.05);
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
0.001);
// Clear max position and check that the subsystem returns to kRange.upper
- this->subsystem_.clear_max_position();
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+ this->subsystem()->clear_max_position();
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that the subsystem maintains its current position when sent a null goal
TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.05;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
- this->VerifyNearGoal();
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
- // Run with a null goal
- this->RunForTime(chrono::seconds(2), true, true);
+ this->RunFor(chrono::seconds(5));
- // Check that the subsystem has not moved
- this->VerifyNearGoal();
+ EXPECT_NEAR(kRange.upper, this->subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(this->subsystem_plant_.subsystem_velocity(), 0, 0.001);
}
// Tests that the subsystem estops when a zeroing error occurs
TYPED_TEST_P(IntakeSystemTest, ZeroingErrorTest) {
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::RUNNING);
- this->subsystem_.TriggerEstimatorError();
- this->RunIteration(true, false);
- EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::ESTOP);
+ EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::RUNNING);
+ this->subsystem()->TriggerEstimatorError();
+ this->RunFor(this->dt());
+ EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::ESTOP);
}
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
new file mode 100644
index 0000000..3b35837
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
@@ -0,0 +1,46 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/profiled_subsystem.q";
+
+message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
+ double unsafe_goal;
+ .frc971.ProfileParameters profile_params;
+};
+
+message StaticZeroingSingleDOFProfiledSubsystemOutput {
+ double output;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Status {
+ PotAndAbsoluteEncoderProfiledJointStatus status;
+ };
+
+ message Position {
+ PotAndAbsolutePosition position;
+ };
+
+ queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+ queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Status {
+ AbsoluteEncoderProfiledJointStatus status;
+ };
+
+ message Position {
+ AbsolutePosition position;
+ };
+
+ queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+ queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+ queue Status status;
+ queue Position position;
+};
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 75d0b56..41269eb 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -157,9 +157,9 @@
// position is null if the position data is stale, output_enabled is true if
// the output will actually go to the motors, and goal_angle and goal_velocity
// are the goal position and velocities.
- double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
- bool output_enabled,
- double goal_angle, double goal_velocity);
+ double Update(const ::aos::monotonic_clock::time_point monotonic_now,
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled, double goal_angle, double goal_velocity);
// True if the code is zeroing.
bool is_zeroing() const { return state_ == ZEROING; }
@@ -270,9 +270,9 @@
// Updates the zeroed joint controller and state machine.
template <int kNumZeroSensors>
double ZeroedJoint<kNumZeroSensors>::Update(
+ const ::aos::monotonic_clock::time_point monotonic_now,
const ZeroedJoint<kNumZeroSensors>::PositionData *position,
- bool output_enabled,
- double goal_angle, double goal_velocity) {
+ bool output_enabled, double goal_angle, double goal_velocity) {
// Uninitialize the bot if too many cycles pass without an encoder.
if (position == NULL) {
LOG(WARNING, "no new pos given\n");
@@ -282,7 +282,7 @@
output_enabled = false;
LOG(WARNING, "err_count is %d so disabling\n", error_count_);
- if (::aos::monotonic_clock::now() > kRezeroTime + last_good_time_) {
+ if (monotonic_now > kRezeroTime + last_good_time_) {
LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
error_count_);
state_ = UNINITIALIZED;
@@ -290,7 +290,7 @@
}
}
if (position != NULL) {
- last_good_time_ = ::aos::monotonic_clock::now();
+ last_good_time_ = monotonic_now;
error_count_ = 0;
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 3c95408..81baa68 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -19,8 +19,7 @@
// long efficiently.
//
// The intended use is to have a subclass for each loop which implements the
-// pure virtual methods and is then run in a separate thread. The operator()
-// loops writing values until Quit() is called.
+// pure virtual methods.
template <typename T>
class LoopOutputHandler {
public:
@@ -44,8 +43,6 @@
});
}
- void Quit() { event_loop_->Exit(); }
-
// Note, all subclasses should call Stop.
virtual ~LoopOutputHandler() {}
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index 2cccf65..ef3ec31 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -49,7 +49,7 @@
class WPILibAdapterRobot : public frc::RobotBase {
public:
void StartCompetition() override {
- ::aos::InitNRT();
+ ::aos::InitNRT(true);
robot_.Run();
}
diff --git a/y2012/control_loops/accessories/accessories.cc b/y2012/control_loops/accessories/accessories.cc
index d320468..9d96840 100644
--- a/y2012/control_loops/accessories/accessories.cc
+++ b/y2012/control_loops/accessories/accessories.cc
@@ -32,9 +32,12 @@
} // namespace y2012
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2012::control_loops::accessories::AccessoriesLoop accessories(&event_loop);
- accessories.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
index 6e5b845..75153cd 100644
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,17 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
+ &event_loop, ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
&localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 86c7a90..8f35b92 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -143,9 +143,12 @@
} // namespace y2012
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2012::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 6545dcb..c88daa8 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -34,15 +34,27 @@
public:
// Constructs a motor simulation. initial_position is the inital angle of the
// wrist, which will be treated as 0 by the encoder.
- ClawMotorSimulation(double initial_top_position,
+ ClawMotorSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
+ double initial_top_position,
double initial_bottom_position)
- : claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())),
- claw_queue(".y2014.control_loops.claw_queue",
- ".y2014.control_loops.claw_queue.goal",
- ".y2014.control_loops.claw_queue.position",
- ".y2014.control_loops.claw_queue.output",
- ".y2014.control_loops.claw_queue.status") {
+ : event_loop_(event_loop),
+ claw_position_sender_(event_loop_->MakeSender<ClawQueue::Position>(
+ ".y2014.control_loops.claw_queue.position")),
+ claw_output_fetcher_(event_loop_->MakeFetcher<ClawQueue::Output>(
+ ".y2014.control_loops.claw_queue.output")),
+ claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())) {
Reinitialize(initial_top_position, initial_bottom_position);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
void Reinitialize(double initial_top_position,
@@ -177,8 +189,8 @@
// Sends out the position queue messages.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<::y2014::control_loops::ClawQueue::Position>
- position = claw_queue.position.MakeMessage();
+ ::aos::Sender<::y2014::control_loops::ClawQueue::Position>::Message
+ position = claw_position_sender_.MakeMessage();
// Initialize all the counters to their previous values.
*position = last_position_;
@@ -203,11 +215,11 @@
// Simulates the claw moving for one timestep.
void Simulate() {
const constants::Values& v = constants::GetValues();
- EXPECT_TRUE(claw_queue.output.FetchLatest());
+ EXPECT_TRUE(claw_output_fetcher_.Fetch());
Eigen::Matrix<double, 2, 1> U;
- U << claw_queue.output->bottom_claw_voltage,
- claw_queue.output->top_claw_voltage;
+ U << claw_output_fetcher_->bottom_claw_voltage,
+ claw_output_fetcher_->top_claw_voltage;
claw_plant_->Update(U);
// Check that the claw is within the limits.
@@ -222,87 +234,92 @@
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
}
+
+ private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Sender<ClawQueue::Position> claw_position_sender_;
+ ::aos::Fetcher<ClawQueue::Output> claw_output_fetcher_;
+
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
- private:
+ bool first_ = true;
+
// Resets the plant so that it starts at initial_position.
void ReinitializePartial(ClawType type, double initial_position) {
initial_position_[type] = initial_position;
}
- ::y2014::control_loops::ClawQueue claw_queue;
double initial_position_[CLAW_COUNT];
::y2014::control_loops::ClawQueue::Position last_position_;
};
-
class ClawTest : public ::aos::testing::ControlLoopTest {
protected:
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::y2014::control_loops::ClawQueue claw_queue;
+ ClawTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ test_event_loop_(MakeEventLoop()),
+ claw_goal_sender_(test_event_loop_->MakeSender<ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
+ claw_goal_fetcher_(test_event_loop_->MakeFetcher<ClawQueue::Goal>(
+ ".y2014.control_loops.claw_queue.goal")),
+ claw_event_loop_(MakeEventLoop()),
+ claw_motor_(claw_event_loop_.get()),
+ claw_plant_event_loop_(MakeEventLoop()),
+ claw_motor_plant_(claw_plant_event_loop_.get(), dt(), 0.4, 0.2),
+ min_separation_(constants::GetValues().claw.claw_min_separation) {}
- ::aos::ShmEventLoop event_loop_;
+ void VerifyNearGoal() {
+ claw_goal_fetcher_.Fetch();
+ double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
+ double separation =
+ claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
+ EXPECT_NEAR(claw_goal_fetcher_->bottom_angle, bottom, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->separation_angle, separation, 1e-4);
+ EXPECT_LE(min_separation_, separation);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<ClawQueue::Goal> claw_goal_sender_;
+ ::aos::Fetcher<ClawQueue::Goal> claw_goal_fetcher_;
+
// Create a loop and simulation plant.
+ ::std::unique_ptr<::aos::EventLoop> claw_event_loop_;
ClawMotor claw_motor_;
+
+ ::std::unique_ptr<::aos::EventLoop> claw_plant_event_loop_;
ClawMotorSimulation claw_motor_plant_;
// Minimum amount of acceptable separation between the top and bottom of the
// claw.
double min_separation_;
- ClawTest()
- : claw_queue(".y2014.control_loops.claw_queue",
- ".y2014.control_loops.claw_queue.goal",
- ".y2014.control_loops.claw_queue.position",
- ".y2014.control_loops.claw_queue.output",
- ".y2014.control_loops.claw_queue.status"),
- claw_motor_(&event_loop_),
- claw_motor_plant_(0.4, 0.2),
- min_separation_(constants::GetValues().claw.claw_min_separation) {}
-
- void VerifyNearGoal() {
- claw_queue.goal.FetchLatest();
- claw_queue.position.FetchLatest();
- double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
- double separation =
- claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
- EXPECT_NEAR(claw_queue.goal->bottom_angle, bottom, 1e-4);
- EXPECT_NEAR(claw_queue.goal->separation_angle, separation, 1e-4);
- EXPECT_LE(min_separation_, separation);
- }
};
TEST_F(ClawTest, HandlesNAN) {
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(::std::nan(""))
- .separation_angle(::std::nan(""))
- .Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(5))) {
- claw_motor_plant_.SendPositionMessage();
- claw_motor_.Iterate();
- claw_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = ::std::nan("");
+ message->separation_angle = ::std::nan("");
+ EXPECT_TRUE(message.Send());
}
+
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.1)
- .separation_angle(0.2)
- .Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(5))) {
- claw_motor_plant_.SendPositionMessage();
- claw_motor_.Iterate();
- claw_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = 0.1;
+ message->separation_angle = 0.2;
+ EXPECT_TRUE(message.Send());
}
+
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
@@ -391,18 +408,16 @@
TEST_P(ZeroingClawTest, ParameterizedZero) {
claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.1)
- .separation_angle(0.2)
- .Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(7))) {
- claw_motor_plant_.SendPositionMessage();
- claw_motor_.Iterate();
- claw_motor_plant_.Simulate();
-
- SimulateTimestep(true);
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = 0.1;
+ message->separation_angle = 0.2;
+ EXPECT_TRUE(message.Send());
}
+
+ SetEnabled(true);
+ RunFor(chrono::seconds(7));
+
VerifyNearGoal();
}
@@ -509,15 +524,15 @@
protected:
void TestWindup(ClawMotor::CalibrationMode mode,
monotonic_clock::time_point start_time, double offset) {
+ SetEnabled(true);
int capped_count = 0;
const constants::Values& values = constants::GetValues();
bool kicked = false;
bool measured = false;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(7))) {
- claw_motor_plant_.SendPositionMessage();
- if (monotonic_clock::now() >= start_time && mode == claw_motor_.mode() &&
- !kicked) {
+ if (test_event_loop_->monotonic_now() >= start_time &&
+ mode == claw_motor_.mode() && !kicked) {
EXPECT_EQ(mode, claw_motor_.mode());
// Move the zeroing position far away and verify that it gets moved
// back.
@@ -558,9 +573,7 @@
}
}
- claw_motor_.Iterate();
- claw_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
EXPECT_TRUE(kicked);
EXPECT_TRUE(measured);
@@ -572,10 +585,12 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupPositive) {
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.1)
- .separation_angle(0.2)
- .Send();
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = 0.1;
+ message->separation_angle = 0.2;
+ EXPECT_TRUE(message.Send());
+ }
TestWindup(ClawMotor::UNKNOWN_LOCATION,
monotonic_clock::time_point(chrono::seconds(1)), 971.0);
@@ -586,10 +601,12 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegative) {
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.1)
- .separation_angle(0.2)
- .Send();
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = 0.1;
+ message->separation_angle = 0.2;
+ EXPECT_TRUE(message.Send());
+ }
TestWindup(ClawMotor::UNKNOWN_LOCATION,
monotonic_clock::time_point(chrono::seconds(1)), -971.0);
@@ -600,10 +617,12 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegativeFineTune) {
- claw_queue.goal.MakeWithBuilder()
- .bottom_angle(0.1)
- .separation_angle(0.2)
- .Send();
+ {
+ auto message = claw_goal_sender_.MakeMessage();
+ message->bottom_angle = 0.1;
+ message->separation_angle = 0.2;
+ EXPECT_TRUE(message.Send());
+ }
TestWindup(ClawMotor::FINE_TUNE_BOTTOM,
monotonic_clock::time_point(chrono::seconds(2)), -971.0);
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
index 85497d9..65627d5 100644
--- a/y2014/control_loops/claw/claw_main.cc
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -4,10 +4,13 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014::control_loops::ClawMotor claw(&event_loop);
- claw.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 21dfcd2..7749631 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -7,13 +7,16 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- ::y2014::control_loops::GetDrivetrainConfig());
+ &event_loop, ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
&event_loop, &localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index d87c638..d1ef2a5 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -219,6 +219,7 @@
const ::y2014::control_loops::ShooterQueue::Position *position,
::y2014::control_loops::ShooterQueue::Output *output,
::y2014::control_loops::ShooterQueue::Status *status) {
+ const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
if (goal && ::std::isnan(goal->shot_power)) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because got a shot power of NAN.\n");
@@ -332,11 +333,11 @@
// The plunger is back and we are latched. We most likely got here
// from Initialize, in which case we want to 'load' again anyways to
// zero.
- Load();
+ Load(monotonic_now);
latch_piston_ = true;
} else {
// Off the sensor, start loading.
- Load();
+ Load(monotonic_now);
latch_piston_ = false;
}
}
@@ -361,7 +362,7 @@
if (position) {
if (!position->pusher_distal.current &&
!position->pusher_proximal.current) {
- Load();
+ Load(monotonic_now);
}
latch_piston_ = position->plunger;
}
@@ -371,7 +372,7 @@
case STATE_LOAD:
// If we are disabled right now, reset the timer.
if (disabled) {
- Load();
+ Load(monotonic_now);
// Latch defaults to true when disabled. Leave it latched until we have
// useful sensor data.
latch_piston_ = true;
@@ -406,17 +407,17 @@
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
loading_problem_end_time_ =
- monotonic_clock::now() + kLoadProblemEndTimeout;
+ monotonic_now + kLoadProblemEndTimeout;
}
}
- if (load_timeout_ < monotonic_clock::now()) {
+ if (load_timeout_ < monotonic_now) {
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
if (!(position->pusher_distal.current ||
position->pusher_proximal.current) ||
(load_timeout_ + chrono::milliseconds(500) <
- monotonic_clock::now())) {
+ monotonic_now)) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -431,9 +432,9 @@
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
- if (monotonic_clock::now() > loading_problem_end_time_) {
+ if (monotonic_now > loading_problem_end_time_) {
// Timeout by unloading.
- Unload();
+ Unload(monotonic_now);
} else if (position && position->plunger && position->latch) {
// If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
@@ -471,21 +472,21 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = monotonic_clock::now() + kShooterBrakeSetTime;
+ shooter_brake_set_time_ = monotonic_now + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
brake_piston_ = false;
}
if (goal && goal->unload_requested) {
- Unload();
+ Unload(monotonic_now);
}
break;
case STATE_READY:
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (monotonic_clock::now() > shooter_brake_set_time_) {
+ if (monotonic_now > shooter_brake_set_time_) {
status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
@@ -494,7 +495,7 @@
if (goal && goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_now + kShotEndTimeout;
firing_starting_position_ = shooter_.absolute_position();
state_ = STATE_FIRE;
}
@@ -519,7 +520,7 @@
brake_piston_ = true;
if (goal && goal->unload_requested) {
- Unload();
+ Unload(monotonic_now);
}
break;
@@ -529,7 +530,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_now + kShotEndTimeout;
}
}
}
@@ -548,7 +549,7 @@
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
- monotonic_clock::now() > shot_end_time_) &&
+ monotonic_now > shot_end_time_) &&
robot_state().voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
@@ -558,7 +559,7 @@
break;
case STATE_UNLOAD:
// Reset the timeouts.
- if (disabled) Unload();
+ if (disabled) Unload(monotonic_now);
// If it is latched and the plunger is back, move the pusher back to catch
// the plunger.
@@ -588,10 +589,10 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
}
- if (monotonic_clock::now() > unload_timeout_) {
+ if (monotonic_now > unload_timeout_) {
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
@@ -602,7 +603,7 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index a9a255b..01096a0 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -175,14 +175,14 @@
friend class testing::ShooterTest_RezeroWhileUnloading_Test;
// Enter state STATE_UNLOAD
- void Unload() {
+ void Unload(::aos::monotonic_clock::time_point monotonic_now) {
state_ = STATE_UNLOAD;
- unload_timeout_ = ::aos::monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
}
// Enter state STATE_LOAD
- void Load() {
+ void Load(::aos::monotonic_clock::time_point monotonic_now) {
state_ = STATE_LOAD;
- load_timeout_ = ::aos::monotonic_clock::now() + kLoadTimeout;
+ load_timeout_ = monotonic_now + kLoadTimeout;
}
::y2014::control_loops::ShooterQueue::Position last_position_;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index e88a33e..b6883ad 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -28,19 +28,32 @@
class ShooterSimulation {
public:
// Constructs a motor simulation.
- ShooterSimulation(double initial_position)
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
+ ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
+ double initial_position)
+ : event_loop_(event_loop),
+ shooter_position_sender_(
+ event_loop_->MakeSender<ShooterQueue::Position>(
+ ".y2014.control_loops.shooter_queue.position")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
+ ".y2014.control_loops.shooter_queue.output")),
+ shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
latch_piston_state_(false),
latch_delay_count_(0),
plunger_latched_(false),
brake_piston_state_(true),
- brake_delay_count_(0),
- shooter_queue_(".y2014.control_loops.shooter_queue",
- ".y2014.control_loops.shooter_queue.goal",
- ".y2014.control_loops.shooter_queue.position",
- ".y2014.control_loops.shooter_queue.output",
- ".y2014.control_loops.shooter_queue.status") {
+ brake_delay_count_(0) {
Reinitialize(initial_position);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
// The difference between the position with 0 at the back, and the position
@@ -133,25 +146,24 @@
}
}
- void SendPositionMessage() {
- // the first bool is false
- SendPositionMessage(false, false, false, false);
- }
+ void set_use_passed(bool use_passed) { use_passed_ = use_passed; }
+ void set_plunger_in(bool plunger_in) { plunger_in_ = plunger_in; }
+ void set_latch_in(bool latch_in) { latch_in_ = latch_in; }
+ void set_brake_in(bool brake_in) { brake_in_ = brake_in; }
// Sends out the position queue messages.
- // if the first bool is false then this is
+ // if used_passed_ is false then this is
// just the default state, otherwise will force
- // it into a state using the passed values
- void SendPositionMessage(bool use_passed, bool plunger_in,
- bool latch_in, bool brake_in) {
+ // it into a state using the plunger_in_, brake_in_, and latch_in_ values.
+ void SendPositionMessage() {
const constants::Values &values = constants::GetValues();
- ::aos::ScopedMessagePtr<::y2014::control_loops::ShooterQueue::Position>
- position = shooter_queue_.position.MakeMessage();
+ ::aos::Sender<ShooterQueue::Position>::Message position =
+ shooter_position_sender_.MakeMessage();
- if (use_passed) {
- plunger_latched_ = latch_in && plunger_in;
+ if (use_passed_) {
+ plunger_latched_ = latch_in_ && plunger_in_;
latch_piston_state_ = plunger_latched_;
- brake_piston_state_ = brake_in;
+ brake_piston_state_ = brake_in_;
}
SetPhysicalSensors(position.get());
@@ -175,22 +187,22 @@
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- if (shooter_queue_.output->latch_piston && !latch_piston_state_ &&
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ if (shooter_output_fetcher_->latch_piston && !latch_piston_state_ &&
latch_delay_count_ <= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = 6;
- } else if (!shooter_queue_.output->latch_piston &&
+ } else if (!shooter_output_fetcher_->latch_piston &&
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = -6;
}
- if (shooter_queue_.output->brake_piston && !brake_piston_state_ &&
+ if (shooter_output_fetcher_->brake_piston && !brake_piston_state_ &&
brake_delay_count_ <= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = 5;
- } else if (!shooter_queue_.output->brake_piston &&
+ } else if (!shooter_output_fetcher_->brake_piston &&
brake_piston_state_ && brake_delay_count_ >= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = -5;
@@ -251,10 +263,16 @@
EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
GetAbsolutePosition());
- last_voltage_ = shooter_queue_.output->voltage;
- ::aos::time::IncrementMockTime(chrono::milliseconds(10));
+ last_voltage_ = shooter_output_fetcher_->voltage;
}
+ private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
+ ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+
+ bool first_ = true;
+
// pointer to plant
const ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
@@ -271,12 +289,16 @@
// greater than zero, delaying close. less than zero delaying open
int brake_delay_count_;
- private:
- ::y2014::control_loops::ShooterQueue shooter_queue_;
+ // Overrides for testing.
+ bool use_passed_ = false;
+ bool plunger_in_ = false;
+ bool latch_in_ = false;
+ bool brake_in_ = false;
+
double initial_position_;
double last_voltage_;
- ::y2014::control_loops::ShooterQueue::Position last_position_message_;
+ ShooterQueue::Position last_position_message_;
double last_plant_position_;
};
@@ -284,35 +306,40 @@
class ShooterTestTemplated
: public ::aos::testing::ControlLoopTestTemplated<TestType> {
protected:
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::y2014::control_loops::ShooterQueue shooter_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- // Create a loop and simulation plant.
- ShooterMotor shooter_motor_;
- ShooterSimulation shooter_motor_plant_;
+ ShooterTestTemplated()
+ : ::aos::testing::ControlLoopTestTemplated<TestType>(
+ // TODO(austin): I think this runs at 5 ms in real life.
+ chrono::microseconds(5000)),
+ test_event_loop_(this->MakeEventLoop()),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
+ ".y2014.control_loops.shooter_queue.goal")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
+ ".y2014.control_loops.shooter_queue.goal")),
+ shooter_event_loop_(this->MakeEventLoop()),
+ shooter_motor_(shooter_event_loop_.get()),
+ shooter_plant_event_loop_(this->MakeEventLoop()),
+ shooter_motor_plant_(shooter_plant_event_loop_.get(), this->dt(), 0.2) {
+ }
void Reinitialize(double position) {
shooter_motor_plant_.Reinitialize(position);
}
- ShooterTestTemplated()
- : shooter_queue_(".y2014.control_loops.shooter_queue",
- ".y2014.control_loops.shooter_queue.goal",
- ".y2014.control_loops.shooter_queue.position",
- ".y2014.control_loops.shooter_queue.output",
- ".y2014.control_loops.shooter_queue.status"),
- shooter_motor_(&event_loop_),
- shooter_motor_plant_(0.2) {}
-
void VerifyNearGoal() {
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.position.FetchLatest();
- double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_.goal->shot_power, pos, 1e-4);
+ shooter_goal_fetcher_.Fetch();
+ const double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_goal_fetcher_->shot_power, pos, 1e-4);
}
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
+ ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+
+ // Create a loop and simulation plant.
+ ::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
+ ShooterMotor shooter_motor_;
+ ::std::unique_ptr<::aos::EventLoop> shooter_plant_event_loop_;
+ ShooterSimulation shooter_motor_plant_;
};
typedef ShooterTestTemplated<::testing::Test> ShooterTest;
@@ -350,99 +377,104 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
- // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ RunFor(chrono::seconds(2));
+
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1200))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1200));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder()
- .shot_power(35.0)
- .shot_requested(true)
- .Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 35.0;
+ message->shot_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
bool hit_fire = false;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(5200))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- shooter_queue_.goal.MakeWithBuilder()
- .shot_power(17.0)
- .shot_requested(false)
- .Send();
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 17.0;
+ message->shot_requested = false;
+ EXPECT_TRUE(message.Send());
}
hit_fire = true;
}
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
- EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
- pos, 0.05);
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
+
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().shot_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 0.0;
+ message->shot_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
bool hit_fire = false;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(5500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- shooter_queue_.goal.MakeWithBuilder()
- .shot_requested(false)
- .Send();
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_requested = false;
+ EXPECT_TRUE(message.Send());
}
hit_fire = true;
}
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
@@ -451,13 +483,16 @@
// Verifies that it doesn't try to go out too far if you give it a ridicilous
// power.
TEST_F(ShooterTest, LoadTooFar) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(500.0).Send();
- while (monotonic_clock::now() <
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 500.0;
+ EXPECT_TRUE(message.Send());
+ }
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(1600))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
EXPECT_LT(shooter_motor_plant_.GetAbsolutePosition(),
constants::GetValuesForTeam(kTeamNumber).shooter.upper_limit);
}
@@ -466,53 +501,55 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
- EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().shot_power(14.0).Send();
+ RunFor(chrono::milliseconds(1500));
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 14.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(500));
+
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
TEST_F(ShooterTest, Unload) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(8)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -522,34 +559,31 @@
// Tests that it rezeros while unloading.
TEST_F(ShooterTest, RezeroWhileUnloading) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
+
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_motor_.shooter_.offset_ += 0.01;
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(chrono::milliseconds(500));
+
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
}
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
-
- while (::aos::monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
::aos::monotonic_clock::time_point(chrono::seconds(10)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -559,24 +593,28 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
int kicked_delay = 20;
int capped_goal_count = 0;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(9500)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
@@ -587,8 +625,6 @@
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
++capped_goal_count;
}
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -600,24 +636,28 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
int kicked_delay = 20;
int capped_goal_count = 0;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(9500)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
@@ -628,8 +668,6 @@
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
++capped_goal_count;
}
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -645,20 +683,20 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
+ SetEnabled(true);
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::seconds(2));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -666,21 +704,21 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnProximal) {
+ SetEnabled(true);
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(3))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::seconds(3));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -690,6 +728,7 @@
::testing::TestWithParam<::std::tuple<bool, bool, bool, double>>> {};
TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ SetEnabled(true);
bool latch = ::std::get<0>(GetParam());
bool brake = ::std::get<1>(GetParam());
bool plunger_back = ::std::get<2>(GetParam());
@@ -699,20 +738,26 @@
// latch, brake, plunger_back, start_pos);
bool initialized = false;
Reinitialize(start_pos);
- shooter_queue_.goal.MakeWithBuilder().shot_power(120.0).Send();
- while (monotonic_clock::now() <
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 120.0;
+ EXPECT_TRUE(message.Send());
+ }
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ shooter_motor_plant_.set_use_passed(!initialized);
+ shooter_motor_plant_.set_plunger_in(plunger_back);
+ shooter_motor_plant_.set_latch_in(latch);
+ shooter_motor_plant_.set_brake_in(brake);
initialized = true;
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index a7b60f9..2da76c3 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -4,10 +4,13 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014::control_loops::ShooterMotor shooter(&event_loop);
- shooter.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index ecd55c9..24ac312 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -441,9 +441,12 @@
} // namespace y2014
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index f13a8ab..48f09c5 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,18 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ &event_loop,
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
&event_loop, &localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index d783d98..1a0d82b 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -4,10 +4,13 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014_bot3::control_loops::Rollers rollers(&event_loop);
- rollers.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 989a1c1..44eb2b8 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -140,9 +140,12 @@
} // namespace y2014_bot3
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014_bot3::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 12d8642..58ba7ce 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,17 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
+ &event_loop, ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
&localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 3c8be30..30b5166 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -48,21 +48,33 @@
class ShooterSimulation {
public:
// Constructs a shooter simulation.
- ShooterSimulation()
- : shooter_plant_left_(new ShooterPlant(
+ ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ shooter_position_sender_(
+ event_loop_->MakeSender<ShooterQueue::Position>(
+ ".y2016.control_loops.shooter.shooter_queue.position")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
+ ".y2016.control_loops.shooter.shooter_queue.output")),
+ shooter_plant_left_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_plant_right_(new ShooterPlant(
- ::y2016::control_loops::shooter::MakeShooterPlant())),
- shooter_queue_(".y2016.control_loops.shooter.shooter_queue",
- ".y2016.control_loops.shooter.shooter_queue.goal",
- ".y2016.control_loops.shooter.shooter_queue.position",
- ".y2016.control_loops.shooter.shooter_queue.output",
- ".y2016.control_loops.shooter.shooter_queue.status") {}
+ ::y2016::control_loops::shooter::MakeShooterPlant())) {
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<ShooterQueue::Position> position =
- shooter_queue_.position.MakeMessage();
+ ::aos::Sender<ShooterQueue::Position>::Message position =
+ shooter_position_sender_.MakeMessage();
position->theta_left = shooter_plant_left_->Y(0, 0);
position->theta_right = shooter_plant_right_->Y(0, 0);
@@ -79,170 +91,190 @@
// Simulates shooter for a single timestep.
void Simulate() {
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
::Eigen::Matrix<double, 1, 1> U_left;
::Eigen::Matrix<double, 1, 1> U_right;
- U_left(0, 0) = shooter_queue_.output->voltage_left +
+ U_left(0, 0) = shooter_output_fetcher_->voltage_left +
shooter_plant_left_->voltage_offset();
- U_right(0, 0) = shooter_queue_.output->voltage_right +
+ U_right(0, 0) = shooter_output_fetcher_->voltage_right +
shooter_plant_right_->voltage_offset();
shooter_plant_left_->Update(U_left);
shooter_plant_right_->Update(U_right);
}
+ private:
+ ::aos::EventLoop *event_loop_;
+
+ ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
+ ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
- private:
- ShooterQueue shooter_queue_;
+ bool first_ = true;
};
class ShooterTest : public ::aos::testing::ControlLoopTest {
protected:
ShooterTest()
- : shooter_queue_(".y2016.control_loops.shooter.shooter_queue",
- ".y2016.control_loops.shooter.shooter_queue.goal",
- ".y2016.control_loops.shooter.shooter_queue.position",
- ".y2016.control_loops.shooter.shooter_queue.output",
- ".y2016.control_loops.shooter.shooter_queue.status"),
- shooter_(&event_loop_),
- shooter_plant_() {
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ test_event_loop_(MakeEventLoop()),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
+ ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
+ ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_status_fetcher_(
+ test_event_loop_->MakeFetcher<ShooterQueue::Status>(
+ ".y2016.control_loops.shooter.shooter_queue.status")),
+ shooter_output_fetcher_(
+ test_event_loop_->MakeFetcher<ShooterQueue::Output>(
+ ".y2016.control_loops.shooter.shooter_queue.output")),
+ shooter_event_loop_(MakeEventLoop()),
+ shooter_(shooter_event_loop_.get()),
+ shooter_plant_event_loop_(MakeEventLoop()),
+ shooter_plant_(shooter_plant_event_loop_.get(), dt()) {
set_team_id(kTeamNumber);
}
void VerifyNearGoal() {
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.status.FetchLatest();
+ shooter_goal_fetcher_.Fetch();
+ shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
- EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+ EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
- shooter_queue_.status->left.angular_velocity, 10.0);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
- shooter_queue_.status->right.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
+ shooter_status_fetcher_->left.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
+ shooter_status_fetcher_->right.angular_velocity, 10.0);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
- shooter_queue_.status->left.avg_angular_velocity, 10.0);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
- shooter_queue_.status->right.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
+ shooter_status_fetcher_->left.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
+ shooter_status_fetcher_->right.avg_angular_velocity, 10.0);
}
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
+ ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<ShooterQueue::Status> shooter_status_fetcher_;
+ ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
- shooter_plant_.SendPositionMessage();
- shooter_.Iterate();
- shooter_plant_.Simulate();
-
- TickTime();
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for, bool enabled = true) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- RunIteration(enabled);
- }
- }
-
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ShooterQueue shooter_queue_;
-
- ::aos::ShmEventLoop event_loop_;
// Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
Shooter shooter_;
+
+ ::std::unique_ptr<::aos::EventLoop> shooter_plant_event_loop_;
ShooterSimulation shooter_plant_;
};
// Tests that the shooter does nothing when the goal is zero.
TEST_F(ShooterTest, DoesNothing) {
- ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity(0.0)
- .Send());
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = shooter_goal_sender_.MakeMessage();
+ message->angular_velocity = 0.0;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(dt());
VerifyNearGoal();
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- EXPECT_EQ(shooter_queue_.output->voltage_left, 0.0);
- EXPECT_EQ(shooter_queue_.output->voltage_right, 0.0);
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
}
// Tests that the shooter spins up to speed and that it then spins down
// without applying any power.
TEST_F(ShooterTest, SpinUpAndDown) {
+ SetEnabled(true);
// Spin up.
- EXPECT_TRUE(
- shooter_queue_.goal.MakeWithBuilder().angular_velocity(450.0).Send());
- RunForTime(chrono::seconds(1));
+ {
+ auto message = shooter_goal_sender_.MakeMessage();
+ message->angular_velocity = 450.0;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(chrono::seconds(1));
VerifyNearGoal();
- EXPECT_TRUE(shooter_queue_.status->ready);
- EXPECT_TRUE(
- shooter_queue_.goal.MakeWithBuilder().angular_velocity(0.0).Send());
+ shooter_status_fetcher_.Fetch();
+ EXPECT_TRUE(shooter_status_fetcher_->ready);
+ {
+ auto message = shooter_goal_sender_.MakeMessage();
+ message->angular_velocity = 0.0;
+ EXPECT_TRUE(message.Send());
+ }
// Make sure we don't apply voltage on spin-down.
- RunIteration();
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- EXPECT_EQ(0.0, shooter_queue_.output->voltage_left);
- EXPECT_EQ(0.0, shooter_queue_.output->voltage_right);
+ RunFor(dt());
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
// Continue to stop.
- RunForTime(chrono::seconds(5));
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- EXPECT_EQ(0.0, shooter_queue_.output->voltage_left);
- EXPECT_EQ(0.0, shooter_queue_.output->voltage_right);
+ RunFor(chrono::seconds(5));
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
}
// Tests that the shooter doesn't say it is ready if one side isn't up to speed.
// According to our tuning, we may overshoot the goal on one shooter and
// mistakenly say that we are ready. This test should look at both extremes.
TEST_F(ShooterTest, SideLagTest) {
+ SetEnabled(true);
// Spin up.
- EXPECT_TRUE(
- shooter_queue_.goal.MakeWithBuilder().angular_velocity(20.0).Send());
+ {
+ auto message = shooter_goal_sender_.MakeMessage();
+ message->angular_velocity = 20.0;
+ EXPECT_TRUE(message.Send());
+ }
// Cause problems by adding a voltage error on one side.
shooter_plant_.set_right_voltage_offset(-4.0);
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.status.FetchLatest();
+ shooter_goal_fetcher_.Fetch();
+ shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
- EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+ EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Left should be up to speed, right shouldn't.
- EXPECT_TRUE(shooter_queue_.status->left.ready);
- EXPECT_FALSE(shooter_queue_.status->right.ready);
- EXPECT_FALSE(shooter_queue_.status->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left.ready);
+ EXPECT_FALSE(shooter_status_fetcher_->right.ready);
+ EXPECT_FALSE(shooter_status_fetcher_->ready);
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.status.FetchLatest();
+ shooter_goal_fetcher_.Fetch();
+ shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
- EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+ EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Both should be up to speed.
- EXPECT_TRUE(shooter_queue_.status->left.ready);
- EXPECT_TRUE(shooter_queue_.status->right.ready);
- EXPECT_TRUE(shooter_queue_.status->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left.ready);
+ EXPECT_TRUE(shooter_status_fetcher_->right.ready);
+ EXPECT_TRUE(shooter_status_fetcher_->ready);
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(ShooterTest, Disabled) {
- ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity(200.0)
- .Send());
- RunForTime(chrono::seconds(5), false);
- EXPECT_EQ(nullptr, shooter_queue_.output.get());
+ {
+ auto message = shooter_goal_sender_.MakeMessage();
+ message->angular_velocity = 200.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(5));
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index 8070dde..b9737fd 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -4,10 +4,13 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2016::control_loops::shooter::Shooter shooter(&event_loop);
- shooter.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e7983f3..969102f 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -81,22 +81,38 @@
class SuperstructureSimulation {
public:
static constexpr double kNoiseScalar = 0.1;
- SuperstructureSimulation()
- : intake_plant_(new IntakePlant(MakeIntakePlant())),
+ SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<SuperstructureQueue::Position>(
+ ".y2016.control_loops.superstructure_queue.position")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2016.control_loops.superstructure_queue.output")),
+ intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
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") {
+ pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference) {
InitializeIntakePosition(0.0);
InitializeShoulderPosition(0.0);
InitializeRelativeWristPosition(0.0);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
void InitializeIntakePosition(double start_pos) {
@@ -128,8 +144,8 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::SuperstructureQueue::Position>
- position = superstructure_queue_.position.MakeMessage();
+ ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
+ position = superstructure_position_sender_.MakeMessage();
pot_encoder_intake_.GetSensorValues(&position->intake);
pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
@@ -156,42 +172,45 @@
// Simulates for a single timestep.
void Simulate() {
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ const double begin_shoulder_velocity = shoulder_angular_velocity();
+ const double begin_intake_velocity = intake_angular_velocity();
+ const double begin_wrist_velocity = wrist_angular_velocity();
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_queue_.output->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake +
intake_plant_->voltage_offset();
- arm_U << superstructure_queue_.output->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder +
arm_plant_->shoulder_voltage_offset(),
- superstructure_queue_.output->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
- EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
- if (superstructure_queue_.status->state == Superstructure::RUNNING ||
- superstructure_queue_.status->state ==
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+ if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state ==
Superstructure::LANDING_RUNNING) {
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
Superstructure::kOperatingVoltage);
} else {
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- CHECK_GE(superstructure_queue_.output->voltage_shoulder,
+ CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -231,127 +250,33 @@
EXPECT_LE(angle_shoulder, constants::Values::kShoulderRange.upper_hard);
EXPECT_GE(angle_wrist, constants::Values::kWristRange.lower_hard);
EXPECT_LE(angle_wrist, constants::Values::kWristRange.upper_hard);
- }
- private:
- ::std::unique_ptr<IntakePlant> intake_plant_;
- ::std::unique_ptr<ArmPlant> arm_plant_;
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+ const double shoulder_acceleration =
+ (shoulder_angular_velocity() - begin_shoulder_velocity) / loop_time;
+ const double intake_acceleration =
+ (intake_angular_velocity() - begin_intake_velocity) / loop_time;
+ const double wrist_acceleration =
+ (wrist_angular_velocity() - begin_wrist_velocity) / loop_time;
+ EXPECT_GE(peak_shoulder_acceleration_, shoulder_acceleration);
+ EXPECT_LE(-peak_shoulder_acceleration_, shoulder_acceleration);
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
- PositionSensorSimulator pot_encoder_intake_;
- PositionSensorSimulator pot_encoder_shoulder_;
- PositionSensorSimulator pot_encoder_wrist_;
+ EXPECT_GE(peak_shoulder_velocity_, shoulder_angular_velocity());
+ EXPECT_LE(-peak_shoulder_velocity_, shoulder_angular_velocity());
+ EXPECT_GE(peak_intake_velocity_, intake_angular_velocity());
+ EXPECT_LE(-peak_intake_velocity_, intake_angular_velocity());
+ EXPECT_GE(peak_wrist_velocity_, wrist_angular_velocity());
+ EXPECT_LE(-peak_wrist_velocity_, wrist_angular_velocity());
- SuperstructureQueue superstructure_queue_;
-};
-
-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_(&event_loop_),
- superstructure_plant_() {}
-
- void VerifyNearGoal() {
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.status.FetchLatest();
-
- EXPECT_TRUE(superstructure_queue_.goal.get() != nullptr);
- EXPECT_TRUE(superstructure_queue_.status.get() != nullptr);
-
- EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
- superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
- superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
- superstructure_queue_.status->wrist.angle, 0.001);
-
- EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
- superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
- superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
- superstructure_plant_.wrist_angle(), 0.001);
- }
-
- // Runs one iteration of the whole simulation and checks that separation
- // remains reasonable.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
-
- superstructure_plant_.SendPositionMessage();
- superstructure_.Iterate();
- superstructure_plant_.Simulate();
-
- TickTime();
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(monotonic_clock::duration run_for, bool enabled = true) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- const auto loop_start_time = monotonic_clock::now();
- double begin_shoulder_velocity =
- superstructure_plant_.shoulder_angular_velocity();
- double begin_intake_velocity =
- superstructure_plant_.intake_angular_velocity();
- double begin_wrist_velocity =
- superstructure_plant_.wrist_angular_velocity();
- RunIteration(enabled);
- 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) /
- loop_time;
- const double intake_acceleration =
- (superstructure_plant_.intake_angular_velocity() -
- begin_intake_velocity) /
- loop_time;
- const double wrist_acceleration =
- (superstructure_plant_.wrist_angular_velocity() -
- begin_wrist_velocity) /
- loop_time;
- EXPECT_GE(peak_shoulder_acceleration_, shoulder_acceleration);
- EXPECT_LE(-peak_shoulder_acceleration_, shoulder_acceleration);
- EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
- EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
- EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
- EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
-
- EXPECT_GE(peak_shoulder_velocity_,
- superstructure_plant_.shoulder_angular_velocity());
- EXPECT_LE(-peak_shoulder_velocity_,
- superstructure_plant_.shoulder_angular_velocity());
- EXPECT_GE(peak_intake_velocity_,
- superstructure_plant_.intake_angular_velocity());
- EXPECT_LE(-peak_intake_velocity_,
- superstructure_plant_.intake_angular_velocity());
- EXPECT_GE(peak_wrist_velocity_,
- superstructure_plant_.wrist_angular_velocity());
- EXPECT_LE(-peak_wrist_velocity_,
- superstructure_plant_.wrist_angular_velocity());
-
- if (check_for_collisions_) {
- ASSERT_FALSE(collided());
- }
+ if (check_for_collisions_) {
+ ASSERT_FALSE(collided());
}
}
- // Helper function to quickly check if either the estimation detected a
- // collision or if there's a collision using ground-truth plant values.
- bool collided() const {
- return superstructure_.collided() ||
- CollisionAvoidance::collided_with_given_angles(
- superstructure_plant_.shoulder_angle(),
- superstructure_plant_.wrist_angle(),
- superstructure_plant_.intake_angle());
- }
-
// Runs iterations while watching the average acceleration per cycle and
// making sure it doesn't exceed the provided bounds.
void set_peak_intake_acceleration(double value) {
@@ -369,19 +294,33 @@
}
void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
- bool check_for_collisions_ = true;
+ void set_check_for_collisions(bool check_for_collisions) {
+ check_for_collisions_ = check_for_collisions;
+ }
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointed to
- // shared memory that is no longer valid.
- SuperstructureQueue superstructure_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- // Create a control loop and simulation.
- Superstructure superstructure_;
- SuperstructureSimulation superstructure_plant_;
+ bool collided() const {
+ return CollisionAvoidance::collided_with_given_angles(
+ shoulder_angle(), wrist_angle(), intake_angle());
+ }
private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+
+ bool first_ = true;
+
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+
+ ::std::unique_ptr<IntakePlant> intake_plant_;
+ ::std::unique_ptr<ArmPlant> arm_plant_;
+
+ PositionSensorSimulator pot_encoder_intake_;
+ PositionSensorSimulator pot_encoder_shoulder_;
+ PositionSensorSimulator pot_encoder_wrist_;
+
+ bool check_for_collisions_ = true;
// The acceleration limits to check for while moving for the 3 axes.
double peak_intake_acceleration_ = 1e10;
double peak_shoulder_acceleration_ = 1e10;
@@ -392,42 +331,102 @@
double peak_wrist_velocity_ = 1e10;
};
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ test_event_loop_(MakeEventLoop()),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_event_loop_(MakeEventLoop()),
+ superstructure_(superstructure_event_loop_.get()),
+ superstructure_plant_event_loop_(MakeEventLoop()),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {}
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ superstructure_status_fetcher_->wrist.angle, 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ superstructure_plant_.intake_angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ superstructure_plant_.shoulder_angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ superstructure_plant_.wrist_angle(), 0.001);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+
+ ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
+ Superstructure superstructure_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
+};
+
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0)
- .angle_shoulder(0)
- .angle_wrist(0)
- .max_angular_velocity_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_intake(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0;
+ message->angle_shoulder = 0;
+ message->angle_wrist = 0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- // TODO(phil): Send a goal of some sort.
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Tests that the loop can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
// Set a reasonable goal.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI / 4.0)
- .angle_shoulder(1.4)
- .angle_wrist(M_PI / 4.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI / 4.0;
+ message->angle_shoulder = 1.4;
+ message->angle_wrist = M_PI / 4.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -435,109 +434,121 @@
// Tests that the loop doesn't try and go beyond the physical range of the
// mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
+ SetEnabled(true);
// Set some ridiculous goals to test upper limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI * 10)
- .angle_shoulder(M_PI * 10)
- .angle_wrist(M_PI * 10)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
- RunForTime(chrono::seconds(10));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI * 10;
+ message->angle_shoulder = M_PI * 10;
+ message->angle_wrist = M_PI * 10;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist.angle, 0.001);
// Set some ridiculous goals to test limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI * 10)
- .angle_shoulder(M_PI * 10)
- .angle_wrist(-M_PI * 10.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI * 10;
+ message->angle_shoulder = M_PI * 10;
+ message->angle_wrist = -M_PI * 10.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist.angle, 0.001);
// Set some ridiculous goals to test lower limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(-M_PI * 10)
- .angle_shoulder(-M_PI * 10)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = -M_PI * 10;
+ message->angle_shoulder = -M_PI * 10;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Tests that starting at the lower hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, LowerHardstopStartup) {
+ SetEnabled(true);
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
@@ -545,40 +556,46 @@
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper;
+ ASSERT_TRUE(message.Send());
+ }
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
// We have to wait for it to put the superstructure in a safe position as
// well.
- RunForTime(chrono::seconds(20));
+ RunFor(chrono::seconds(20));
VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -586,19 +603,21 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(15));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(15));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
- RunForTime(chrono::milliseconds(10000));
+ RunFor(chrono::milliseconds(10000));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
}
@@ -606,22 +625,24 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower + 0.03)
- .angle_shoulder(constants::Values::kShoulderRange.lower + 0.03)
- .angle_wrist(constants::Values::kWristRange.lower + 0.03)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
+ message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::milliseconds(100), false);
+ RunFor(chrono::milliseconds(100));
EXPECT_EQ(0.0, superstructure_.intake_.goal(0, 0));
EXPECT_EQ(0.0, superstructure_.arm_.goal(0, 0));
EXPECT_EQ(0.0, superstructure_.arm_.goal(2, 0));
// Now make sure they move correctly
- RunForTime(chrono::milliseconds(4000), true);
+ SetEnabled(true);
+ RunFor(chrono::milliseconds(4000));
EXPECT_NE(0.0, superstructure_.intake_.goal(0, 0));
EXPECT_NE(0.0, superstructure_.arm_.goal(0, 0));
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
@@ -629,6 +650,7 @@
// Tests that disabling while zeroing at any state restarts from beginning
TEST_F(SuperstructureTest, DisabledWhileZeroingHigh) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -636,17 +658,19 @@
superstructure_plant_.InitializeAbsoluteWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(constants::Values::kWristRange.upper)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = constants::Values::kWristRange.upper;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Expected states to cycle through and check in order.
Superstructure::State kExpectedStateOrder[] = {
@@ -659,7 +683,7 @@
// Cycle through until arm_ and intake_ are initialized in superstructure.cc
while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
- RunIteration(true);
+ RunFor(dt());
}
static const int kNumberOfStates =
@@ -675,7 +699,8 @@
// of 10000 times to ensure a breakout
for (int o = 0;
superstructure_.state() < kExpectedStateOrder[j] && o < 10000; o++) {
- RunIteration(true);
+ RunFor(dt());
+ EXPECT_LT(o, 9999);
}
EXPECT_EQ(kExpectedStateOrder[j], superstructure_.state());
}
@@ -683,34 +708,40 @@
EXPECT_EQ(kExpectedStateOrder[i], superstructure_.state());
// Disable
- RunIteration(false);
+ SetEnabled(false);
+ RunFor(dt());
+ SetEnabled(true);
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
}
- RunForTime(chrono::seconds(10));
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Tests that disabling while zeroing at any state restarts from beginning
TEST_F(SuperstructureTest, DisabledWhileZeroingLow) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(constants::Values::kWristRange.lower)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = constants::Values::kWristRange.lower;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Expected states to cycle through and check in order.
Superstructure::State kExpectedStateOrder[] = {
@@ -723,7 +754,7 @@
// Cycle through until arm_ and intake_ are initialized in superstructure.cc
while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
- RunIteration(true);
+ RunFor(dt());
}
static const int kNumberOfStates =
@@ -739,7 +770,8 @@
// of 10000 times to ensure a breakout
for (int o = 0;
superstructure_.state() < kExpectedStateOrder[j] && o < 10000; o++) {
- RunIteration(true);
+ RunFor(dt());
+ EXPECT_LT(o, 9999);
}
EXPECT_EQ(kExpectedStateOrder[j], superstructure_.state());
}
@@ -747,12 +779,15 @@
EXPECT_EQ(kExpectedStateOrder[i], superstructure_.state());
// Disable
- RunIteration(false);
+ SetEnabled(false);
+ RunFor(dt());
+ SetEnabled(true);
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
}
- RunForTime(chrono::seconds(10));
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
}
@@ -766,8 +801,9 @@
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
+ SetEnabled(true);
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
@@ -775,13 +811,15 @@
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send();
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -795,168 +833,185 @@
constants::Values::kShoulderEncoderIndexDifference * 10 - 0.001);
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderEncoderIndexDifference * 10)
- .angle_wrist(0.0)
- .Send();
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ constants::Values::kShoulderEncoderIndexDifference * 10;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
// Run disabled for 2 seconds
- RunForTime(chrono::seconds(2), false);
+ RunFor(chrono::seconds(2));
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
superstructure_plant_.set_power_error(1.0, 1.5, 1.0);
- RunForTime(chrono::seconds(1), false);
+ RunFor(chrono::seconds(1));
EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
- RunForTime(chrono::seconds(2), true);
+ SetEnabled(true);
+ RunFor(chrono::seconds(2));
VerifyNearGoal();
}
// Tests that the zeroing errors in the arm are caught
TEST_F(SuperstructureTest, ArmZeroingErrorTest) {
- RunIteration();
+ SetEnabled(true);
+ RunFor(2 * dt());
EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
superstructure_.arm_.TriggerEstimatorError();
- RunIteration();
+ RunFor(2 * dt());
EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
}
// Tests that the zeroing errors in the intake are caught
TEST_F(SuperstructureTest, IntakeZeroingErrorTest) {
- RunIteration();
+ SetEnabled(true);
+ RunFor(2 * dt());
EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
superstructure_.intake_.TriggerEstimatorError();
- RunIteration();
+ RunFor(2 * dt());
EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
}
// Tests that the loop respects shoulder acceleration limits while moving.
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.5)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.5;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
// TODO(austin): The profile isn't feasible, so when we try to track it, we
// have trouble going from the acceleration step to the constant velocity
// step. We end up under and then overshooting.
- set_peak_intake_acceleration(1.10);
- set_peak_shoulder_acceleration(1.20);
- set_peak_wrist_acceleration(1.10);
- RunForTime(chrono::seconds(6));
+ superstructure_plant_.set_peak_intake_acceleration(1.10);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.20);
+ superstructure_plant_.set_peak_wrist_acceleration(1.10);
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
}
// Tests that the loop respects intake acceleration limits while moving.
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.5)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.5;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_acceleration(1.20);
- set_peak_shoulder_acceleration(1.20);
- set_peak_wrist_acceleration(1.20);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_acceleration(1.20);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.20);
+ superstructure_plant_.set_peak_wrist_acceleration(1.20);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
// Tests that the loop respects wrist acceleration limits while moving.
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.5)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.5;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_acceleration(1.05);
- set_peak_shoulder_acceleration(1.05);
- set_peak_wrist_acceleration(1.05);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_acceleration(1.05);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.05);
+ superstructure_plant_.set_peak_wrist_acceleration(1.05);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -964,43 +1019,46 @@
// Tests that the loop respects intake handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.5)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(4.5)
- .max_angular_acceleration_intake(800)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(100)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(100)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.5;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 4.5;
+ message->max_angular_acceleration_intake = 800;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 100;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 100;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(4.65);
- set_peak_shoulder_velocity(1.00);
- set_peak_wrist_velocity(1.00);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(4.65);
+ superstructure_plant_.set_peak_shoulder_velocity(1.00);
+ superstructure_plant_.set_peak_wrist_velocity(1.00);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1008,39 +1066,44 @@
// Tests that the loop respects shoulder handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.9)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1.0)
- .max_angular_acceleration_intake(1.0)
- .max_angular_velocity_shoulder(5.0)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(100)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.9;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1.0;
+ message->max_angular_acceleration_intake = 1.0;
+ message->max_angular_velocity_shoulder = 5.0;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 100;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(1.0);
- set_peak_shoulder_velocity(5.5);
- set_peak_wrist_velocity(1.0);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(1.0);
+ superstructure_plant_.set_peak_shoulder_velocity(5.5);
+ superstructure_plant_.set_peak_wrist_velocity(1.0);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1048,45 +1111,46 @@
// Tests that the loop respects wrist handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(1.3)
- .max_angular_velocity_intake(1.0)
- .max_angular_acceleration_intake(1.0)
- .max_angular_velocity_shoulder(1.0)
- .max_angular_acceleration_shoulder(1.0)
- .max_angular_velocity_wrist(10.0)
- .max_angular_acceleration_wrist(160.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 1.3;
+ message->max_angular_velocity_intake = 1.0;
+ message->max_angular_acceleration_intake = 1.0;
+ message->max_angular_velocity_shoulder = 1.0;
+ message->max_angular_acceleration_shoulder = 1.0;
+ message->max_angular_velocity_wrist = 10.0;
+ message->max_angular_acceleration_wrist = 160.0;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(1.0);
- set_peak_shoulder_velocity(1.0);
- set_peak_wrist_velocity(10.2);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(1.0);
+ superstructure_plant_.set_peak_shoulder_velocity(1.0);
+ superstructure_plant_.set_peak_wrist_velocity(10.2);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1094,243 +1158,275 @@
// Make sure that the intake moves out of the way when the arm wants to move
// into a shooting position.
TEST_F(SuperstructureTest, AvoidCollisionWhenMovingArmFromStart) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(0.0);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper) // stowed
- .angle_shoulder(constants::Values::kShoulderRange.lower) // Down
- .angle_wrist(0.0) // Stowed
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
+ message->angle_wrist = 0.0; // Stowed
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper) // stowed
- .angle_shoulder(M_PI / 4.0) // in the collision area
- .angle_wrist(M_PI / 2.0) // down
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = M_PI / 4.0; // in the collision area
+ message->angle_wrist = M_PI / 2.0; // down
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_queue_.status->intake.angle, M_PI);
- EXPECT_LT(superstructure_queue_.status->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle,
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_queue_.status->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle,
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
- ASSERT_TRUE(
- 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
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = M_PI / 2.0; // in the collision area
+ message->angle_wrist = M_PI; // forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Make sure that the shooter holds itself level when the arm comes down
// into a stowing/intaking position.
TEST_F(SuperstructureTest, AvoidCollisionWhenStowingArm) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(0.0); // forward
superstructure_plant_.InitializeShoulderPosition(M_PI / 2.0); // up
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(M_PI) // intentionally asking for forward
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = M_PI; // intentionally asking for forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_queue_.status->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
}
// Make sure that we can properly detect a collision.
TEST_F(SuperstructureTest, DetectAndFixCollisionBetweenArmAndIntake) {
+ SetEnabled(true);
// Zero & go straight up with the shoulder.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI * 0.5)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI * 0.5;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// Since we're explicitly checking for collisions, we don't want to fail the
// test because of collisions.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
// Move shoulder down until collided by applying a voltage offset while
// disabled.
superstructure_plant_.set_power_error(0.0, -1.0, 0.0);
- while (!collided()) {
- RunIteration(false);
+ SetEnabled(false);
+ while (!superstructure_plant_.collided()) {
+ RunFor(dt());
}
- RunForTime(chrono::milliseconds(500), false); // Move a bit further down.
+ RunFor(chrono::milliseconds(500)); // Move a bit further down.
- ASSERT_TRUE(collided());
+ ASSERT_TRUE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
// Make sure that the collision avoidance will properly move the limbs out of
// the collision area.
- RunForTime(chrono::seconds(10));
- ASSERT_FALSE(collided());
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
+ ASSERT_FALSE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Make sure that we can properly detect a collision.
TEST_F(SuperstructureTest, DetectAndFixShoulderInDrivebase) {
+ SetEnabled(true);
// Zero & go straight up with the shoulder.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// Since we're explicitly checking for collisions, we don't want to fail the
// test because of collisions.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
// Move wrist up until on top of the bellypan
superstructure_plant_.set_power_error(0.0, 0.0, -1.0);
+ SetEnabled(false);
while (superstructure_plant_.wrist_angle() > -0.2) {
- RunIteration(false);
+ RunFor(dt());
}
- ASSERT_TRUE(collided());
+ ASSERT_TRUE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::LANDING_SLOW_RUNNING, superstructure_.state());
// Make sure that the collision avoidance will properly move the limbs out of
// the collision area.
superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
- RunForTime(chrono::seconds(3));
- ASSERT_FALSE(collided());
+ SetEnabled(true);
+ RunFor(chrono::seconds(3));
+ ASSERT_FALSE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
}
// Make sure that the landing voltage limit works.
TEST_F(SuperstructureTest, LandingDownVoltageLimit) {
+ SetEnabled(true);
+
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(0.0);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0) // intentionally asking for forward
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0; // intentionally asking for forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// If we are near the bottom of the range, we won't have enough power to
// compensate for the offset. This means that we fail if we get to the goal.
superstructure_plant_.set_power_error(
0.0, -Superstructure::kLandingShoulderDownVoltage, 0.0);
- RunForTime(chrono::seconds(2));
+ RunFor(chrono::seconds(2));
superstructure_plant_.set_power_error(
0.0, -2.0 * Superstructure::kLandingShoulderDownVoltage, 0.0);
- RunForTime(chrono::seconds(2));
+ RunFor(chrono::seconds(2));
+ superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_queue_.goal->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder);
}
// Make sure that we land slowly.
TEST_F(SuperstructureTest, LandSlowly) {
+ SetEnabled(true);
// Zero & go to initial position.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI * 0.25)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(8));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI * 0.25;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Wait until we hit the transition point.
do {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
} while (superstructure_plant_.shoulder_angle() >
Superstructure::kShoulderTransitionToLanded);
- set_peak_shoulder_velocity(0.55);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_shoulder_velocity(0.55);
+ RunFor(chrono::seconds(4));
}
// Make sure that we quickly take off from a land.
TEST_F(SuperstructureTest, TakeOffQuickly) {
+ SetEnabled(true);
// Zero & go to initial position.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(8));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI / 2.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI / 2.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Wait until we hit the transition point.
do {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
} while (superstructure_plant_.shoulder_angle() <
Superstructure::kShoulderTransitionToLanded);
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 435724b..00fa7dd 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -4,11 +4,14 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
- superstructure.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 64c033a..4e4ccef 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -455,9 +455,12 @@
} // namespace y2016
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2016::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
index 808f215..a78d97f 100644
--- a/y2017/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,17 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
+ &event_loop, ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
&localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 46a3af6..d1cb15f 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -117,29 +117,31 @@
// the position.
class SuperstructureSimulation {
public:
- SuperstructureSimulation()
- : hood_plant_(new HoodPlant(
+ SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<SuperstructureQueue::Position>(
+ ".y2017.control_loops.superstructure_queue.position")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2017.control_loops.superstructure_queue.status")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2017.control_loops.superstructure_queue.output")),
+ hood_plant_(new HoodPlant(
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
hood_encoder_(constants::Values::kHoodEncoderIndexDifference),
-
intake_plant_(new IntakePlant(
::y2017::control_loops::superstructure::intake::MakeIntakePlant())),
intake_pot_encoder_(constants::Values::kIntakeEncoderIndexDifference),
shooter_plant_(new ShooterPlant(::y2017::control_loops::superstructure::
shooter::MakeShooterPlant())),
-
indexer_encoder_(2.0 * M_PI),
turret_encoder_(2.0 * M_PI),
- column_plant_(new ColumnPlant(
- ::y2017::control_loops::superstructure::column::MakeColumnPlant())),
-
- superstructure_queue_(
- ".y2017.control_loops.superstructure_queue",
- ".y2017.control_loops.superstructure_queue.goal",
- ".y2017.control_loops.superstructure_queue.position",
- ".y2017.control_loops.superstructure_queue.output",
- ".y2017.control_loops.superstructure_queue.status") {
+ column_plant_(new ColumnPlant(::y2017::control_loops::superstructure::
+ column::MakeColumnPlant())) {
// Start the hood out in the middle by default.
InitializeHoodPosition((constants::Values::kHoodRange.lower +
constants::Values::kHoodRange.upper) /
@@ -157,6 +159,17 @@
InitializeIntakePosition((constants::Values::kIntakeRange.lower +
constants::Values::kIntakeRange.upper) /
2.0);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
void InitializeHoodPosition(double start_pos) {
@@ -203,8 +216,8 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
- superstructure_queue_.position.MakeMessage();
+ ::aos::Sender<SuperstructureQueue::Position>::Message position =
+ superstructure_position_sender_.MakeMessage();
hood_encoder_.GetSensorValues(&position->hood);
intake_pot_encoder_.GetSensorValues(&position->intake);
@@ -260,66 +273,70 @@
// Simulates the superstructure for a single timestep.
void Simulate() {
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
- EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+ const double last_intake_velocity = intake_velocity();
+ const double last_turret_velocity = turret_angular_velocity();
+ const double last_hood_velocity = hood_angular_velocity();
+
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
const double voltage_check_hood =
(static_cast<hood::Hood::State>(
- superstructure_queue_.status->hood.state) ==
+ superstructure_status_fetcher_->hood.state) ==
hood::Hood::State::RUNNING)
? superstructure::hood::Hood::kOperatingVoltage
: superstructure::hood::Hood::kZeroingVoltage;
const double voltage_check_indexer =
(static_cast<column::Column::State>(
- superstructure_queue_.status->turret.state) ==
+ superstructure_status_fetcher_->turret.state) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_turret =
(static_cast<column::Column::State>(
- superstructure_queue_.status->turret.state) ==
+ superstructure_status_fetcher_->turret.state) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_intake =
(static_cast<intake::Intake::State>(
- superstructure_queue_.status->intake.state) ==
+ superstructure_status_fetcher_->intake.state) ==
intake::Intake::State::RUNNING)
? superstructure::intake::Intake::kOperatingVoltage
: superstructure::intake::Intake::kZeroingVoltage;
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_hood),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_hood),
voltage_check_hood);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
voltage_check_intake);
- EXPECT_LE(::std::abs(superstructure_queue_.output->voltage_indexer),
+ EXPECT_LE(::std::abs(superstructure_output_fetcher_->voltage_indexer),
voltage_check_indexer)
<< ": check voltage " << voltage_check_indexer;
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_turret),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_turret),
voltage_check_turret);
::Eigen::Matrix<double, 1, 1> hood_U;
- hood_U << superstructure_queue_.output->voltage_hood +
+ hood_U << superstructure_output_fetcher_->voltage_hood +
hood_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_queue_.output->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> shooter_U;
- shooter_U << superstructure_queue_.output->voltage_shooter +
+ shooter_U << superstructure_output_fetcher_->voltage_shooter +
shooter_plant_->voltage_offset();
::Eigen::Matrix<double, 2, 1> column_U;
- column_U << superstructure_queue_.output->voltage_indexer +
+ column_U << superstructure_output_fetcher_->voltage_indexer +
column_plant_->indexer_voltage_offset(),
- superstructure_queue_.output->voltage_turret +
+ superstructure_output_fetcher_->voltage_turret +
column_plant_->turret_voltage_offset();
hood_plant_->Update(hood_U);
@@ -407,140 +424,28 @@
EXPECT_LE(angle_turret, constants::Values::kTurretRange.upper_hard);
EXPECT_GE(position_intake, constants::Values::kIntakeRange.lower_hard);
EXPECT_LE(position_intake, constants::Values::kIntakeRange.upper_hard);
- }
- private:
- ::std::unique_ptr<HoodPlant> hood_plant_;
- PositionSensorSimulator hood_encoder_;
+ // Check to make sure no constriants are violated.
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+ const double intake_acceleration =
+ (intake_velocity() - last_intake_velocity) / loop_time;
+ const double turret_acceleration =
+ (turret_angular_velocity() - last_turret_velocity) / loop_time;
+ const double hood_acceleration =
+ (hood_angular_velocity() - last_hood_velocity) / loop_time;
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_turret_acceleration_, turret_acceleration);
+ EXPECT_LE(-peak_turret_acceleration_, turret_acceleration);
+ EXPECT_GE(peak_hood_acceleration_, hood_acceleration);
+ EXPECT_LE(-peak_hood_acceleration_, hood_acceleration);
- ::std::unique_ptr<IntakePlant> intake_plant_;
- PositionSensorSimulator intake_pot_encoder_;
-
- ::std::unique_ptr<ShooterPlant> shooter_plant_;
-
- PositionSensorSimulator indexer_encoder_;
- PositionSensorSimulator turret_encoder_;
- ::std::unique_ptr<ColumnPlant> column_plant_;
-
- bool freeze_indexer_ = false;
- bool freeze_turret_ = false;
-
- SuperstructureQueue superstructure_queue_;
-};
-
-class SuperstructureTest : public ::aos::testing::ControlLoopTest {
- protected:
- SuperstructureTest()
- : superstructure_queue_(
- ".y2017.control_loops.superstructure_queue",
- ".y2017.control_loops.superstructure_queue.goal",
- ".y2017.control_loops.superstructure_queue.position",
- ".y2017.control_loops.superstructure_queue.output",
- ".y2017.control_loops.superstructure_queue.status"),
- superstructure_(&event_loop_) {
- set_team_id(::frc971::control_loops::testing::kTeamNumber);
- }
-
- void VerifyNearGoal() {
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.status.FetchLatest();
-
- ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr);
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
-
- EXPECT_NEAR(superstructure_queue_.goal->hood.angle,
- superstructure_queue_.status->hood.position, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->hood.angle,
- superstructure_plant_.hood_position(), 0.001);
-
- EXPECT_NEAR(superstructure_queue_.goal->turret.angle,
- superstructure_queue_.status->turret.position, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->turret.angle,
- superstructure_plant_.turret_position(), 0.001);
-
- EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
- superstructure_queue_.status->intake.position, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->intake.distance,
- superstructure_plant_.intake_position(), 0.001);
-
- // Check that the angular velocity, average angular velocity, and estimated
- // angular velocity match when we are done for the shooter.
- EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
- superstructure_queue_.status->shooter.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
- superstructure_queue_.status->shooter.avg_angular_velocity,
- 0.1);
- EXPECT_NEAR(superstructure_queue_.goal->shooter.angular_velocity,
- superstructure_plant_.shooter_velocity(), 0.1);
-
- // Check that the angular velocity, average angular velocity, and estimated
- // angular velocity match when we are done for the indexer.
- EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
- superstructure_queue_.status->indexer.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
- superstructure_queue_.status->indexer.avg_angular_velocity,
- 0.1);
- EXPECT_NEAR(superstructure_queue_.goal->indexer.angular_velocity,
- superstructure_plant_.indexer_velocity(), 0.1);
- }
-
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
-
- superstructure_plant_.SendPositionMessage();
- superstructure_.Iterate();
- superstructure_plant_.Simulate();
-
- TickTime(::std::chrono::microseconds(5050));
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for,
- bool enabled = true) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- const auto loop_start_time = monotonic_clock::now();
- double begin_intake_velocity = superstructure_plant_.intake_velocity();
- double begin_turret_velocity =
- superstructure_plant_.turret_angular_velocity();
- double begin_hood_velocity =
- superstructure_plant_.hood_angular_velocity();
-
- RunIteration(enabled);
-
- 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;
- const double turret_acceleration =
- (superstructure_plant_.turret_angular_velocity() -
- begin_turret_velocity) /
- loop_time;
- const double hood_acceleration =
- (superstructure_plant_.hood_angular_velocity() -
- begin_hood_velocity) /
- loop_time;
- EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
- EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
- EXPECT_GE(peak_turret_acceleration_, turret_acceleration);
- EXPECT_LE(-peak_turret_acceleration_, turret_acceleration);
- EXPECT_GE(peak_hood_acceleration_, hood_acceleration);
- EXPECT_LE(-peak_hood_acceleration_, hood_acceleration);
-
- EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
- EXPECT_LE(-peak_intake_velocity_,
- superstructure_plant_.intake_velocity());
- EXPECT_GE(peak_turret_velocity_,
- superstructure_plant_.turret_angular_velocity());
- EXPECT_LE(-peak_turret_velocity_,
- superstructure_plant_.turret_angular_velocity());
- EXPECT_GE(peak_hood_velocity_,
- superstructure_plant_.hood_angular_velocity());
- EXPECT_LE(-peak_hood_velocity_,
- superstructure_plant_.hood_angular_velocity());
- }
+ EXPECT_GE(peak_intake_velocity_, intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_, intake_velocity());
+ EXPECT_GE(peak_turret_velocity_, turret_angular_velocity());
+ EXPECT_LE(-peak_turret_velocity_, turret_angular_velocity());
+ EXPECT_GE(peak_hood_velocity_, hood_angular_velocity());
+ EXPECT_LE(-peak_hood_velocity_, hood_angular_velocity());
}
void set_peak_intake_acceleration(double value) {
@@ -556,17 +461,31 @@
void set_peak_turret_velocity(double value) { peak_turret_velocity_ = value; }
void set_peak_hood_velocity(double value) { peak_hood_velocity_ = value; }
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory
- // that is no longer valid.
- SuperstructureQueue superstructure_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- // Create a control loop and simulation.
- Superstructure superstructure_;
- SuperstructureSimulation superstructure_plant_;
-
private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+
+ ::std::unique_ptr<HoodPlant> hood_plant_;
+ PositionSensorSimulator hood_encoder_;
+
+ ::std::unique_ptr<IntakePlant> intake_plant_;
+ PositionSensorSimulator intake_pot_encoder_;
+
+ ::std::unique_ptr<ShooterPlant> shooter_plant_;
+
+ PositionSensorSimulator indexer_encoder_;
+ PositionSensorSimulator turret_encoder_;
+ ::std::unique_ptr<ColumnPlant> column_plant_;
+
+ bool freeze_indexer_ = false;
+ bool freeze_turret_ = false;
+
+ bool first_ = true;
+
// The acceleration limits to check for while moving.
double peak_intake_acceleration_ = 1e10;
double peak_turret_acceleration_ = 1e10;
@@ -577,27 +496,118 @@
double peak_hood_velocity_ = 1e10;
};
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
+ ".y2017.control_loops.superstructure_queue.goal")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
+ ".y2017.control_loops.superstructure_queue.goal")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2017.control_loops.superstructure_queue.status")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2017.control_loops.superstructure_queue.output")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
+ ".y2017.control_loops.superstructure_queue.position")),
+ superstructure_event_loop_(MakeEventLoop()),
+ superstructure_(superstructure_event_loop_.get()),
+ superstructure_plant_event_loop_(MakeEventLoop()),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
+ superstructure_status_fetcher_->hood.position, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
+ superstructure_plant_.hood_position(), 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
+ superstructure_status_fetcher_->turret.position, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
+ superstructure_plant_.turret_position(), 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
+ superstructure_status_fetcher_->intake.position, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
+ superstructure_plant_.intake_position(), 0.001);
+
+ // Check that the angular velocity, average angular velocity, and estimated
+ // angular velocity match when we are done for the shooter.
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
+ superstructure_status_fetcher_->shooter.angular_velocity, 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
+ superstructure_status_fetcher_->shooter.avg_angular_velocity,
+ 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
+ superstructure_plant_.shooter_velocity(), 0.1);
+
+ // Check that the angular velocity, average angular velocity, and estimated
+ // angular velocity match when we are done for the indexer.
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
+ superstructure_status_fetcher_->indexer.angular_velocity, 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
+ superstructure_status_fetcher_->indexer.avg_angular_velocity,
+ 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
+ superstructure_plant_.indexer_velocity(), 0.1);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+
+ ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Position>
+ superstructure_position_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
+ Superstructure superstructure_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
+};
+
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
+ SetEnabled(true);
+
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = 0.2;
goal->turret.angle = 0.0;
goal->intake.distance = 0.05;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
}
// Tests that the hood, turret and intake loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
+
// Set a reasonable goal.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = 0.1;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
@@ -614,7 +624,7 @@
}
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -624,15 +634,17 @@
//
// We are going to disable collision detection to make this easier to implement.
TEST_F(SuperstructureTest, SaturationTest) {
+ SetEnabled(true);
+
// Zero it before we move.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.distance = constants::Values::kIntakeRange.upper;
goal->turret.angle = constants::Values::kTurretRange.upper;
goal->hood.angle = constants::Values::kHoodRange.upper;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
superstructure_.set_ignore_collisions(true);
@@ -640,7 +652,7 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.distance = constants::Values::kIntakeRange.lower;
goal->intake.profile_params.max_velocity = 20.0;
goal->intake.profile_params.max_acceleration = 0.1;
@@ -652,19 +664,19 @@
goal->hood.profile_params.max_acceleration = 1.0;
ASSERT_TRUE(goal.Send());
}
- set_peak_intake_velocity(23.0);
- set_peak_turret_velocity(23.0);
- set_peak_hood_velocity(23.0);
- set_peak_intake_acceleration(0.2);
- set_peak_turret_acceleration(1.1);
- set_peak_hood_acceleration(1.1);
+ superstructure_plant_.set_peak_intake_velocity(23.0);
+ superstructure_plant_.set_peak_turret_velocity(23.0);
+ superstructure_plant_.set_peak_hood_velocity(23.0);
+ superstructure_plant_.set_peak_intake_acceleration(0.2);
+ superstructure_plant_.set_peak_turret_acceleration(1.1);
+ superstructure_plant_.set_peak_hood_acceleration(1.1);
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.distance = constants::Values::kIntakeRange.upper;
goal->intake.profile_params.max_velocity = 0.1;
goal->intake.profile_params.max_acceleration = 100;
@@ -677,13 +689,13 @@
ASSERT_TRUE(goal.Send());
}
- set_peak_intake_velocity(0.2);
- set_peak_turret_velocity(1.1);
- set_peak_hood_velocity(1.1);
- set_peak_intake_acceleration(103);
- set_peak_turret_acceleration(103);
- set_peak_hood_acceleration(103);
- RunForTime(chrono::seconds(8));
+ superstructure_plant_.set_peak_intake_velocity(0.2);
+ superstructure_plant_.set_peak_turret_velocity(1.1);
+ superstructure_plant_.set_peak_hood_velocity(1.1);
+ superstructure_plant_.set_peak_intake_acceleration(103);
+ superstructure_plant_.set_peak_turret_acceleration(103);
+ superstructure_plant_.set_peak_hood_acceleration(103);
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -691,9 +703,11 @@
// Tests that the hood, turret and intake loops doesn't try and go beyond the
// physical range of the mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
+ SetEnabled(true);
+
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = 100.0;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
@@ -708,22 +722,22 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.upper,
- superstructure_queue_.status->hood.position, 0.001);
+ superstructure_status_fetcher_->hood.position, 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.upper,
- superstructure_queue_.status->turret.position, 0.001);
+ superstructure_status_fetcher_->turret.position, 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_queue_.status->intake.position, 0.001);
+ superstructure_status_fetcher_->intake.position, 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = -100.0;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
@@ -739,23 +753,23 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_queue_.status->hood.position, 0.001);
+ superstructure_status_fetcher_->hood.position, 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.lower,
- superstructure_queue_.status->turret.position, 0.001);
+ superstructure_status_fetcher_->turret.position, 0.001);
EXPECT_NEAR(column::Column::kIntakeZeroingMinDistance,
- superstructure_queue_.status->intake.position, 0.001);
+ superstructure_status_fetcher_->intake.position, 0.001);
// Now, center the turret so we can try ridiculous things without having the
// intake pushed out.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = -100.0;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
@@ -771,23 +785,25 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_queue_.status->hood.position, 0.001);
+ superstructure_status_fetcher_->hood.position, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->turret.position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->turret.position, 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_queue_.status->intake.position, 0.001);
+ superstructure_status_fetcher_->intake.position, 0.001);
}
// Tests that the hood, turret and intake loops zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
+ SetEnabled(true);
+
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
goal->hood.profile_params.max_velocity = 1;
goal->hood.profile_params.max_acceleration = 0.5;
@@ -802,14 +818,15 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
@@ -817,6 +834,7 @@
}
TEST_F(SuperstructureTest, LowerHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.lower_hard);
@@ -826,19 +844,20 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower_hard);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
goal->turret.angle = constants::Values::kTurretRange.lower;
goal->intake.distance = constants::Values::kIntakeRange.upper;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.upper_hard);
@@ -848,19 +867,20 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper_hard);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.upper;
goal->turret.angle = constants::Values::kTurretRange.upper;
goal->intake.distance = constants::Values::kIntakeRange.upper;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
+ SetEnabled(true);
superstructure_plant_.InitializeHoodPosition(
constants::Values::kHoodRange.upper);
@@ -869,14 +889,14 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.upper - 0.1;
goal->turret.angle = constants::Values::kTurretRange.upper - 0.1;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.1;
goal->indexer.angular_velocity = -5.0;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
@@ -884,7 +904,7 @@
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
EXPECT_EQ(hood::Hood::State::ZEROING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::UNINITIALIZED,
@@ -892,7 +912,7 @@
EXPECT_EQ(column::Column::State::ZEROING_UNINITIALIZED,
superstructure_.column().state());
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
@@ -902,22 +922,26 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder().Send());
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
+ ASSERT_TRUE(goal.Send());
+ }
+ {
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::milliseconds(100), false);
+ RunFor(chrono::milliseconds(100));
EXPECT_EQ(0.0, superstructure_.hood().goal(0, 0));
EXPECT_EQ(0.0, superstructure_.intake().goal(0, 0));
EXPECT_EQ(0.0, superstructure_.column().goal(2, 0));
// Now make sure they move correctly
- RunForTime(chrono::seconds(4), true);
+ SetEnabled(true);
+ RunFor(chrono::seconds(4));
EXPECT_NE(0.0, superstructure_.hood().goal(0, 0));
EXPECT_NE(0.0, superstructure_.intake().goal(0, 0));
EXPECT_NE(0.0, superstructure_.column().goal(2, 0));
@@ -931,7 +955,7 @@
constants::GetValues().hood.zeroing.measured_index_position - 0.001);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower;
goal->turret.angle = 0.0;
goal->intake.distance = constants::Values::kIntakeRange.lower;
@@ -939,7 +963,7 @@
}
// Run disabled for 2 seconds
- RunForTime(chrono::seconds(2), false);
+ RunFor(chrono::seconds(2));
EXPECT_EQ(hood::Hood::State::DISABLED_INITIALIZED,
superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
@@ -951,9 +975,9 @@
superstructure_plant_.set_turret_voltage_offset(-1.0);
superstructure_plant_.set_indexer_voltage_offset(2.0);
- RunForTime(chrono::seconds(1), false);
+ RunFor(chrono::seconds(1));
superstructure_plant_.set_hood_voltage_offset(0.0);
- RunForTime(chrono::seconds(5), false);
+ RunFor(chrono::seconds(5));
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
@@ -961,7 +985,8 @@
superstructure_plant_.set_turret_voltage_offset(0.0);
superstructure_plant_.set_indexer_voltage_offset(0.0);
- RunForTime(chrono::seconds(4), true);
+ SetEnabled(true);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -971,9 +996,11 @@
// Tests that the shooter spins up to speed and that it then spins down
// without applying any power.
TEST_F(SuperstructureTest, ShooterSpinUpAndDown) {
+ SetEnabled(true);
+
// Spin up.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
@@ -982,11 +1009,11 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_queue_.status->shooter.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->shooter.ready);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
@@ -996,19 +1023,20 @@
}
// Make sure we don't apply voltage on spin-down.
- RunIteration();
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
- EXPECT_EQ(0.0, superstructure_queue_.output->voltage_shooter);
+ RunFor(dt());
+
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
// Continue to stop.
- RunForTime(chrono::seconds(5));
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
- EXPECT_EQ(0.0, superstructure_queue_.output->voltage_shooter);
+ RunFor(chrono::seconds(5));
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(SuperstructureTest, ShooterDisabled) {
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
@@ -1016,19 +1044,22 @@
goal->indexer.angular_velocity = 20.0;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5), false);
- EXPECT_EQ(nullptr, superstructure_queue_.output.get());
+ RunFor(chrono::seconds(5));
+ EXPECT_EQ(nullptr, superstructure_output_fetcher_.get());
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Tests that when the indexer gets stuck, it detects it and unjams.
TEST_F(SuperstructureTest, StuckIndexerTest) {
+ SetEnabled(true);
+
// Spin up.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
@@ -1037,44 +1068,44 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_queue_.status->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
// Now, stick it.
- const auto stuck_start_time = monotonic_clock::now();
+ const auto stuck_start_time = monotonic_now();
superstructure_plant_.set_freeze_indexer(true);
- while (monotonic_clock::now() < stuck_start_time + chrono::seconds(1)) {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ while (monotonic_now() < stuck_start_time + chrono::seconds(1)) {
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_queue_.status->indexer.state) ==
+ superstructure_status_fetcher_->indexer.state) ==
column::Column::IndexerState::REVERSING) {
break;
}
}
// Make sure it detected it reasonably fast.
- const auto stuck_detection_time = monotonic_clock::now();
+ const auto stuck_detection_time = monotonic_now();
EXPECT_TRUE(stuck_detection_time - stuck_start_time <
chrono::milliseconds(200));
// Grab the position we were stuck at.
- superstructure_queue_.position.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
+ superstructure_position_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double indexer_position =
- superstructure_queue_.position->column.indexer.encoder;
+ superstructure_position_fetcher_->column.indexer.encoder;
// Now, unstick it.
superstructure_plant_.set_freeze_indexer(false);
- const auto unstuck_start_time = monotonic_clock::now();
- while (monotonic_clock::now() < unstuck_start_time + chrono::seconds(1)) {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ const auto unstuck_start_time = monotonic_now();
+ while (monotonic_now() < unstuck_start_time + chrono::seconds(1)) {
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_queue_.status->indexer.state) ==
+ superstructure_status_fetcher_->indexer.state) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1082,30 +1113,32 @@
// Make sure it took some time, but not too much to detect us not being stuck
// anymore.
- const auto unstuck_detection_time = monotonic_clock::now();
+ const auto unstuck_detection_time = monotonic_now();
EXPECT_TRUE(unstuck_detection_time - unstuck_start_time <
chrono::milliseconds(600));
EXPECT_TRUE(unstuck_detection_time - unstuck_start_time >
chrono::milliseconds(100));
// Verify that it actually moved.
- superstructure_queue_.position.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.position.get() != nullptr);
+ superstructure_position_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double unstuck_indexer_position =
- superstructure_queue_.position->column.indexer.encoder;
+ superstructure_position_fetcher_->column.indexer.encoder;
EXPECT_LT(unstuck_indexer_position, indexer_position - 0.1);
// Now, verify that everything works as expected.
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Tests that when the indexer gets stuck forever, it switches back and forth at
// a reasonable rate.
TEST_F(SuperstructureTest, ReallyStuckIndexerTest) {
+ SetEnabled(true);
+
// Spin up.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
@@ -1114,37 +1147,37 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_queue_.status->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
// Now, stick it.
- const auto stuck_start_time = monotonic_clock::now();
+ const auto stuck_start_time = monotonic_now();
superstructure_plant_.set_freeze_indexer(true);
- while (monotonic_clock::now() < stuck_start_time + chrono::seconds(1)) {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ while (monotonic_now() < stuck_start_time + chrono::seconds(1)) {
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_queue_.status->indexer.state) ==
+ superstructure_status_fetcher_->indexer.state) ==
column::Column::IndexerState::REVERSING) {
break;
}
}
// Make sure it detected it reasonably fast.
- const auto stuck_detection_time = monotonic_clock::now();
+ const auto stuck_detection_time = monotonic_now();
EXPECT_TRUE(stuck_detection_time - stuck_start_time <
chrono::milliseconds(200));
// Now, try to unstick it.
- const auto unstuck_start_time = monotonic_clock::now();
- while (monotonic_clock::now() < unstuck_start_time + chrono::seconds(1)) {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ const auto unstuck_start_time = monotonic_now();
+ while (monotonic_now() < unstuck_start_time + chrono::seconds(1)) {
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_queue_.status->indexer.state) ==
+ superstructure_status_fetcher_->indexer.state) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1152,7 +1185,7 @@
// Make sure it took some time, but not too much to detect us not being stuck
// anymore.
- const auto unstuck_detection_time = monotonic_clock::now();
+ const auto unstuck_detection_time = monotonic_now();
EXPECT_TRUE(unstuck_detection_time - unstuck_start_time <
chrono::milliseconds(1050));
EXPECT_TRUE(unstuck_detection_time - unstuck_start_time >
@@ -1162,21 +1195,21 @@
(unstuck_detection_time - unstuck_start_time).count() / 1000000));
// Now, make sure it transitions to stuck again after a delay.
- const auto restuck_start_time = monotonic_clock::now();
+ const auto restuck_start_time = monotonic_now();
superstructure_plant_.set_freeze_indexer(true);
- while (monotonic_clock::now() < restuck_start_time + chrono::seconds(1)) {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ while (monotonic_now() < restuck_start_time + chrono::seconds(1)) {
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_queue_.status->indexer.state) ==
+ superstructure_status_fetcher_->indexer.state) ==
column::Column::IndexerState::REVERSING) {
break;
}
}
// Make sure it detected it reasonably fast.
- const auto restuck_detection_time = monotonic_clock::now();
+ const auto restuck_detection_time = monotonic_now();
EXPECT_TRUE(restuck_detection_time - restuck_start_time >
chrono::milliseconds(400));
EXPECT_TRUE(restuck_detection_time - restuck_start_time <
diff --git a/y2017/control_loops/superstructure/superstructure_main.cc b/y2017/control_loops/superstructure/superstructure_main.cc
index 6ebf91a..1dbb342 100644
--- a/y2017/control_loops/superstructure/superstructure_main.cc
+++ b/y2017/control_loops/superstructure/superstructure_main.cc
@@ -4,11 +4,14 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2017::control_loops::superstructure::Superstructure superstructure(
&event_loop);
- superstructure.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 79d5c12..a5470d4 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -274,9 +274,12 @@
} // namespace y2017
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2017::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index 038497a..1bb1dc0 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,17 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
+ &event_loop, ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
&localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 1a2aa5e..5dabd1b 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -41,6 +41,7 @@
srcs = [
"superstructure_lib_test.cc",
],
+ shard_count = 5,
deps = [
":superstructure_lib",
":superstructure_queue",
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 202c428..1d4cf5d 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -187,8 +187,9 @@
class SuperstructureSimulation {
public:
- SuperstructureSimulation()
- : left_intake_(::y2018::control_loops::superstructure::intake::
+ SuperstructureSimulation(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ left_intake_(::y2018::control_loops::superstructure::intake::
MakeDelayedIntakePlant(),
constants::GetValues().left_intake.zeroing),
right_intake_(::y2018::control_loops::superstructure::intake::
@@ -196,17 +197,32 @@
constants::GetValues().right_intake.zeroing),
arm_(constants::GetValues().arm_proximal.zeroing,
constants::GetValues().arm_distal.zeroing),
- superstructure_queue_(".y2018.control_loops.superstructure",
- ".y2018.control_loops.superstructure.goal",
- ".y2018.control_loops.superstructure.output",
- ".y2018.control_loops.superstructure.status",
- ".y2018.control_loops.superstructure.position") {
+ superstructure_position_sender_(
+ event_loop_->MakeSender<SuperstructureQueue::Position>(
+ ".y2018.control_loops.superstructure.position")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2018.control_loops.superstructure.status")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2018.control_loops.superstructure.output")) {
// Start the intake out in the middle by default.
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
2.0);
InitializeArmPosition(arm::UpPoint());
+
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ ::std::chrono::microseconds(5050));
}
void InitializeIntakePosition(double start_pos) {
@@ -219,8 +235,7 @@
}
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
- superstructure_queue_.position.MakeMessage();
+ auto position = superstructure_position_sender_.MakeMessage();
left_intake_.GetSensorValues(&position->left_intake);
right_intake_.GetSensorValues(&position->right_intake);
@@ -245,110 +260,116 @@
// Simulates the intake for a single timestep.
void Simulate() {
- ASSERT_TRUE(superstructure_queue_.output.FetchLatest());
- ASSERT_TRUE(superstructure_queue_.status.FetchLatest());
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- left_intake_.Simulate(superstructure_queue_.output->left_intake);
- right_intake_.Simulate(superstructure_queue_.output->right_intake);
+ left_intake_.Simulate(superstructure_output_fetcher_->left_intake);
+ right_intake_.Simulate(superstructure_output_fetcher_->right_intake);
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
- << superstructure_queue_.output->voltage_proximal,
- superstructure_queue_.output->voltage_distal)
+ << superstructure_output_fetcher_->voltage_proximal,
+ superstructure_output_fetcher_->voltage_distal)
.finished(),
- superstructure_queue_.output->release_arm_brake);
+ superstructure_output_fetcher_->release_arm_brake);
}
private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
IntakeSideSimulation left_intake_;
IntakeSideSimulation right_intake_;
ArmSimulation arm_;
- SuperstructureQueue superstructure_queue_;
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
};
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : superstructure_queue_(".y2018.control_loops.superstructure",
- ".y2018.control_loops.superstructure.goal",
- ".y2018.control_loops.superstructure.output",
- ".y2018.control_loops.superstructure.status",
- ".y2018.control_loops.superstructure.position"),
- superstructure_(&event_loop_, ".y2018.control_loops.superstructure") {
+ : ::aos::testing::ControlLoopTest(::std::chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
+ ".y2018.control_loops.superstructure.goal")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
+ ".y2018.control_loops.superstructure.goal")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2018.control_loops.superstructure.status")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2018.control_loops.superstructure.output")),
+ superstructure_event_loop_(MakeEventLoop()),
+ superstructure_(superstructure_event_loop_.get(),
+ ".y2018.control_loops.superstructure"),
+ superstructure_plant_event_loop_(MakeEventLoop()),
+ superstructure_plant_(superstructure_plant_event_loop_.get()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
void VerifyNearGoal() {
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.status.FetchLatest();
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
- ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr) << ": No goal";
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// Left side test.
- EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
- superstructure_queue_.status->left_intake.spring_position +
- superstructure_queue_.status->left_intake.motor_position,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
+ superstructure_status_fetcher_->left_intake.spring_position +
+ superstructure_status_fetcher_->left_intake.motor_position,
0.001);
- EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
- EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
- superstructure_queue_.status->right_intake.spring_position +
- superstructure_queue_.status->right_intake.motor_position,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
+ superstructure_status_fetcher_->right_intake.spring_position +
+ superstructure_status_fetcher_->right_intake.motor_position,
0.001);
- EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
superstructure_plant_.right_intake().spring_position(), 0.001);
}
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- superstructure_plant_.SendPositionMessage();
- superstructure_.Iterate();
- superstructure_plant_.Simulate();
-
- TickTime(::std::chrono::microseconds(5050));
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for,
- bool enabled = true) {
- const auto end_time = monotonic_clock::now() + run_for;
- while (monotonic_clock::now() < end_time) {
- RunIteration(enabled);
- }
- }
-
- ::aos::ShmEventLoop event_loop_;
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory
- // that is no longer valid.
- SuperstructureQueue superstructure_queue_;
+ ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
// Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
Superstructure superstructure_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
SuperstructureSimulation superstructure_plant_;
};
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoalAndDoesNothing) {
- RunForTime(chrono::seconds(2));
- superstructure_queue_.output.FetchLatest();
+ SetEnabled(true);
+ RunFor(chrono::seconds(2));
+ superstructure_output_fetcher_.Fetch();
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_right().state());
- EXPECT_EQ(superstructure_queue_.output->left_intake.voltage_elastic, 0.0);
- EXPECT_EQ(superstructure_queue_.output->right_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->left_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->right_intake.voltage_elastic, 0.0);
}
// Tests that the intake loop can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
// Set a reasonable goal.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 0.1;
goal->intake.right_intake_angle = 0.2;
@@ -359,7 +380,7 @@
}
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -367,11 +388,12 @@
// Tests that the intake loop can reach a goal after starting at a non-zero
// position.
TEST_F(SuperstructureTest, OffsetStartReachesGoal) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(0.5);
// Set a reasonable goal.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 0.1;
goal->intake.right_intake_angle = 0.2;
@@ -382,7 +404,7 @@
}
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -390,9 +412,10 @@
// Tests that the intake loops doesn't try and go beyond the
// physical range of the mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
+ SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 5.0 * M_PI;
goal->intake.right_intake_angle = 5.0 * M_PI;
@@ -401,27 +424,27 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->left_intake.spring_position +
- superstructure_queue_.status->left_intake.motor_position,
+ superstructure_status_fetcher_->left_intake.spring_position +
+ superstructure_status_fetcher_->left_intake.motor_position,
0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_queue_.status->right_intake.motor_position,
+ superstructure_status_fetcher_->right_intake.motor_position,
0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = -5.0 * M_PI;
goal->intake.right_intake_angle = -5.0 * M_PI;
@@ -431,67 +454,70 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->left_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->left_intake.spring_position,
+ superstructure_status_fetcher_->left_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_queue_.status->right_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->right_intake.spring_position,
+ superstructure_status_fetcher_->right_intake.motor_position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
0.001);
}
TEST_F(SuperstructureTest, DISABLED_LowerHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().lower_hard);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
goal->arm_goal_position = arm::UpIndex();
goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 1.0;
goal->intake.right_intake_angle = 1.0;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, DISABLED_UpperHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper_hard);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
goal->arm_goal_position = arm::UpIndex();
goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle =
constants::Values::kIntakeRange().upper - 0.1;
goal->intake.right_intake_angle =
@@ -500,7 +526,7 @@
goal->open_claw = true;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_left().state());
@@ -509,14 +535,14 @@
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
EXPECT_EQ(intake::IntakeSide::State::ZEROING,
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::ZEROING,
superstructure_.intake_right().state());
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_left().state());
@@ -528,10 +554,11 @@
// Tests that we don't freak out without a goal.
TEST_F(SuperstructureTest, ArmSimpleGoal) {
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = -0.8;
goal->intake.right_intake_angle = -0.8;
goal->arm_goal_position = arm::FrontHighBoxIndex();
@@ -544,20 +571,22 @@
// Tests that we can can execute a move.
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
+ SetEnabled(true);
+ {
+ auto goal = superstructure_goal_sender_.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = arm::FrontHighBoxIndex();
+ goal->open_claw = true;
+ ASSERT_TRUE(goal.Send());
+ }
- auto goal = superstructure_queue_.goal.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
-
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::ReadyAboveBoxIndex();
@@ -565,27 +594,30 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that we can can execute a move which moves through multiple nodes.
TEST_F(SuperstructureTest, ArmMultistepMove) {
+ SetEnabled(true);
superstructure_plant_.InitializeArmPosition(arm::ReadyAboveBoxPoint());
- auto goal = superstructure_queue_.goal.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::BackLowBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ {
+ auto goal = superstructure_goal_sender_.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = arm::BackLowBoxIndex();
+ goal->open_claw = true;
+ ASSERT_TRUE(goal.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->intake.left_intake_angle = 0.0;
goal->intake.right_intake_angle = 0.0;
goal->arm_goal_position = arm::ReadyAboveBoxIndex();
@@ -593,7 +625,7 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
index f5b026c..789f13f 100644
--- a/y2018/control_loops/superstructure/superstructure_main.cc
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -5,11 +5,13 @@
int main() {
::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2018::control_loops::superstructure::Superstructure superstructure(
&event_loop);
- ::aos::GoRT();
- superstructure.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index d819c6f..b4c4efa 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -373,9 +373,12 @@
} // namespace y2018
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2018::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}
diff --git a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
index c768dc9..095aaf9 100644
--- a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -7,14 +7,18 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ &event_loop,
::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
&event_loop, &localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 2e16e1d..9b9ba30 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -180,7 +180,7 @@
srcs = ["target_selector_test.cc"],
deps = [
":target_selector",
- "//aos/events:shm-event-loop",
+ "//aos/events:simulated_event_loop",
"//aos/testing:googletest",
"//aos/testing:test_shm",
],
@@ -210,6 +210,7 @@
"arm": [],
}),
linkstatic = True,
+ shard_count = 5,
deps = [
":localizer",
":drivetrain_base",
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index 63da6c9..ada8f53 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -8,14 +8,17 @@
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2019::control_loops::drivetrain::EventLoopLocalizer localizer(
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ &event_loop, ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
&localizer);
- drivetrain.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 6dd8f31..94ab9ea 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -25,16 +25,26 @@
}
EventLoopLocalizer::EventLoopLocalizer(
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
- dt_config,
- ::aos::EventLoop *event_loop)
+ ::aos::EventLoop *event_loop,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
: event_loop_(event_loop),
cameras_(MakeCameras(&robot_pose_)),
localizer_(dt_config, &robot_pose_),
target_selector_(event_loop) {
- localizer_.ResetInitialState(::aos::monotonic_clock::now(),
- Localizer::State::Zero(), localizer_.P());
- ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0, true);
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+ localizer_.ResetInitialState(monotonic_now, Localizer::State::Zero(),
+ localizer_.P());
+ ResetPosition(monotonic_now, 0.5, 3.4, 0.0, 0.0, true);
+ event_loop->OnRun([this]() {
+ // Reset time now that we have an official start time. Reset the states to
+ // their current state since nothing will have moved.
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+ localizer_.ResetInitialState(monotonic_now, localizer_.X_hat(),
+ localizer_.P());
+ });
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 183e817..dac614b 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -29,9 +29,9 @@
typedef typename Localizer::Pose Pose;
EventLoopLocalizer(
+ ::aos::EventLoop *event_loop,
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config,
- ::aos::EventLoop *event_loop);
+ &dt_config);
void Reset(::aos::monotonic_clock::time_point t,
const Localizer::State &state, double theta_uncertainty);
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 9137402..ad7223e 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -42,73 +42,76 @@
// We must use the 2019 drivetrain config so that we don't have to deal
// with shifting:
LocalizedDrivetrainTest()
- : dt_config_(GetTest2019DrivetrainConfig()),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
- camera_queue_(".y2019.control_loops.drivetrain.camera_frames"),
- localizer_(dt_config_, &event_loop_),
- drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
- drivetrain_motor_plant_(&simulation_event_loop_, dt_config_),
+ : ::aos::testing::ControlLoopTest(GetTest2019DrivetrainConfig().dt),
+ test_event_loop_(MakeEventLoop()),
+ drivetrain_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_goal_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ localizer_control_sender_(
+ test_event_loop_->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ ".frc971.control_loops.drivetrain.localizer_control")),
+
+ drivetrain_event_loop_(MakeEventLoop()),
+ dt_config_(GetTest2019DrivetrainConfig()),
+ camera_sender_(test_event_loop_->MakeSender<CameraFrame>(
+ ".y2019.control_loops.drivetrain.camera_frames")),
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+
+ drivetrain_plant_event_loop_(MakeEventLoop()),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
cameras_(MakeCameras(&robot_pose_)),
- last_frame_(monotonic_clock::now()) {
+ last_frame_(monotonic_now()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
// Because 0, 0 is on the top level of the Hab and right on the edge of an
// obstacle, it ends up confusing the localizer; as such, starting there
// is just prone to causing confusion.
SetStartingPosition({3.0, 2.0, 0.0});
set_battery_voltage(12.0);
+
+ test_event_loop_->MakeWatcher(
+ ".frc971.control_loops.drivetrain_queue.status",
+ [this](const ::frc971::control_loops::DrivetrainQueue::Status &) {
+ // Needs to do camera updates right after we run the control loop.
+ if (enable_cameras_) {
+ SendDelayedFrames();
+ if (last_frame_ + ::std::chrono::milliseconds(33) <
+ monotonic_now()) {
+ CaptureFrames();
+ last_frame_ = monotonic_now();
+ }
+ }
+ });
}
+ virtual ~LocalizedDrivetrainTest() {}
+
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
- *drivetrain_motor_plant_.mutable_state() << xytheta.x(), xytheta.y(),
+ *drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
Eigen::Matrix<double, EventLoopLocalizer::Localizer::kNStates, 1>
localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
- localizer_.Reset(monotonic_clock::now(), localizer_state, 0.0);
- }
-
- void RunIteration() {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- if (enable_cameras_) {
- SendDelayedFrames();
- if (last_frame_ + ::std::chrono::milliseconds(33) <
- monotonic_clock::now()) {
- CaptureFrames();
- last_frame_ = monotonic_clock::now();
- }
- }
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true, dt_config_.dt);
- }
-
- void RunForTime(monotonic_clock::duration run_for) {
- const auto end_time = monotonic_clock::now() + run_for;
- while (monotonic_clock::now() < end_time) {
- RunIteration();
- }
- // Let the estimator catch up with the final simulation step:
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
+ localizer_.Reset(monotonic_now(), localizer_state, 0.0);
}
void VerifyNearGoal() {
- my_drivetrain_queue_.goal.FetchLatest();
- my_drivetrain_queue_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
- drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
- drivetrain_motor_plant_.GetRightPosition(), 1e-3);
+ drivetrain_goal_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ drivetrain_plant_.GetLeftPosition(), 1e-3);
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ drivetrain_plant_.GetRightPosition(), 1e-3);
}
void VerifyEstimatorAccurate(double eps) {
- const Eigen::Matrix<double, 5, 1> true_state =
- drivetrain_motor_plant_.state();
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
@@ -117,14 +120,14 @@
}
void CaptureFrames() {
- *robot_pose_.mutable_pos() << drivetrain_motor_plant_.GetPosition().x(),
- drivetrain_motor_plant_.GetPosition().y(), 0.0;
- robot_pose_.set_theta(drivetrain_motor_plant_.state()(2, 0));
+ *robot_pose_.mutable_pos() << drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.0;
+ robot_pose_.set_theta(drivetrain_plant_.state()(2, 0));
for (size_t ii = 0; ii < cameras_.size(); ++ii) {
const auto target_views = cameras_[ii].target_views();
CameraFrame frame;
frame.timestamp = chrono::duration_cast<chrono::nanoseconds>(
- monotonic_clock::now().time_since_epoch())
+ monotonic_now().time_since_epoch())
.count();
frame.camera = ii;
frame.num_targets = 0;
@@ -147,7 +150,7 @@
frame.targets[jj].height = view.reading.height;
}
}
- camera_delay_queue_.emplace(monotonic_clock::now(), frame);
+ camera_delay_queue_.emplace(monotonic_now(), frame);
}
}
@@ -155,28 +158,33 @@
const ::std::chrono::milliseconds camera_latency(50);
while (!camera_delay_queue_.empty() &&
::std::get<0>(camera_delay_queue_.front()) <
- monotonic_clock::now() - camera_latency) {
- ::aos::ScopedMessagePtr<CameraFrame> message =
- camera_queue_.MakeMessage();
+ monotonic_now() - camera_latency) {
+ auto message = camera_sender_.MakeMessage();
*message = ::std::get<1>(camera_delay_queue_.front());
ASSERT_TRUE(message.Send());
camera_delay_queue_.pop();
}
}
- virtual ~LocalizedDrivetrainTest() {}
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
dt_config_;
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
- ::aos::Queue<CameraFrame> camera_queue_;
- ::aos::ShmEventLoop event_loop_;
- ::aos::ShmEventLoop simulation_event_loop_;
+ ::aos::Sender<CameraFrame> camera_sender_;
EventLoopLocalizer localizer_;
- DrivetrainLoop drivetrain_motor_;
- DrivetrainSimulation drivetrain_motor_plant_;
+ DrivetrainLoop drivetrain_;
+
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
+ DrivetrainSimulation drivetrain_plant_;
EventLoopLocalizer::Pose robot_pose_;
const ::std::array<EventLoopLocalizer::Camera, constants::Values::kNumCameras>
cameras_;
@@ -198,40 +206,50 @@
// Tests that no camera updates, combined with a perfect model, results in no
// error.
TEST_F(LocalizedDrivetrainTest, NoCameraUpdate) {
+ SetEnabled(true);
set_enable_cameras(false);
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(3));
+ VerifyEstimatorAccurate(1e-10);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
VerifyEstimatorAccurate(1e-10);
}
// Bad camera updates (NaNs) should have no effect.
TEST_F(LocalizedDrivetrainTest, BadCameraUpdate) {
+ SetEnabled(true);
set_enable_cameras(true);
set_bad_frames(true);
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(3));
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
VerifyEstimatorAccurate(1e-10);
}
// Tests that camera udpates with a perfect models results in no errors.
TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
+ SetEnabled(true);
set_enable_cameras(true);
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(3));
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
VerifyEstimatorAccurate(1e-7);
}
@@ -239,18 +257,21 @@
// Tests that not having cameras with an initial disturbance results in
// estimator error. This is to test the test.
TEST_F(LocalizedDrivetrainTest, NoCameraWithDisturbanceFails) {
+ SetEnabled(true);
set_enable_cameras(false);
- (*drivetrain_motor_plant_.mutable_state())(0, 0) += 0.05;
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(3));
+ (*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(3));
// VerifyNearGoal succeeds because it is just checking wheel positions:
VerifyNearGoal();
const Eigen::Matrix<double, 5, 1> true_state =
- drivetrain_motor_plant_.state();
+ drivetrain_plant_.state();
// Everything but X-value should be correct:
EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
@@ -262,21 +283,25 @@
// Tests that when we reset the position of the localizer via the queue to
// compensate for the disturbance, the estimator behaves perfectly.
TEST_F(LocalizedDrivetrainTest, ResetLocalizer) {
+ SetEnabled(true);
set_enable_cameras(false);
- (*drivetrain_motor_plant_.mutable_state())(0, 0) += 0.05;
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- ::aos::Queue<::frc971::control_loops::drivetrain::LocalizerControl>
- localizer_queue(".frc971.control_loops.drivetrain.localizer_control");
- ASSERT_TRUE(localizer_queue.MakeWithBuilder()
- .x(drivetrain_motor_plant_.state().x())
- .y(drivetrain_motor_plant_.state().y())
- .theta(drivetrain_motor_plant_.state()(2, 0))
- .Send());
- RunForTime(chrono::seconds(3));
+ (*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+
+ {
+ auto message = localizer_control_sender_.MakeMessage();
+ message->x = drivetrain_plant_.state().x();
+ message->y = drivetrain_plant_.state().y();
+ message->theta = drivetrain_plant_.state()(2, 0);
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
VerifyEstimatorAccurate(1e-5);
}
@@ -284,15 +309,18 @@
// Tests that, when a small error in X is introduced, the camera corrections do
// correct for it.
TEST_F(LocalizedDrivetrainTest, CameraUpdate) {
+ SetEnabled(true);
set_enable_cameras(true);
SetStartingPosition({4.0, 0.5, 0.0});
- (*drivetrain_motor_plant_.mutable_state())(0, 0) += 0.05;
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(5));
+ (*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
VerifyEstimatorAccurate(5e-3);
}
@@ -304,23 +332,25 @@
// Tests that using the line following drivetrain and just driving straight
// forward from roughly the right spot gets us to the HP slot.
TEST_F(LocalizedDrivetrainTest, LineFollowToHPSlot) {
+ SetEnabled(true);
set_enable_cameras(false);
SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(3)
- .throttle(0.5)
- .Send();
- RunForTime(chrono::seconds(6));
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 3;
+ message->throttle = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(6));
VerifyEstimatorAccurate(1e-8);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
// the target exactly. Instead, just run slightly past the target:
- EXPECT_LT(::std::abs(::aos::math::DiffAngle(
- M_PI, drivetrain_motor_plant_.state()(2, 0))),
- 1e-5);
- EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_motor_plant_.state().x());
- EXPECT_NEAR(HPSlotLeft().abs_pos().y(),
- drivetrain_motor_plant_.state().y(), 1e-4);
+ EXPECT_LT(
+ ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
+ 1e-5);
+ EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
+ EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 1e-4);
}
} // namespace testing
diff --git a/y2019/control_loops/drivetrain/replay_localizer.cc b/y2019/control_loops/drivetrain/replay_localizer.cc
index d5eb661..ce0ac09 100644
--- a/y2019/control_loops/drivetrain/replay_localizer.cc
+++ b/y2019/control_loops/drivetrain/replay_localizer.cc
@@ -64,7 +64,7 @@
typedef ::frc971::IMUValues IMUValues;
typedef ::aos::JoystickState JoystickState;
- LocalizerReplayer() : localizer_(GetDrivetrainConfig(), &event_loop_) {
+ LocalizerReplayer() : localizer_(&event_loop_, GetDrivetrainConfig()) {
replayer_.AddDirectQueueSender<CameraFrame>(
"wpilib_interface.stripped.IMU", "camera_frames",
".y2019.control_loops.drivetrain.camera_frames");
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index e656534..32e060f 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -1,7 +1,6 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
-#include "aos/events/shm-event-loop.h"
-#include "aos/testing/test_shm.h"
+#include "aos/events/simulated-event-loop.h"
#include "gtest/gtest.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
@@ -39,21 +38,23 @@
class TargetSelectorParamTest : public ::testing::TestWithParam<TestParams> {
public:
TargetSelectorParamTest()
- : target_selector_hint_sender_(
- test_event_loop_.MakeSender<
+ : event_loop_(this->event_loop_factory_.MakeEventLoop()),
+ test_event_loop_(this->event_loop_factory_.MakeEventLoop()),
+ target_selector_hint_sender_(
+ test_event_loop_->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_sender_(test_event_loop_.MakeSender<
+ superstructure_goal_sender_(test_event_loop_->MakeSender<
::y2019::control_loops::superstructure::
SuperstructureQueue::Goal>(
".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
private:
- ::aos::testing::TestSharedMemory my_shm_;
+ ::aos::SimulatedEventLoopFactory event_loop_factory_;
protected:
- ::aos::ShmEventLoop event_loop_;
- ::aos::ShmEventLoop test_event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
@@ -63,7 +64,7 @@
};
TEST_P(TargetSelectorParamTest, ExpectReturn) {
- TargetSelector selector(&event_loop_);
+ TargetSelector selector(event_loop_.get());
{
auto super_goal = superstructure_goal_sender_.MakeMessage();
super_goal->suction.gamepiece_mode = GetParam().ball_mode ? 0 : 1;
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index 9f7cc9b..eb5b591 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -31,11 +31,14 @@
}
bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
- const double wrist_position = status->wrist.position;
- const double elevator_position = status->elevator.position;
- const double intake_position = status->intake.position;
- const bool has_piece = status->has_piece;
+ return IsCollided(status->wrist.position, status->elevator.position,
+ status->intake.position, status->has_piece);
+}
+bool CollisionAvoidance::IsCollided(const double wrist_position,
+ const double elevator_position,
+ const double intake_position,
+ const bool has_piece) {
const double wrist_elevator_collision_max_angle =
has_piece ? kWristElevatorCollisionMaxAngle
: kWristElevatorCollisionMaxAngleWithoutObject;
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 364836a..7feb45d 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -19,6 +19,8 @@
// Reports if the superstructure is collided.
bool IsCollided(const SuperstructureQueue::Status *status);
+ bool IsCollided(double wrist_position, double elevator_position,
+ double intake_position, bool has_piece);
// Checks and alters goals to make sure they're safe.
// TODO(austin): Either we will have a unit delay because this has to happen
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 7628ca8..bad771e 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -37,8 +37,20 @@
// the position.
class SuperstructureSimulation {
public:
- SuperstructureSimulation()
- : elevator_plant_(
+ SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<SuperstructureQueue::Position>(
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position")),
+ superstructure_status_fetcher_(event_loop_->MakeFetcher<
+ SuperstructureQueue::Status>(
+ ".y2019.control_loops.superstructure.superstructure_queue.status")),
+ superstructure_output_fetcher_(event_loop_->MakeFetcher<
+ SuperstructureQueue::Output>(
+ ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ elevator_plant_(
new CappedTestPlant(::y2019::control_loops::superstructure::
elevator::MakeElevatorPlant())),
elevator_pot_encoder_(M_PI * 2.0 *
@@ -57,15 +69,7 @@
stilts_plant_(new CappedTestPlant(
::y2019::control_loops::superstructure::stilts::MakeStiltsPlant())),
stilts_pot_encoder_(M_PI * 2.0 *
- constants::Values::kStiltsEncoderRatio()),
-
- superstructure_queue_(
- ".y2019.control_loops.superstructure.superstructure_queue",
- ".y2019.control_loops.superstructure.superstructure_queue.goal",
- ".y2019.control_loops.superstructure.superstructure_queue.output",
- ".y2019.control_loops.superstructure.superstructure_queue.status",
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position") {
+ constants::Values::kStiltsEncoderRatio()) {
// Start the elevator out in the middle by default.
InitializeElevatorPosition(constants::Values::kElevatorRange().upper);
@@ -76,6 +80,17 @@
// Start the stilts out in the middle by default.
InitializeStiltsPosition(constants::Values::kStiltsRange().lower);
+
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
void InitializeElevatorPosition(double start_pos) {
@@ -121,8 +136,8 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
- superstructure_queue_.position.MakeMessage();
+ ::aos::Sender<SuperstructureQueue::Position>::Message position =
+ superstructure_position_sender_.MakeMessage();
elevator_pot_encoder_.GetSensorValues(&position->elevator);
wrist_pot_encoder_.GetSensorValues(&position->wrist);
@@ -148,85 +163,78 @@
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
- void set_elevator_voltage_offset(double voltage_offset) {
- elevator_plant_->set_voltage_offset(voltage_offset);
- }
-
- void set_wrist_voltage_offset(double voltage_offset) {
- wrist_plant_->set_voltage_offset(voltage_offset);
- }
-
- void set_intake_voltage_offset(double voltage_offset) {
- intake_plant_->set_voltage_offset(voltage_offset);
- }
-
- void set_stilts_voltage_offset(double voltage_offset) {
- stilts_plant_->set_voltage_offset(voltage_offset);
- }
-
void set_simulated_pressure(double pressure) {
simulated_pressure_ = pressure;
}
// Simulates the superstructure for a single timestep.
void Simulate() {
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
- EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+ const double last_elevator_velocity = elevator_velocity();
+ const double last_wrist_velocity = wrist_velocity();
+ const double last_intake_velocity = intake_velocity();
+ const double last_stilts_velocity = stilts_velocity();
+
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ if (check_collisions_) {
+ CheckCollisions(superstructure_status_fetcher_.get());
+ }
const double voltage_check_elevator =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_queue_.status->elevator.state) ==
+ superstructure_status_fetcher_->elevator.state) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().elevator.subsystem_params.operating_voltage
: constants::GetValues().elevator.subsystem_params.zeroing_voltage;
const double voltage_check_wrist =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_queue_.status->wrist.state) ==
+ superstructure_status_fetcher_->wrist.state) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().wrist.subsystem_params.operating_voltage
: constants::GetValues().wrist.subsystem_params.zeroing_voltage;
const double voltage_check_intake =
(static_cast<AbsoluteEncoderSubsystem::State>(
- superstructure_queue_.status->intake.state) ==
+ superstructure_status_fetcher_->intake.state) ==
AbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().intake.operating_voltage
: constants::GetValues().intake.zeroing_voltage;
const double voltage_check_stilts =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_queue_.status->stilts.state) ==
+ superstructure_status_fetcher_->stilts.state) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().stilts.subsystem_params.operating_voltage
: constants::GetValues().stilts.subsystem_params.zeroing_voltage;
- EXPECT_NEAR(superstructure_queue_.output->elevator_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage, 0.0,
voltage_check_elevator);
- EXPECT_NEAR(superstructure_queue_.output->wrist_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage, 0.0,
voltage_check_wrist);
- EXPECT_NEAR(superstructure_queue_.output->intake_joint_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage, 0.0,
voltage_check_intake);
- EXPECT_NEAR(superstructure_queue_.output->stilts_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->stilts_voltage, 0.0,
voltage_check_stilts);
::Eigen::Matrix<double, 1, 1> elevator_U;
- elevator_U << superstructure_queue_.output->elevator_voltage +
+ elevator_U << superstructure_output_fetcher_->elevator_voltage +
elevator_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> wrist_U;
- wrist_U << superstructure_queue_.output->wrist_voltage +
+ wrist_U << superstructure_output_fetcher_->wrist_voltage +
wrist_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_queue_.output->intake_joint_voltage +
+ intake_U << superstructure_output_fetcher_->intake_joint_voltage +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> stilts_U;
- stilts_U << superstructure_queue_.output->stilts_voltage +
+ stilts_U << superstructure_output_fetcher_->stilts_voltage +
stilts_plant_->voltage_offset();
elevator_plant_->Update(elevator_U);
@@ -257,140 +265,42 @@
EXPECT_GE(position_stilts, constants::Values::kStiltsRange().lower_hard);
EXPECT_LE(position_stilts, constants::Values::kStiltsRange().upper_hard);
- }
- private:
- ::std::unique_ptr<CappedTestPlant> elevator_plant_;
- PositionSensorSimulator elevator_pot_encoder_;
-
- ::std::unique_ptr<CappedTestPlant> wrist_plant_;
- PositionSensorSimulator wrist_pot_encoder_;
-
- ::std::unique_ptr<CappedTestPlant> intake_plant_;
- PositionSensorSimulator intake_pot_encoder_;
-
- ::std::unique_ptr<CappedTestPlant> stilts_plant_;
- PositionSensorSimulator stilts_pot_encoder_;
-
- double simulated_pressure_ = 1.0;
-
- SuperstructureQueue superstructure_queue_;
-};
-
-class SuperstructureTest : public ::aos::testing::ControlLoopTest {
- protected:
- SuperstructureTest()
- : superstructure_queue_(
- ".y2019.control_loops.superstructure.superstructure_queue",
- ".y2019.control_loops.superstructure.superstructure_queue.goal",
- ".y2019.control_loops.superstructure.superstructure_queue.output",
- ".y2019.control_loops.superstructure.superstructure_queue.status",
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position"),
- superstructure_(&event_loop_) {
- set_team_id(::frc971::control_loops::testing::kTeamNumber);
- }
-
- void VerifyNearGoal() {
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.status.FetchLatest();
-
- EXPECT_NEAR(superstructure_queue_.goal->elevator.unsafe_goal,
- superstructure_queue_.status->elevator.position, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->wrist.unsafe_goal,
- superstructure_plant_.wrist_position(), 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->intake.unsafe_goal,
- superstructure_queue_.status->intake.position, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->stilts.unsafe_goal,
- superstructure_plant_.stilts_position(), 0.001);
- }
-
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
-
- superstructure_plant_.SendPositionMessage();
- superstructure_.Iterate();
- superstructure_plant_.Simulate();
-
- TickTime(chrono::microseconds(5050));
- }
-
- void CheckCollisions() {
- superstructure_queue_.status.FetchLatest();
- ASSERT_FALSE(
- collision_avoidance_.IsCollided(superstructure_queue_.status.get()));
- }
-
- void WaitUntilZeroed() {
- int i = 0;
- do {
- i++;
- RunIteration();
- superstructure_queue_.status.FetchLatest();
- // 2 Seconds
- ASSERT_LE(i, 2 * 1.0 / .00505);
- } while (!superstructure_queue_.status.get()->zeroed);
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
- bool check_collisions = true) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- const auto loop_start_time = monotonic_clock::now();
- double begin_elevator_velocity =
- superstructure_plant_.elevator_velocity();
- double begin_wrist_velocity = superstructure_plant_.wrist_velocity();
- double begin_intake_velocity = superstructure_plant_.intake_velocity();
- double begin_stilts_velocity = superstructure_plant_.stilts_velocity();
-
- RunIteration(enabled);
- if (check_collisions) {
- CheckCollisions();
- }
-
- const double loop_time = ::aos::time::DurationInSeconds(
- monotonic_clock::now() - loop_start_time);
-
- const double elevator_acceleration =
- (superstructure_plant_.elevator_velocity() -
- begin_elevator_velocity) /
- loop_time;
- const double wrist_acceleration =
- (superstructure_plant_.wrist_velocity() - begin_wrist_velocity) /
- loop_time;
- const double intake_acceleration =
- (superstructure_plant_.intake_velocity() - begin_intake_velocity) /
- loop_time;
- const double stilts_acceleration =
- (superstructure_plant_.stilts_velocity() - begin_stilts_velocity) /
- loop_time;
-
- EXPECT_GE(peak_elevator_acceleration_, elevator_acceleration);
- EXPECT_LE(-peak_elevator_acceleration_, elevator_acceleration);
- EXPECT_GE(peak_elevator_velocity_,
- superstructure_plant_.elevator_velocity());
- EXPECT_LE(-peak_elevator_velocity_,
- superstructure_plant_.elevator_velocity());
-
- EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
- EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
- EXPECT_GE(peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
- EXPECT_LE(-peak_wrist_velocity_, superstructure_plant_.wrist_velocity());
-
- EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
- EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
- EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
- EXPECT_LE(-peak_intake_velocity_,
- superstructure_plant_.intake_velocity());
-
- EXPECT_GE(peak_stilts_acceleration_, stilts_acceleration);
- EXPECT_LE(-peak_stilts_acceleration_, stilts_acceleration);
- EXPECT_GE(peak_stilts_velocity_, superstructure_plant_.stilts_velocity());
- EXPECT_LE(-peak_stilts_velocity_,
- superstructure_plant_.stilts_velocity());
+ // Check that no constraints have been violated.
+ if (check_collisions_) {
+ CheckCollisions(superstructure_status_fetcher_.get());
}
+
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+
+ const double elevator_acceleration =
+ (elevator_velocity() - last_elevator_velocity) / loop_time;
+ const double wrist_acceleration =
+ (wrist_velocity() - last_wrist_velocity) / loop_time;
+ const double intake_acceleration =
+ (intake_velocity() - last_intake_velocity) / loop_time;
+ const double stilts_acceleration =
+ (stilts_velocity() - last_stilts_velocity) / loop_time;
+
+ EXPECT_GE(peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_LE(-peak_elevator_acceleration_, elevator_acceleration);
+ EXPECT_GE(peak_elevator_velocity_, elevator_velocity());
+ EXPECT_LE(-peak_elevator_velocity_, elevator_velocity());
+
+ EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_GE(peak_wrist_velocity_, wrist_velocity());
+ EXPECT_LE(-peak_wrist_velocity_, wrist_velocity());
+
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_intake_velocity_, intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_, intake_velocity());
+
+ EXPECT_GE(peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_LE(-peak_stilts_acceleration_, stilts_acceleration);
+ EXPECT_GE(peak_stilts_velocity_, stilts_velocity());
+ EXPECT_LE(-peak_stilts_velocity_, stilts_velocity());
}
void set_peak_elevator_acceleration(double value) {
@@ -415,20 +325,43 @@
}
void set_peak_stilts_velocity(double value) { peak_stilts_velocity_ = value; }
- ::aos::ShmEventLoop event_loop_;
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory
- // that is no longer valid.
- SuperstructureQueue superstructure_queue_;
-
- // Create a control loop and simulation.
- Superstructure superstructure_;
- SuperstructureSimulation superstructure_plant_;
-
- // Creat a collision avoidance object
- CollisionAvoidance collision_avoidance_;
+ void set_check_collisions(bool check_collisions) {
+ check_collisions_ = check_collisions;
+ }
private:
+ void CheckCollisions(const SuperstructureQueue::Status *status) {
+ ASSERT_FALSE(
+ collision_avoidance_.IsCollided(wrist_position(), elevator_position(),
+ intake_position(), status->has_piece));
+ }
+
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
+
+ ::std::unique_ptr<CappedTestPlant> elevator_plant_;
+ PositionSensorSimulator elevator_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> wrist_plant_;
+ PositionSensorSimulator wrist_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> intake_plant_;
+ PositionSensorSimulator intake_pot_encoder_;
+
+ ::std::unique_ptr<CappedTestPlant> stilts_plant_;
+ PositionSensorSimulator stilts_pot_encoder_;
+
+ double simulated_pressure_ = 1.0;
+
+ bool check_collisions_ = true;
+
// The acceleration limits to check for while moving.
double peak_elevator_acceleration_ = 1e10;
double peak_wrist_acceleration_ = 1e10;
@@ -440,10 +373,84 @@
double peak_wrist_velocity_ = 1e10;
double peak_intake_velocity_ = 1e10;
double peak_stilts_velocity_ = 1e10;
+
+ // Creat a collision avoidance object
+ CollisionAvoidance collision_avoidance_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ superstructure_goal_fetcher_(test_event_loop_->MakeFetcher<
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ superstructure_goal_sender_(test_event_loop_->MakeSender<
+ SuperstructureQueue::Goal>(
+ ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ superstructure_status_fetcher_(test_event_loop_->MakeFetcher<
+ SuperstructureQueue::Status>(
+ ".y2019.control_loops.superstructure.superstructure_queue.status")),
+ superstructure_output_fetcher_(test_event_loop_->MakeFetcher<
+ SuperstructureQueue::Output>(
+ ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position")),
+ superstructure_event_loop_(MakeEventLoop()),
+ superstructure_(superstructure_event_loop_.get()),
+ superstructure_plant_event_loop_(MakeEventLoop()),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->elevator.unsafe_goal,
+ superstructure_status_fetcher_->elevator.position, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->wrist.unsafe_goal,
+ superstructure_plant_.wrist_position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake.unsafe_goal,
+ superstructure_status_fetcher_->intake.position, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->stilts.unsafe_goal,
+ superstructure_plant_.stilts_position(), 0.001);
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+ ASSERT_LE(i, 2 * 1.0 / .00505);
+ } while (!superstructure_status_fetcher_.get()->zeroed);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+
+ ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Position>
+ superstructure_position_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
+ Superstructure superstructure_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
};
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
+ SetEnabled(true);
superstructure_plant_.InitializeElevatorPosition(1.4);
superstructure_plant_.InitializeWristPosition(1.0);
superstructure_plant_.InitializeIntakePosition(1.1);
@@ -452,7 +459,7 @@
WaitUntilZeroed();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = 1.4;
goal->wrist.unsafe_goal = 1.0;
@@ -460,14 +467,15 @@
goal->stilts.unsafe_goal = 0.1;
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
}
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
// Set a reasonable goal.
superstructure_plant_.InitializeElevatorPosition(1.4);
@@ -477,7 +485,7 @@
WaitUntilZeroed();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = 1.4;
goal->elevator.profile_params.max_velocity = 1;
goal->elevator.profile_params.max_acceleration = 0.5;
@@ -498,7 +506,7 @@
}
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -508,10 +516,11 @@
//
// We are going to disable collision detection to make this easier to implement.
TEST_F(SuperstructureTest, SaturationTest) {
+ SetEnabled(true);
// Zero it before we move.
WaitUntilZeroed();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
@@ -519,13 +528,13 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
goal->elevator.profile_params.max_velocity = 20.0;
goal->elevator.profile_params.max_acceleration = 0.1;
@@ -544,21 +553,21 @@
ASSERT_TRUE(goal.Send());
}
- set_peak_elevator_velocity(23.0);
- set_peak_elevator_acceleration(0.2);
- set_peak_wrist_velocity(23.0);
- set_peak_wrist_acceleration(0.2);
- set_peak_intake_velocity(23.0);
- set_peak_intake_acceleration(0.2);
- set_peak_stilts_velocity(23.0);
- set_peak_stilts_acceleration(0.2);
+ superstructure_plant_.set_peak_elevator_velocity(23.0);
+ superstructure_plant_.set_peak_elevator_acceleration(0.2);
+ superstructure_plant_.set_peak_wrist_velocity(23.0);
+ superstructure_plant_.set_peak_wrist_acceleration(0.2);
+ superstructure_plant_.set_peak_intake_velocity(23.0);
+ superstructure_plant_.set_peak_intake_acceleration(0.2);
+ superstructure_plant_.set_peak_stilts_velocity(23.0);
+ superstructure_plant_.set_peak_stilts_acceleration(0.2);
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
goal->elevator.profile_params.max_velocity = 0.1;
goal->elevator.profile_params.max_acceleration = 10.0;
@@ -575,14 +584,14 @@
goal->stilts.profile_params.max_velocity = 0.1;
goal->stilts.profile_params.max_acceleration = 10.0;
}
- set_peak_elevator_velocity(0.2);
- set_peak_elevator_acceleration(11.0);
- set_peak_wrist_velocity(0.2);
- set_peak_wrist_acceleration(11.0);
- set_peak_intake_velocity(0.2);
- set_peak_intake_acceleration(11.0);
- set_peak_stilts_velocity(0.2);
- set_peak_stilts_acceleration(11.0);
+ superstructure_plant_.set_peak_elevator_velocity(0.2);
+ superstructure_plant_.set_peak_elevator_acceleration(11.0);
+ superstructure_plant_.set_peak_wrist_velocity(0.2);
+ superstructure_plant_.set_peak_wrist_acceleration(11.0);
+ superstructure_plant_.set_peak_intake_velocity(0.2);
+ superstructure_plant_.set_peak_intake_acceleration(11.0);
+ superstructure_plant_.set_peak_stilts_velocity(0.2);
+ superstructure_plant_.set_peak_stilts_acceleration(11.0);
VerifyNearGoal();
}
@@ -595,7 +604,7 @@
superstructure_plant_.InitializeStiltsPosition(0.1);
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = 1.4;
goal->elevator.profile_params.max_velocity = 1;
@@ -621,8 +630,9 @@
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
+ SetEnabled(true);
WaitUntilZeroed();
- RunForTime(chrono::seconds(2));
+ RunFor(chrono::seconds(2));
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.elevator().state());
@@ -638,6 +648,7 @@
// Move wrist front to back and see if we collide
TEST_F(SuperstructureTest, CollisionTest) {
+ SetEnabled(true);
// Set a reasonable goal.
superstructure_plant_.InitializeElevatorPosition(
constants::Values::kElevatorRange().lower);
@@ -649,7 +660,7 @@
WaitUntilZeroed();
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
goal->wrist.unsafe_goal = -M_PI / 3.0;
goal->intake.unsafe_goal =
@@ -659,19 +670,20 @@
}
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(20), true, true);
+ RunFor(chrono::seconds(20));
VerifyNearGoal();
}
// Tests that the rollers spin when allowed
TEST_F(SuperstructureTest, IntakeRollerTest) {
+ SetEnabled(true);
WaitUntilZeroed();
// Get the elevator and wrist out of the way and set the Intake to where
// we should be able to spin and verify that they do
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
goal->wrist.unsafe_goal = 0.0;
goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
@@ -680,16 +692,16 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5), true, true);
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.output.FetchLatest();
- EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage,
- superstructure_queue_.goal->roller_voltage);
+ RunFor(chrono::seconds(5));
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_output_fetcher_.Fetch();
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage,
+ superstructure_goal_fetcher_->roller_voltage);
VerifyNearGoal();
// Move the intake where we oughtn't to spin the rollers and verify they don't
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
goal->wrist.unsafe_goal = 0.0;
goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
@@ -698,40 +710,41 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(chrono::seconds(5), true, true);
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.output.FetchLatest();
- EXPECT_EQ(superstructure_queue_.output->intake_roller_voltage, 0.0);
+ RunFor(chrono::seconds(5));
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_output_fetcher_.Fetch();
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage, 0.0);
VerifyNearGoal();
}
// Tests the Vacuum detects a gamepiece
TEST_F(SuperstructureTest, VacuumDetectsPiece) {
+ SetEnabled(true);
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
- RunForTime(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10), true,
- false);
+ RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
// Verify that at 0 pressure after short time voltage is still 12
superstructure_plant_.set_simulated_pressure(0.0);
- RunForTime(chrono::seconds(2));
- superstructure_queue_.status.FetchLatest();
- EXPECT_TRUE(superstructure_queue_.status->has_piece);
+ RunFor(chrono::seconds(2));
+ superstructure_status_fetcher_.Fetch();
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
}
// Tests the Vacuum backs off after acquiring a gamepiece
TEST_F(SuperstructureTest, VacuumBacksOff) {
+ SetEnabled(true);
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
@@ -739,25 +752,25 @@
// Verify that at 0 pressure after short time voltage is still high
superstructure_plant_.set_simulated_pressure(0.0);
- RunForTime(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10), true,
- false);
- superstructure_queue_.output.FetchLatest();
- EXPECT_EQ(superstructure_queue_.output->pump_voltage, Vacuum::kPumpVoltage);
+ RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
+ superstructure_output_fetcher_.Fetch();
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, Vacuum::kPumpVoltage);
// Verify that after waiting with a piece the pump voltage goes to the
// has piece voltage
- RunForTime(chrono::seconds(2), true, false);
- superstructure_queue_.output.FetchLatest();
- EXPECT_EQ(superstructure_queue_.output->pump_voltage,
+ RunFor(chrono::seconds(2));
+ superstructure_output_fetcher_.Fetch();
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage,
Vacuum::kPumpHasPieceVoltage);
}
// Tests the Vacuum stops immediately after getting a no suck goal
TEST_F(SuperstructureTest, VacuumStopsQuickly) {
+ SetEnabled(true);
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
@@ -765,27 +778,28 @@
// Get a Gamepiece
superstructure_plant_.set_simulated_pressure(0.0);
- RunForTime(chrono::seconds(2));
- superstructure_queue_.status.FetchLatest();
- EXPECT_TRUE(superstructure_queue_.status->has_piece);
+ RunFor(chrono::seconds(2));
+ superstructure_status_fetcher_.Fetch();
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
// Turn off suction
{
- auto goal = superstructure_queue_.goal.MakeMessage();
+ auto goal = superstructure_goal_sender_.MakeMessage();
goal->suction.grab_piece = false;
ASSERT_TRUE(goal.Send());
}
superstructure_plant_.set_simulated_pressure(1.0);
// Run for a short while and check that voltage dropped to 0
- RunForTime(chrono::milliseconds(10), true, false);
- superstructure_queue_.output.FetchLatest();
- EXPECT_EQ(superstructure_queue_.output->pump_voltage, 0.0);
+ RunFor(chrono::milliseconds(10));
+ superstructure_output_fetcher_.Fetch();
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, 0.0);
}
// Tests that running disabled, ya know, works
TEST_F(SuperstructureTest, DiasableTest) {
- RunForTime(chrono::seconds(2), false, false);
+ superstructure_plant_.set_check_collisions(false);
+ RunFor(chrono::seconds(2));
}
} // namespace testing
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index fe7be09..27520c1 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -4,11 +4,14 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2019::control_loops::superstructure::Superstructure superstructure(
&event_loop);
- superstructure.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9ee1a3e..5f5f06c 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -574,9 +574,12 @@
} // namespace y2019
int main() {
- ::aos::Init(-1);
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2019::input::joysticks::Reader reader(&event_loop);
- reader.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
}