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/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;
}