Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 9b9ba30..1575584 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
-load("//aos/build:queues.bzl", "queue_library")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
genrule(
name = "genrule_drivetrain",
@@ -31,39 +31,6 @@
],
)
-cc_binary(
- name = "replay_localizer",
- srcs = [
- "replay_localizer.cc",
- ],
- defines =
- cpu_select({
- "amd64": [
- "SUPPORT_PLOT=1",
- ],
- "arm": [],
- }),
- linkstatic = True,
- deps = [
- "//frc971/control_loops/drivetrain:localizer_queue",
- ":localizer",
- ":event_loop_localizer",
- ":drivetrain_base",
- "@com_github_gflags_gflags//:gflags",
- "//y2019:constants",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- "//frc971/queues:gyro",
- "//frc971/wpilib:imu_queue",
- ] + cpu_select({
- "amd64": [
- "//third_party/matplotlib-cpp",
- ],
- "arm": [],
- }),
-)
-
cc_library(
name = "polydrivetrain_plants",
srcs = [
@@ -111,24 +78,24 @@
":drivetrain_base",
":event_loop_localizer",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
-queue_library(
- name = "target_selector_queue",
- srcs = [
- "target_selector.q",
- ],
+flatbuffer_cc_library(
+ name = "target_selector_fbs",
+ srcs = ["target_selector.fbs"],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
-queue_library(
- name = "camera_queue",
+flatbuffer_cc_library(
+ name = "camera_fbs",
srcs = [
- "camera.q",
+ "camera.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -167,17 +134,18 @@
hdrs = ["target_selector.h"],
deps = [
":camera",
- ":target_selector_queue",
+ ":target_selector_fbs",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:localizer",
"//y2019:constants",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
],
)
cc_test(
name = "target_selector_test",
srcs = ["target_selector_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
":target_selector",
"//aos/events:simulated_event_loop",
@@ -191,7 +159,7 @@
srcs = ["event_loop_localizer.cc"],
hdrs = ["event_loop_localizer.h"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":localizer",
":target_selector",
"//frc971/control_loops/drivetrain:localizer",
@@ -232,13 +200,14 @@
cc_test(
name = "localized_drivetrain_test",
srcs = ["localized_drivetrain_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":drivetrain_base",
":event_loop_localizer",
":localizer",
"//aos/controls:control_loop_test",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/network:team_number",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_lib",
diff --git a/y2019/control_loops/drivetrain/camera.fbs b/y2019/control_loops/drivetrain/camera.fbs
new file mode 100644
index 0000000..154dd2e
--- /dev/null
+++ b/y2019/control_loops/drivetrain/camera.fbs
@@ -0,0 +1,24 @@
+namespace y2019.control_loops.drivetrain;
+
+// See the Target structure in //y2019/jevois:structures.h for documentation.
+table CameraTarget {
+ distance:float;
+ height:float;
+ heading:float;
+ skew:float;
+}
+
+// Frames from a camera.
+table CameraFrame {
+ // Number of nanoseconds since the aos::monotonic_clock epoch at which this
+ // frame was captured.
+ timestamp:long;
+
+ // Buffer for the 3 targets.
+ targets:[CameraTarget];
+
+ // Index of the camera position (not serial number) which this frame is from.
+ camera:ubyte;
+}
+
+root_type CameraFrame;
diff --git a/y2019/control_loops/drivetrain/camera.q b/y2019/control_loops/drivetrain/camera.q
deleted file mode 100644
index 9d5dffc..0000000
--- a/y2019/control_loops/drivetrain/camera.q
+++ /dev/null
@@ -1,26 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// See the Target structure in //y2019/jevois:structures.h for documentation.
-struct CameraTarget {
- float distance;
- float height;
- float heading;
- float skew;
-};
-
-// Frames from a camera.
-// Published on ".y2019.control_loops.drivetrain.camera_frames"
-message CameraFrame {
- // Number of nanoseconds since the aos::monotonic_clock epoch at which this
- // frame was captured.
- int64_t timestamp;
-
- // Number of targets actually in this frame.
- uint8_t num_targets;
-
- // Buffer for the targets.
- CameraTarget[3] targets;
-
- // Index of the camera position (not serial number) which this frame is from.
- uint8_t camera;
-};
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index ada8f53..77df69b 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -1,6 +1,6 @@
#include "aos/init.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
@@ -10,7 +10,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::control_loops::drivetrain::EventLoopLocalizer localizer(
&event_loop, ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 67a708a..a372df4 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -45,8 +45,7 @@
localizer_.ResetInitialState(monotonic_now, localizer_.X_hat(),
localizer_.P());
});
- frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames");
+ frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>("/drivetrain");
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
@@ -71,37 +70,39 @@
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
now);
while (frame_fetcher_.FetchNext()) {
- HandleFrame(*frame_fetcher_.get());
+ HandleFrame(frame_fetcher_.get());
}
}
-void EventLoopLocalizer::HandleFrame(const CameraFrame &frame) {
+void EventLoopLocalizer::HandleFrame(const CameraFrame *frame) {
// We need to construct TargetView's and pass them to the localizer:
::aos::SizedArray<TargetView, kMaxTargetsPerFrame> views;
// Note: num_targets and camera are unsigned integers and so don't need to be
// checked for < 0.
- if (frame.num_targets > kMaxTargetsPerFrame) {
- AOS_LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
+ size_t camera = frame->camera();
+ if (!frame->has_targets() || frame->targets()->size() > kMaxTargetsPerFrame) {
+ AOS_LOG(ERROR, "Got bad num_targets %d\n",
+ frame->has_targets() ? frame->targets()->size() : 0);
return;
}
- if (frame.camera > cameras_.size()) {
- AOS_LOG(ERROR, "Got bad camera number %d\n", frame.camera);
+ if (camera > cameras_.size()) {
+ AOS_LOG(ERROR, "Got bad camera number %zu\n", camera);
return;
}
- for (int ii = 0; ii < frame.num_targets; ++ii) {
+ for (const CameraTarget *target : *frame->targets()) {
TargetView view;
- view.reading.heading = frame.targets[ii].heading;
- view.reading.distance = frame.targets[ii].distance;
- view.reading.skew = frame.targets[ii].skew;
- view.reading.height = frame.targets[ii].height;
+ view.reading.heading = target->heading();
+ view.reading.distance = target->distance();
+ view.reading.skew = target->skew();
+ view.reading.height = target->height();
if (view.reading.distance < 2.25) {
- cameras_[frame.camera].PopulateNoise(&view);
+ cameras_[camera].PopulateNoise(&view);
views.push_back(view);
}
}
::aos::monotonic_clock::time_point t(
- ::std::chrono::nanoseconds(frame.timestamp));
- localizer_.UpdateTargets(cameras_[frame.camera], views, t);
+ ::std::chrono::nanoseconds(frame->timestamp()));
+ localizer_.UpdateTargets(cameras_[camera], views, t);
}
} // namespace drivetrain
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index dac614b..83b8e79 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -3,7 +3,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/localizer.h"
#include "y2019/control_loops/drivetrain/target_selector.h"
@@ -92,7 +92,7 @@
}
private:
- void HandleFrame(const CameraFrame &frame);
+ void HandleFrame(const CameraFrame *frame);
::aos::EventLoop *event_loop_;
// TODO(james): Make this use Watchers once we have them working in our
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index ad7223e..06d2f24 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -7,9 +7,9 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
// This file tests that the full Localizer, when used with queues within the
// drivetrain, will behave properly. The purpose of this test is to make sure
@@ -20,7 +20,9 @@
namespace drivetrain {
namespace testing {
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::Goal;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace {
DrivetrainConfig<double> GetTest2019DrivetrainConfig() {
@@ -42,25 +44,21 @@
// We must use the 2019 drivetrain config so that we don't have to deal
// with shifting:
LocalizedDrivetrainTest()
- : ::aos::testing::ControlLoopTest(GetTest2019DrivetrainConfig().dt),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ GetTest2019DrivetrainConfig().dt),
test_event_loop_(MakeEventLoop()),
drivetrain_goal_sender_(
- test_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
drivetrain_goal_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
localizer_control_sender_(
- test_event_loop_->MakeSender<
- ::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTest2019DrivetrainConfig()),
- camera_sender_(test_event_loop_->MakeSender<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames")),
+ camera_sender_(
+ test_event_loop_->MakeSender<CameraFrame>("/drivetrain")),
localizer_(drivetrain_event_loop_.get(), dt_config_),
drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
@@ -76,8 +74,8 @@
set_battery_voltage(12.0);
test_event_loop_->MakeWatcher(
- ".frc971.control_loops.drivetrain_queue.status",
- [this](const ::frc971::control_loops::DrivetrainQueue::Status &) {
+ "/drivetrain",
+ [this](const ::frc971::control_loops::drivetrain::Status &) {
// Needs to do camera updates right after we run the control loop.
if (enable_cameras_) {
SendDelayedFrames();
@@ -104,9 +102,9 @@
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
drivetrain_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
drivetrain_plant_.GetRightPosition(), 1e-3);
}
@@ -125,32 +123,36 @@
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_now().time_since_epoch())
- .count();
- frame.camera = ii;
- frame.num_targets = 0;
+ std::unique_ptr<CameraFrameT> frame(new CameraFrameT());
+
for (size_t jj = 0;
jj < ::std::min(EventLoopLocalizer::kMaxTargetsPerFrame,
target_views.size());
++jj) {
EventLoopLocalizer::TargetView view = target_views[jj];
- ++frame.num_targets;
+ std::unique_ptr<CameraTargetT> camera_target(new CameraTargetT());
+
const float nan = ::std::numeric_limits<float>::quiet_NaN();
if (send_bad_frames_) {
- frame.targets[jj].heading = nan;
- frame.targets[jj].distance = nan;
- frame.targets[jj].skew = nan;
- frame.targets[jj].height = nan;
+ camera_target->heading = nan;
+ camera_target->distance = nan;
+ camera_target->skew = nan;
+ camera_target->height = nan;
} else {
- frame.targets[jj].heading = view.reading.heading;
- frame.targets[jj].distance = view.reading.distance;
- frame.targets[jj].skew = view.reading.skew;
- frame.targets[jj].height = view.reading.height;
+ camera_target->heading = view.reading.heading;
+ camera_target->distance = view.reading.distance;
+ camera_target->skew = view.reading.skew;
+ camera_target->height = view.reading.height;
}
+ frame->targets.emplace_back(std::move(camera_target));
}
- camera_delay_queue_.emplace(monotonic_now(), frame);
+
+ frame->timestamp = chrono::duration_cast<chrono::nanoseconds>(
+ monotonic_now().time_since_epoch())
+ .count();
+ frame->camera = ii;
+
+ camera_delay_queue_.emplace(monotonic_now(), std::move(frame));
}
}
@@ -159,20 +161,17 @@
while (!camera_delay_queue_.empty() &&
::std::get<0>(camera_delay_queue_.front()) <
monotonic_now() - camera_latency) {
- auto message = camera_sender_.MakeMessage();
- *message = ::std::get<1>(camera_delay_queue_.front());
- ASSERT_TRUE(message.Send());
+ auto builder = camera_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(CameraFrame::Pack(
+ *builder.fbb(), ::std::get<1>(camera_delay_queue_.front()).get())));
camera_delay_queue_.pop();
}
}
::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_;
+ ::aos::Sender<Goal> drivetrain_goal_sender_;
+ ::aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
@@ -192,7 +191,8 @@
// A queue of camera frames so that we can add a time delay to the data
// coming from the cameras.
- ::std::queue<::std::tuple<::aos::monotonic_clock::time_point, CameraFrame>>
+ ::std::queue<::std::tuple<::aos::monotonic_clock::time_point,
+ std::unique_ptr<CameraFrameT>>>
camera_delay_queue_;
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
@@ -210,11 +210,15 @@
set_enable_cameras(false);
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());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -227,11 +231,15 @@
set_enable_cameras(true);
set_bad_frames(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());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -243,11 +251,15 @@
SetEnabled(true);
set_enable_cameras(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());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -261,17 +273,20 @@
set_enable_cameras(false);
(*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 builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
// VerifyNearGoal succeeds because it is just checking wheel positions:
VerifyNearGoal();
- const Eigen::Matrix<double, 5, 1> true_state =
- drivetrain_plant_.state();
+ const Eigen::Matrix<double, 5, 1> true_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);
@@ -287,19 +302,28 @@
set_enable_cameras(false);
(*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 builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
{
- 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());
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+
+ localizer_control_builder.add_x(drivetrain_plant_.state().x());
+ localizer_control_builder.add_y(drivetrain_plant_.state().y());
+ localizer_control_builder.add_theta(drivetrain_plant_.state()(2, 0));
+
+ EXPECT_TRUE(builder.Send(localizer_control_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -314,11 +338,15 @@
SetStartingPosition({4.0, 0.5, 0.0});
(*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 builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -326,7 +354,9 @@
}
namespace {
-EventLoopLocalizer::Pose HPSlotLeft() { return constants::Field().targets()[7].pose(); }
+EventLoopLocalizer::Pose HPSlotLeft() {
+ return constants::Field().targets()[7].pose();
+}
} // namespace
// Tests that using the line following drivetrain and just driving straight
@@ -336,10 +366,14 @@
set_enable_cameras(false);
SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 3;
- message->throttle = 0.5;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_LINE_FOLLOWER);
+ drivetrain_builder.add_throttle(0.5);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(6));
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 1950468..d9a55f6 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -141,26 +141,75 @@
}
void SetUp() {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.controller_type = 2;
- goal.spline.spline_idx = 1;
- goal.spline.spline_count = 1;
- goal.spline_handle = 1;
- ::std::copy(GetParam().control_pts_x.begin(),
- GetParam().control_pts_x.end(), goal.spline.spline_x.begin());
- ::std::copy(GetParam().control_pts_y.begin(),
- GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
- spline_drivetrain_.SetGoal(goal);
+ flatbuffers::DetachedBuffer goal_buffer;
+ {
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_x.begin(),
+ GetParam().control_pts_x.size());
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_y.begin(),
+ GetParam().control_pts_y.size());
+
+ frc971::MultiSpline::Builder multispline_builder(fbb);
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ frc971::control_loops::drivetrain::SplineGoal::Builder spline_builder(
+ fbb);
+
+ spline_builder.add_spline_idx(1);
+ spline_builder.add_spline(multispline_offset);
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::SplineGoal>
+ spline_offset = spline_builder.Finish();
+
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_spline(spline_offset);
+ goal_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+
+ fbb.Finish(goal_builder.Finish());
+
+ goal_buffer = fbb.Release();
+ }
+ aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Goal> goal(
+ std::move(goal_buffer));
+
+ spline_drivetrain_.SetGoal(&goal.message());
// Let the spline drivetrain compute the spline.
- ::frc971::control_loops::DrivetrainQueue::Status status;
- do {
+ while (true) {
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- spline_drivetrain_.PopulateStatus(&status);
- } while (status.trajectory_logging.planning_state !=
- (int8_t)::frc971::control_loops::drivetrain::SplineDrivetrain::
- PlanState::kPlannedTrajectory);
- spline_drivetrain_.SetGoal(goal);
+
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::TrajectoryLogging>
+ trajectory_logging_offset =
+ spline_drivetrain_.MakeTrajectoryLogging(&fbb);
+
+ ::frc971::control_loops::drivetrain::Status::Builder status_builder(fbb);
+ status_builder.add_trajectory_logging(trajectory_logging_offset);
+ spline_drivetrain_.PopulateStatus(&status_builder);
+ fbb.Finish(status_builder.Finish());
+ aos::FlatbufferDetachedBuffer<::frc971::control_loops::drivetrain::Status>
+ status(fbb.Release());
+
+ if (status.message().trajectory_logging()->planning_state() ==
+ ::frc971::control_loops::drivetrain::PlanningState_PLANNED) {
+ break;
+ }
+ }
+ spline_drivetrain_.SetGoal(&goal.message());
}
void TearDown() {
@@ -373,7 +422,7 @@
spline_drivetrain_.Update(true, known_state);
- ::frc971::control_loops::DrivetrainQueue::Output output;
+ ::frc971::control_loops::drivetrain::OutputT output;
output.left_voltage = 0;
output.right_voltage = 0;
spline_drivetrain_.SetOutput(&output);
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 3e43db9..7f2259d 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -1,5 +1,7 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
+#include "aos/json_to_flatbuffer.h"
+
namespace y2019 {
namespace control_loops {
@@ -11,10 +13,9 @@
back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
constants::Field().targets(), {}),
hint_fetcher_(event_loop->MakeFetcher<drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_fetcher_(event_loop->MakeFetcher<
- superstructure::SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_fetcher_(
+ event_loop->MakeFetcher<superstructure::Goal>("/superstructure")) {}
bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
double command_speed) {
@@ -22,14 +23,14 @@
return false;
}
if (superstructure_goal_fetcher_.Fetch()) {
- ball_mode_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
+ ball_mode_ = superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0;
}
if (hint_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "selector_hint", *hint_fetcher_);
+ VLOG(1) << "selector_hint: " << aos::FlatbufferToJson(hint_fetcher_.get());
// suggested_target is unsigned so we don't check for >= 0.
- if (hint_fetcher_->suggested_target < 4) {
+ if (hint_fetcher_->suggested_target() < 4) {
target_hint_ =
- static_cast<SelectionHint>(hint_fetcher_->suggested_target);
+ static_cast<SelectionHint>(hint_fetcher_->suggested_target());
} else {
AOS_LOG(ERROR, "Got invalid suggested target.\n");
}
diff --git a/y2019/control_loops/drivetrain/target_selector.fbs b/y2019/control_loops/drivetrain/target_selector.fbs
new file mode 100644
index 0000000..d264992
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector.fbs
@@ -0,0 +1,23 @@
+namespace y2019.control_loops.drivetrain;
+
+// These should match the SelectionHint enum in target_selector.h.
+enum SelectionHint : byte {
+ // No selection, we should just default to whatever.
+ NONE,
+ // Near, middle, and far targets.
+ NEAR_SHIP,
+ MID_SHIP,
+ FAR_SHIP,
+ // Far side of the rocket ship.
+ FAR_ROCKET,
+}
+
+
+// A message to provide information to the target selector about what it should
+// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
+table TargetSelectorHint {
+ // Which target we should go for:
+ suggested_target:SelectionHint;
+}
+
+root_type TargetSelectorHint;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 7f86f89..e75e122 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -5,8 +5,8 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/camera.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -68,8 +68,7 @@
FakeCamera back_viewer_;
::aos::Fetcher<drivetrain::TargetSelectorHint> hint_fetcher_;
- ::aos::Fetcher<superstructure::SuperstructureQueue::Goal>
- superstructure_goal_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
// Whether we are currently in ball mode.
bool ball_mode_ = false;
diff --git a/y2019/control_loops/drivetrain/target_selector.q b/y2019/control_loops/drivetrain/target_selector.q
deleted file mode 100644
index 021bd3a..0000000
--- a/y2019/control_loops/drivetrain/target_selector.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// A message to provide information to the target selector about what it should
-// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
-message TargetSelectorHint {
- // Which target we should go for:
- // 0 implies no selection, we should just default to whatever.
- // 1, 2, and 3 imply the near, middle, and far targets.
- // 4 implies far side of the rocket ship.
- // These should match the SelectionHint enum in target_selector.h.
- uint8_t suggested_target;
-};
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 32e060f..2e9028f 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -1,8 +1,8 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/events/simulated_event_loop.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -10,7 +10,6 @@
typedef ::frc971::control_loops::TypedPose<double> Pose;
typedef ::Eigen::Matrix<double, 5, 1> State;
-using SelectionHint = TargetSelector::SelectionHint;
namespace {
// Accessors to get some useful particular targets on the field:
@@ -29,7 +28,7 @@
struct TestParams {
State state;
bool ball_mode;
- SelectionHint selection_hint;
+ drivetrain::SelectionHint selection_hint;
double command_speed;
bool expect_target;
Pose expected_pose;
@@ -38,18 +37,21 @@
class TargetSelectorParamTest : public ::testing::TestWithParam<TestParams> {
public:
TargetSelectorParamTest()
- : event_loop_(this->event_loop_factory_.MakeEventLoop()),
+ : configuration_(aos::configuration::ReadConfig("y2019/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ 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<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
private:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
::aos::SimulatedEventLoopFactory event_loop_factory_;
protected:
@@ -58,22 +60,33 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2019::control_loops::superstructure::Goal>
superstructure_goal_sender_;
-
};
TEST_P(TargetSelectorParamTest, ExpectReturn) {
TargetSelector selector(event_loop_.get());
{
- auto super_goal = superstructure_goal_sender_.MakeMessage();
- super_goal->suction.gamepiece_mode = GetParam().ball_mode ? 0 : 1;
- ASSERT_TRUE(super_goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ superstructure::SuctionGoal::Builder suction_builder =
+ builder.MakeBuilder<superstructure::SuctionGoal>();
+
+ suction_builder.add_gamepiece_mode(GetParam().ball_mode ? 0 : 1);
+
+ flatbuffers::Offset<superstructure::SuctionGoal> suction_offset =
+ suction_builder.Finish();
+
+ superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ goal_builder.add_suction(suction_offset);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
{
- auto hint = target_selector_hint_sender_.MakeMessage();
- hint->suggested_target = static_cast<int>(GetParam().selection_hint);
- ASSERT_TRUE(hint.Send());
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(drivetrain::CreateTargetSelectorHint(
+ *builder.fbb(), GetParam().selection_hint)));
}
bool expect_target = GetParam().expect_target;
const State state = GetParam().state;
@@ -105,7 +118,7 @@
// targets:
TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
1.0,
false,
{},
@@ -114,41 +127,43 @@
// anything.
TestParams{(State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- HPSlotLeft(), /*expected_radius=*/0.0},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, HPSlotLeft(), /*expected_radius=*/0.0},
// Put ourselves between the rocket and cargo ship; we should see the
// hatches driving one direction and the near cargo ship port the other.
// We also command a speed opposite the current direction of motion and
// confirm that that behaves as expected.
TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
// And we shouldn't see anything spinning in place:
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.0,
false,
{},
@@ -156,7 +171,7 @@
// Drive backwards off the field--we should not see anything.
TestParams{(State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-1.0,
false,
{},
@@ -164,21 +179,14 @@
// In ball mode, we should be able to see the portal, and get zero
// radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- 1.0,
- true,
- RocketPortal(),
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketPortal(),
/*expected_radius=*/0.0},
// Reversing direction should get cargo ship with zero radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- -1.0,
- true,
- CargoNearLeft(),
- /*expected_radius=*/0.0}
- ));
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(),
+ /*expected_radius=*/0.0}));
} // namespace testing
} // namespace control_loops
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f43b448..7cbb202 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -1,16 +1,48 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "superstructure_queue",
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
srcs = [
- "superstructure.q",
+ "superstructure_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
],
)
@@ -24,11 +56,14 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
":vacuum",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:constants",
"//y2019:status_light",
],
@@ -39,18 +74,23 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = [
+ "//y2019:config.json",
+ ],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_lib",
- ":superstructure_queue",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//aos/time",
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
@@ -64,7 +104,7 @@
deps = [
":superstructure_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -77,9 +117,12 @@
"collision_avoidance.h",
],
deps = [
- ":superstructure_queue",
- "//aos/controls:control_loop_queues",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/controls:control_loop",
"//frc971:constants",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -92,8 +135,11 @@
"vacuum.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
"//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -104,7 +150,8 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/testing:googletest",
],
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index eb5b591..9c2bf93 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -1,7 +1,11 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include <cmath>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -30,9 +34,9 @@
clear_max_intake_goal();
}
-bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
- return IsCollided(status->wrist.position, status->elevator.position,
- status->intake.position, status->has_piece);
+bool CollisionAvoidance::IsCollided(const Status *status) {
+ return IsCollided(status->wrist()->position(), status->elevator()->position(),
+ status->intake()->position(), status->has_piece());
}
bool CollisionAvoidance::IsCollided(const double wrist_position,
@@ -73,13 +77,12 @@
return false;
}
-void CollisionAvoidance::UpdateGoal(
- const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal) {
- 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;
+void CollisionAvoidance::UpdateGoal(const Status *status,
+ const Goal *unsafe_goal) {
+ 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();
// Start with our constraints being wide open.
clear_max_wrist_goal();
@@ -155,8 +158,8 @@
}
if (unsafe_goal) {
- const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
- const double intake_goal = unsafe_goal->intake.unsafe_goal;
+ const double wrist_goal = unsafe_goal->wrist()->unsafe_goal();
+ const double intake_goal = unsafe_goal->intake()->unsafe_goal();
// Compute if we need to move the intake.
const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 7feb45d..338864e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -2,9 +2,12 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
#include <cmath>
-#include "aos/controls/control_loops.q.h"
+
#include "frc971/constants.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -18,15 +21,14 @@
CollisionAvoidance();
// Reports if the superstructure is collided.
- bool IsCollided(const SuperstructureQueue::Status *status);
+ bool IsCollided(const 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
// after the controls, or we need to be more clever about restructuring.
- void UpdateGoal(const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal);
+ void UpdateGoal(const Status *status, const Goal *unsafe_goal);
// Returns the goals to give to the respective control loops in
// superstructure.
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 82bff4c..6b1f031 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -1,14 +1,21 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "aos/commonmath.h"
+#include "aos/flatbuffers.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
namespace testing {
+using aos::FlatbufferDetachedBuffer;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
/*
Test List:
FullClockwiseRotationFromBottomBackIntakeIn
@@ -22,48 +29,142 @@
QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
*/
+FlatbufferDetachedBuffer<Goal> MakeZeroGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal> wrist_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder wrist_builder(fbb);
+
+ wrist_builder.add_unsafe_goal(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder elevator_builder(fbb);
+
+ elevator_builder.add_unsafe_goal(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder(fbb);
+
+ intake_builder.add_unsafe_goal(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
+FlatbufferDetachedBuffer<Status> MakeZeroStatus() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus> wrist_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder wrist_builder(fbb);
+
+ wrist_builder.add_position(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder elevator_builder(fbb);
+
+ elevator_builder.add_position(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
+ intake_offset;
+ {
+ AbsoluteEncoderProfiledJointStatus::Builder intake_builder(fbb);
+
+ intake_builder.add_position(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Status::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_has_piece(false);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
class CollisionAvoidanceTests : public ::testing::Test,
public ::testing::WithParamInterface<bool> {
public:
- CollisionAvoidanceTests() { status.has_piece = GetParam(); }
+ CollisionAvoidanceTests()
+ : unsafe_goal_(MakeZeroGoal()), status_(MakeZeroStatus()) {
+ status_.mutable_message()->mutate_has_piece(GetParam());
+ }
void Iterate() {
- SuperstructureQueue::Goal safe_goal;
- bool was_collided = avoidance.IsCollided(&status);
+ FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
+ bool was_collided = avoidance.IsCollided(&status_.message());
while (true) {
- avoidance.UpdateGoal(&status, &unsafe_goal);
+ avoidance.UpdateGoal(&status_.message(), &unsafe_goal_.message());
if (!was_collided) {
- EXPECT_FALSE(avoidance.IsCollided(&status));
+ EXPECT_FALSE(avoidance.IsCollided(&status_.message()));
} else {
- was_collided = avoidance.IsCollided(&status);
+ was_collided = avoidance.IsCollided(&status_.message());
}
- safe_goal.wrist.unsafe_goal =
- ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
- avoidance.max_wrist_goal());
+ safe_goal.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().wrist()->unsafe_goal(),
+ avoidance.min_wrist_goal(), avoidance.max_wrist_goal()));
- safe_goal.elevator.unsafe_goal = ::std::max(
- unsafe_goal.elevator.unsafe_goal, avoidance.min_elevator_goal());
+ safe_goal.mutable_message()->mutable_elevator()->mutate_unsafe_goal(
+ ::std::max(unsafe_goal_.message().elevator()->unsafe_goal(),
+ avoidance.min_elevator_goal()));
- safe_goal.intake.unsafe_goal =
- ::aos::Clip(unsafe_goal.intake.unsafe_goal,
- avoidance.min_intake_goal(), avoidance.max_intake_goal());
+ safe_goal.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().intake()->unsafe_goal(),
+ avoidance.min_intake_goal(),
+ avoidance.max_intake_goal()));
- LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
- LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
- LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ LimitedMove(status_.message().wrist()->position(),
+ safe_goal.message().wrist()->unsafe_goal()));
+ status_.mutable_message()->mutable_elevator()->mutate_position(
+ LimitedMove(status_.message().elevator()->position(),
+ safe_goal.message().elevator()->unsafe_goal()));
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ LimitedMove(status_.message().intake()->position(),
+ safe_goal.message().intake()->unsafe_goal()));
if (IsMoving()) {
break;
}
- past_status = status;
+ past_wrist_position_ = status_.message().wrist()->position();
+ past_elevator_position_ = status_.message().elevator()->position();
+ past_intake_position_ = status_.message().intake()->position();
}
}
bool IsMoving() {
- if ((past_status.wrist.position == status.wrist.position) &&
- (past_status.elevator.position == status.elevator.position) &&
- (past_status.intake.position == status.intake.position)) {
+ if ((past_wrist_position_ == status_.message().wrist()->position()) &&
+ (past_elevator_position_ == status_.message().elevator()->position()) &&
+ (past_intake_position_ == status_.message().intake()->position())) {
return true;
} else {
return false;
@@ -71,9 +172,11 @@
}
// provide goals and status messages
- SuperstructureQueue::Goal unsafe_goal;
- SuperstructureQueue::Status status;
- SuperstructureQueue::Status past_status;
+ FlatbufferDetachedBuffer<Goal> unsafe_goal_;
+ FlatbufferDetachedBuffer<Status> status_;
+ float past_wrist_position_ = 0;
+ float past_elevator_position_ = 0;
+ float past_intake_position_ = 0;
protected:
// setup for all tests
@@ -81,20 +184,22 @@
void CheckGoals() {
// check to see if we reached the goals
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position,
- 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().elevator()->unsafe_goal(),
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
private:
- void LimitedMove(float *position, double goal) {
- if (*position + kIterationMove < goal) {
- *position += kIterationMove;
- } else if (*position - kIterationMove > goal) {
- *position -= kIterationMove;
+ float LimitedMove(float position, double goal) {
+ if (position + kIterationMove < goal) {
+ return position + kIterationMove;
+ } else if (position - kIterationMove > goal) {
+ return position - kIterationMove;
} else {
- *position = goal;
+ return goal;
}
}
@@ -105,17 +210,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -128,18 +235,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle);
Iterate();
@@ -151,18 +259,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- status.wrist.position = avoidance.kWristMaxAngle / 2.0;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle / 2.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -174,18 +283,20 @@
FullCounterClockwiseRotationFromBottomBackIntakeIn) {
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -198,17 +309,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -221,18 +334,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
@@ -244,23 +358,27 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal = 4.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(4.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Unreasonable Wrist Goal
@@ -268,89 +386,101 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Elevator
TEST_P(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 3.14;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(3.14);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 4.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(4.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Intake
TEST_P(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist Above Elevator
TEST_P(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 0.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 0.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
INSTANTIATE_TEST_CASE_P(CollisionAvoidancePieceTest, CollisionAvoidanceTests,
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index ff9bb13..91e1704 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -1,34 +1,36 @@
#include "y2019/control_loops/superstructure/superstructure.h"
-#include "aos/controls/control_loops.q.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2019::StatusLight>(".y2019.status_light")),
+ event_loop->MakeSender<::y2019::StatusLight>("/superstructure")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
elevator_(constants::GetValues().elevator.subsystem_params),
wrist_(constants::GetValues().wrist.subsystem_params),
intake_(constants::GetValues().intake),
stilts_(constants::GetValues().stilts.subsystem_params) {}
-void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
elevator_.Reset();
@@ -37,49 +39,92 @@
stilts_.Reset();
}
- elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
- &(position->elevator),
- output != nullptr ? &(output->elevator_voltage) : nullptr,
- &(status->elevator));
+ OutputT output_struct;
- wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
- &(position->wrist),
- output != nullptr ? &(output->wrist_voltage) : nullptr,
- &(status->wrist));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_status_offset = elevator_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->elevator() : nullptr,
+ position->elevator(),
+ output != nullptr ? &(output_struct.elevator_voltage) : nullptr,
+ status->fbb());
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ wrist_status_offset = wrist_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
+ status->fbb());
- stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
- &(position->stilts),
- output != nullptr ? &(output->stilts_voltage) : nullptr,
- &(status->stilts));
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
+ intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake_joint(),
+ output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
+ status->fbb());
- vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece),
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ stilts_status_offset = stilts_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->stilts() : nullptr,
+ position->stilts(),
+ output != nullptr ? &(output_struct.stilts_voltage) : nullptr,
+ status->fbb());
+
+ bool has_piece;
+ vacuum_.Iterate(unsafe_goal != nullptr ? unsafe_goal->suction() : nullptr,
+ position->suction_pressure(), &output_struct, &has_piece,
event_loop());
- status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
- status->intake.zeroed && status->stilts.zeroed;
+ bool zeroed;
+ bool estopped;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus *elevator_status =
+ GetMutableTemporaryPointer(*status->fbb(), elevator_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *wrist_status =
+ GetMutableTemporaryPointer(*status->fbb(), wrist_status_offset);
+ AbsoluteEncoderProfiledJointStatus *intake_status =
+ GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *stilts_status =
+ GetMutableTemporaryPointer(*status->fbb(), stilts_status_offset);
- status->estopped = status->elevator.estopped || status->wrist.estopped ||
- status->intake.estopped || status->stilts.estopped;
+ zeroed = elevator_status->zeroed() && wrist_status->zeroed() &&
+ intake_status->zeroed() && stilts_status->zeroed();
+
+ estopped = elevator_status->estopped() || wrist_status->estopped() ||
+ intake_status->estopped() || stilts_status->estopped();
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+ status_builder.add_has_piece(has_piece);
+
+ status_builder.add_elevator(elevator_status_offset);
+ status_builder.add_wrist(wrist_status_offset);
+ status_builder.add_intake(intake_status_offset);
+ status_builder.add_stilts(stilts_status_offset);
+
+ flatbuffers::Offset<Status> status_offset = status_builder.Finish();
+
+ Status *status_flatbuffer =
+ GetMutableTemporaryPointer(*status->fbb(), status_offset);
if (output) {
- if (unsafe_goal && status->intake.position > kMinIntakeAngleForRollers) {
- output->intake_roller_voltage = unsafe_goal->roller_voltage;
+ if (unsafe_goal &&
+ status_flatbuffer->intake()->position() > kMinIntakeAngleForRollers) {
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
- output->intake_roller_voltage = 0.0;
+ output_struct.intake_roller_voltage = 0.0;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (unsafe_goal) {
- if (!unsafe_goal->suction.grab_piece) {
+ if (!unsafe_goal->has_suction() || !unsafe_goal->suction()->grab_piece()) {
wrist_.set_controller_index(0);
elevator_.set_controller_index(0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ } else if (unsafe_goal->suction()->gamepiece_mode() == 0) {
wrist_.set_controller_index(1);
elevator_.set_controller_index(1);
} else {
@@ -90,7 +135,7 @@
// TODO(theo) move these up when Iterate() is split
// update the goals
- collision_avoidance_.UpdateGoal(status, unsafe_goal);
+ collision_avoidance_.UpdateGoal(status_flatbuffer, unsafe_goal);
elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
@@ -102,23 +147,23 @@
if (status && unsafe_goal) {
// Light Logic
- if (status->estopped) {
+ if (status_flatbuffer->estopped()) {
// Estop is red
SendColors(1.0, 0.0, 0.0);
} else if (drivetrain_status_fetcher_.get() &&
- drivetrain_status_fetcher_->line_follow_logging.frozen) {
+ drivetrain_status_fetcher_->line_follow_logging()->frozen()) {
// Vision align is flashing white for button pressed, purple for target
// acquired.
++line_blink_count_;
if (line_blink_count_ < 20) {
- if (drivetrain_status_fetcher_->line_follow_logging.have_target) {
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
SendColors(1.0, 0.0, 1.0);
} else {
SendColors(1.0, 1.0, 1.0);
}
} else {
// And then flash with green if we have a game piece.
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
SendColors(0.0, 1.0, 0.0);
} else {
SendColors(0.0, 0.0, 0.0);
@@ -130,15 +175,17 @@
}
} else {
line_blink_count_ = 0;
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
// Green if we have a game piece.
SendColors(0.0, 1.0, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0 &&
- !status->has_piece) {
+ } else if ((!unsafe_goal->has_suction() ||
+ unsafe_goal->suction()->gamepiece_mode() == 0) &&
+ !status_flatbuffer->has_piece()) {
// Ball mode is orange
SendColors(1.0, 0.1, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 1 &&
- !status->has_piece) {
+ } else if (unsafe_goal->has_suction() &&
+ unsafe_goal->suction()->gamepiece_mode() == 1 &&
+ !status_flatbuffer->has_piece()) {
// Disk mode is deep blue
SendColors(0.05, 0.1, 0.5);
} else {
@@ -146,15 +193,20 @@
}
}
}
+
+ status->Send(status_offset);
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
- if (!new_status_light.Send()) {
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index aab8e7d..f4a23d9 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,26 +2,27 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/control_loops/superstructure/vacuum.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name =
- ".y2019.control_loops.superstructure.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
@@ -39,16 +40,15 @@
const Vacuum &vacuum() const { return vacuum_; }
protected:
- virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
void SendColors(float red, float green, float blue);
::aos::Sender<::y2019::StatusLight> status_light_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
PotAndAbsoluteEncoderSubsystem elevator_;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d19c2b8..0000000
--- a/y2019/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,115 +0,0 @@
-package y2019.control_loops.superstructure;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/profiled_subsystem.q";
-
-struct SuctionGoal {
- // True = apply suction
- bool grab_piece;
-
- // 0 = ball mode
- // 1 = disk mode
-
- int32_t gamepiece_mode;
-};
-
-// Published on ".y2019.control_loops.superstructure.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Meters, 0 = lowest position - mechanical hard stop,
- // positive = upward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
- // 0 = linkage on the sprocket is pointing straight up,
- // positive = forward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
- // 0 = Straight up parallel to elevator
- // Positive rotates toward intake from 0
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
-
- // Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the
- // bottom of the middle wheels.
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
-
- // Positive is rollers intaking inward.
- double roller_voltage;
-
- SuctionGoal suction;
- };
-
- message Status {
- // All subsystems know their location.
- bool zeroed;
-
- // If true, we have aborted. This is the or of all subsystem estops.
- bool estopped;
-
- // Whether suction_pressure indicates cargo is held
- bool has_piece;
-
- // Status of each subsystem.
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
- };
-
- message Position {
- // Input from pressure sensor in bar
- // 1 = 1 atm, 0 = full vacuum
- float suction_pressure;
-
- // Position of the elevator, 0 at lowest position, positive when up.
- .frc971.PotAndAbsolutePosition elevator;
-
- // Position of wrist, 0 when up, positive is rotating toward the front,
- // over the top.
- .frc971.PotAndAbsolutePosition wrist;
-
- // Position of the intake. 0 when rollers are retracted, positive extended.
- .frc971.AbsolutePosition intake_joint;
-
- // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
- .frc971.PotAndAbsolutePosition stilts;
-
- // True if the platform detection sensors detect the platform directly
- // below the robot right behind the left and right wheels. Useful for
- // determining when the robot is all the way on the platform.
- bool platform_left_detect;
- bool platform_right_detect;
- };
-
- message Output {
- // Voltage sent to motors moving elevator up/down. Positive is up.
- double elevator_voltage;
-
- // Voltage sent to wrist motors on elevator to rotate.
- // Positive rotates over the top towards the front of the robot.
- double wrist_voltage;
-
- // Voltage sent to motors on intake joint. Positive extends rollers.
- double intake_joint_voltage;
-
- // Voltage sent to rollers on intake. Positive rolls inward.
- double intake_roller_voltage;
-
- // Voltage sent to motors to move stilts height. Positive moves robot
- // upward.
- double stilts_voltage;
-
- // True opens solenoid (applies suction)
- // Top/bottom are when wrist is toward the front of the robot
- bool intake_suction_top;
- bool intake_suction_bottom;
-
- // Voltage sent to the vacuum pump motors.
- double pump_voltage;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2019/control_loops/superstructure/superstructure_goal.fbs b/y2019/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..77e7c8d
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,37 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table SuctionGoal {
+ // True = apply suction
+ grab_piece:bool;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ gamepiece_mode:int;
+}
+
+table Goal {
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ elevator:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the
+ // bottom of the middle wheels.
+ stilts:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Positive is rollers intaking inward.
+ roller_voltage:float;
+
+ suction:SuctionGoal;
+}
+
+root_type Goal;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index bad771e..7af8ffd 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,7 +4,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -27,8 +26,12 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
@@ -41,15 +44,11 @@
: 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")),
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")),
elevator_plant_(
new CappedTestPlant(::y2019::control_loops::superstructure::
elevator::MakeElevatorPlant())),
@@ -136,16 +135,38 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::Sender<SuperstructureQueue::Position>::Message position =
- superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- elevator_pot_encoder_.GetSensorValues(&position->elevator);
- wrist_pot_encoder_.GetSensorValues(&position->wrist);
- intake_pot_encoder_.GetSensorValues(&position->intake_joint);
- stilts_pot_encoder_.GetSensorValues(&position->stilts);
- position->suction_pressure = simulated_pressure_;
+ frc971::PotAndAbsolutePosition::Builder elevator_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ elevator_pot_encoder_.GetSensorValues(&elevator_builder);
- position.Send();
+ frc971::PotAndAbsolutePosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ wrist_pot_encoder_.GetSensorValues(&wrist_builder);
+
+ frc971::AbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_offset =
+ intake_pot_encoder_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndAbsolutePosition::Builder stilts_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ stilts_pot_encoder_.GetSensorValues(&stilts_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_intake_joint(intake_offset);
+ position_builder.add_stilts(stilts_offset);
+ position_builder.add_suction_pressure(simulated_pressure_);
+
+ builder.Send(position_builder.Finish());
}
double elevator_position() const { return elevator_plant_->X(0, 0); }
@@ -183,58 +204,58 @@
const double voltage_check_elevator =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->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_status_fetcher_->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_status_fetcher_->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_status_fetcher_->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_output_fetcher_->elevator_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage(), 0.0,
voltage_check_elevator);
- EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage(), 0.0,
voltage_check_wrist);
- EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage(), 0.0,
voltage_check_intake);
- EXPECT_NEAR(superstructure_output_fetcher_->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_output_fetcher_->elevator_voltage +
+ elevator_U << superstructure_output_fetcher_->elevator_voltage() +
elevator_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> wrist_U;
- wrist_U << superstructure_output_fetcher_->wrist_voltage +
+ wrist_U << superstructure_output_fetcher_->wrist_voltage() +
wrist_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_output_fetcher_->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_output_fetcher_->stilts_voltage +
+ stilts_U << superstructure_output_fetcher_->stilts_voltage() +
stilts_plant_->voltage_offset();
elevator_plant_->Update(elevator_U);
@@ -330,19 +351,19 @@
}
private:
- void CheckCollisions(const SuperstructureQueue::Status *status) {
- ASSERT_FALSE(
- collision_avoidance_.IsCollided(wrist_position(), elevator_position(),
- intake_position(), status->has_piece));
+ void CheckCollisions(const 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_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
bool first_ = true;
@@ -381,24 +402,20 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ 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_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -410,13 +427,13 @@
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,
+ 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,
+ 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);
}
@@ -428,17 +445,16 @@
superstructure_status_fetcher_.Fetch();
// 2 Seconds
ASSERT_LE(i, 2 * 1.0 / .00505);
- } while (!superstructure_status_fetcher_.get()->zeroed);
+ } 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_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -459,13 +475,29 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->wrist.unsafe_goal = 1.0;
- goal->intake.unsafe_goal = 1.1;
- goal->stilts.unsafe_goal = 0.1;
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -485,24 +517,36 @@
WaitUntilZeroed();
{
- 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;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -520,13 +564,33 @@
// Zero it before we move.
WaitUntilZeroed();
{
- 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;
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().upper);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -534,24 +598,36 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- 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;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
- goal->wrist.profile_params.max_velocity = 20.0;
- goal->wrist.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->intake.profile_params.max_velocity = 20.0;
- goal->intake.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 20.0;
- goal->stilts.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_elevator_velocity(23.0);
superstructure_plant_.set_peak_elevator_acceleration(0.2);
@@ -564,36 +640,6 @@
RunFor(chrono::seconds(8));
VerifyNearGoal();
-
- // Now do a high acceleration move with a low velocity limit.
- {
- 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;
-
- goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
- goal->wrist.profile_params.max_velocity = 0.1;
- goal->wrist.profile_params.max_acceleration = 10.0;
-
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
- goal->intake.profile_params.max_velocity = 0.1;
- goal->intake.profile_params.max_acceleration = 10.0;
-
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 0.1;
- goal->stilts.profile_params.max_acceleration = 10.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();
}
// Tests if the robot zeroes properly... maybe redundant?
@@ -604,25 +650,36 @@
superstructure_plant_.InitializeStiltsPosition(0.1);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->elevator.profile_params.max_velocity = 1;
- goal->elevator.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitUntilZeroed();
VerifyNearGoal();
@@ -660,13 +717,30 @@
WaitUntilZeroed();
{
- 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 =
- CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), -M_PI / 3.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), CollisionAvoidance::kIntakeInAngle -
+ CollisionAvoidance::kEpsIntake);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -683,37 +757,71 @@
// 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_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;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
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);
+ 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_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;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
superstructure_goal_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
VerifyNearGoal();
}
@@ -723,10 +831,33 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
@@ -735,7 +866,7 @@
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
}
// Tests the Vacuum backs off after acquiring a gamepiece
@@ -744,23 +875,47 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Verify that at 0 pressure after short time voltage is still high
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, Vacuum::kPumpVoltage);
+ 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
RunFor(chrono::seconds(2));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage,
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(),
Vacuum::kPumpHasPieceVoltage);
}
@@ -770,30 +925,77 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Get a Gamepiece
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
// Turn off suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = false;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), false);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_simulated_pressure(1.0);
// Run for a short while and check that voltage dropped to 0
RunFor(chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(), 0.0);
}
// Tests that running disabled, ya know, works
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index 27520c1..f831774 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2019/control_loops/superstructure/superstructure.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-int main() {
+int main(int /*argc*/, char * /*argv*/ []) {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2019/control_loops/superstructure/superstructure_output.fbs b/y2019/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..b679347
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,30 @@
+namespace y2019.control_loops.superstructure;
+
+table Output {
+ // Voltage sent to motors moving elevator up/down. Positive is up.
+ elevator_voltage:double;
+
+ // Voltage sent to wrist motors on elevator to rotate.
+ // Positive rotates over the top towards the front of the robot.
+ wrist_voltage:double;
+
+ // Voltage sent to motors on intake joint. Positive extends rollers.
+ intake_joint_voltage:double;
+
+ // Voltage sent to rollers on intake. Positive rolls inward.
+ intake_roller_voltage:double;
+
+ // Voltage sent to motors to move stilts height. Positive moves robot
+ // upward.
+ stilts_voltage:double;
+
+ // True opens solenoid (applies suction)
+ // Top/bottom are when wrist is toward the front of the robot
+ intake_suction_top:bool;
+ intake_suction_bottom:bool;
+
+ // Voltage sent to the vacuum pump motors.
+ pump_voltage:double;
+}
+
+root_type Output;
diff --git a/y2019/control_loops/superstructure/superstructure_position.fbs b/y2019/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..80a851e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,30 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Position {
+ // Input from pressure sensor in bar
+ // 1 = 1 atm, 0 = full vacuum
+ suction_pressure:float;
+
+ // Position of the elevator, 0 at lowest position, positive when up.
+ elevator:frc971.PotAndAbsolutePosition;
+
+ // Position of wrist, 0 when up, positive is rotating toward the front,
+ // over the top.
+ wrist:frc971.PotAndAbsolutePosition;
+
+ // Position of the intake. 0 when rollers are retracted, positive extended.
+ intake_joint:frc971.AbsolutePosition;
+
+ // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
+ stilts:frc971.PotAndAbsolutePosition;
+
+ // True if the platform detection sensors detect the platform directly
+ // below the robot right behind the left and right wheels. Useful for
+ // determining when the robot is all the way on the platform.
+ platform_left_detect:bool;
+ platform_right_detect:bool;
+}
+
+root_type Position;
diff --git a/y2019/control_loops/superstructure/superstructure_status.fbs b/y2019/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..9d3f44e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,23 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Status {
+ // All subsystems know their location.
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+ // Whether suction_pressure indicates cargo is held
+ has_piece:bool;
+
+ // Status of each subsystem.
+ elevator:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
+ stilts:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+}
+
+root_type Status;
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 956bb79..6d4696b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -2,7 +2,10 @@
#include <chrono>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -16,7 +19,7 @@
constexpr aos::monotonic_clock::duration Vacuum::kReleaseTime;
void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
+ OutputT *output, bool *has_piece,
aos::EventLoop *event_loop) {
auto monotonic_now = event_loop->monotonic_now();
bool low_pump_voltage = false;
@@ -46,7 +49,7 @@
low_pump_voltage = *has_piece && filtered_pressure_ < kVacuumOnThreshold;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->grab_piece;
+ const bool release = !unsafe_goal->grab_piece();
if (release) {
last_release_time_ = monotonic_now;
@@ -58,10 +61,10 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 0) {
output->intake_suction_top = false;
output->intake_suction_bottom = true;
- } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ } else if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 1) {
output->intake_suction_top = true;
output->intake_suction_bottom = true;
} else {
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index e793530..c4671ab 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -1,8 +1,11 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
-#include "aos/events/event-loop.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -12,8 +15,7 @@
public:
Vacuum() {}
void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
- aos::EventLoop *event_loop);
+ OutputT *output, bool *has_piece, aos::EventLoop *event_loop);
// Voltage to the vaccum pump when we are attempting to acquire a piece
static constexpr double kPumpVoltage = 8.0;