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/README.md b/README.md
index e572ea9..87254ed 100644
--- a/README.md
+++ b/README.md
@@ -98,7 +98,7 @@
```console
# Freshly imaged roboRIOs need to be configured to run the 971 code
# at startup. This is done by using the setup_roborio.sh script.
-bazel run -c opt //aos/config:setup_roborio -- roboRIO-XXX-frc.local
+bazel run -c opt //frc971/config:setup_roborio -- roboRIO-XXX-frc.local
```
### Some other useful packages
diff --git a/WORKSPACE b/WORKSPACE
index eded67f..3eacd10 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -50,10 +50,6 @@
"//debian:python_gtk.bzl",
python_gtk_debs = "files",
)
-load(
- "//debian:ruby.bzl",
- ruby_debs = "files",
-)
load("//debian:packages.bzl", "generate_repositories_for_debs")
generate_repositories_for_debs(python_debs)
@@ -80,8 +76,6 @@
generate_repositories_for_debs(python_gtk_debs)
-generate_repositories_for_debs(ruby_debs)
-
http_archive(
name = "python_repo",
build_file = "@//debian:python.BUILD",
@@ -101,6 +95,11 @@
path = "third_party/abseil",
)
+local_repository(
+ name = "org_tuxfamily_eigen",
+ path = "third_party/eigen",
+)
+
# C++ rules for Bazel.
http_archive(
name = "rules_cc",
@@ -126,13 +125,6 @@
)
http_archive(
- name = "ruby_repo",
- build_file = "@//debian:ruby.BUILD",
- sha256 = "d3e21cca0abcad933de0d4095da35344a60475d1f5828ee99283ed4250ee1320",
- url = "http://www.frc971.org/Build-Dependencies/ruby.tar.gz",
-)
-
-http_archive(
name = "arm_frc_linux_gnueabi_repo",
build_file = "@//tools/cpp/arm-frc-linux-gnueabi:arm-frc-linux-gnueabi.BUILD",
sha256 = "d627c5e437db99780a938392499ef71aecbfb0e9b3fffd53bde7e402a6af4f32",
@@ -356,6 +348,11 @@
path = "third_party/google-glog",
)
+local_repository(
+ name = "com_google_googletest",
+ path = "third_party/googletest",
+)
+
# External dependency: Google Benchmark; has no Bazel build.
local_repository(
name = "com_github_google_benchmark",
@@ -459,28 +456,26 @@
)
emscripten_version = "1.38.31"
+
http_archive(
name = "emscripten_toolchain",
- urls = ["https://github.com/emscripten-core/emscripten/archive/" + emscripten_version + ".tar.gz"],
- strip_prefix = "emscripten-" + emscripten_version,
+ build_file_content = """
+filegroup(
+ name = 'all',
+ visibility = ['//visibility:public'],
+ srcs = glob(['**']),
+)
+""",
# TODO(james): Once a functioning release contains this patch, convert
# to that. See https://github.com/emscripten-core/emscripten/pull/9048
patches = ["@//debian:emscripten_toolchain.patch"],
sha256 = "c87e42cb6a104094e7daf2b7e61ac835f83674ac0168f533455838a1129cc764",
- build_file_content = """
-filegroup(
- name = 'all',
- visibility = ['//visibility:public'],
- srcs = glob(['**']),
-)
-""",
+ strip_prefix = "emscripten-" + emscripten_version,
+ urls = ["https://github.com/emscripten-core/emscripten/archive/" + emscripten_version + ".tar.gz"],
)
new_http_archive(
name = "emscripten_clang",
- sha256 = "a0c2f2c5a897577f40af0fdf68dcf3cf65557ff20c081df26678c066a4fed4b1",
- strip_prefix = "emscripten-llvm-e" + emscripten_version,
- url = "https://s3.amazonaws.com/mozilla-games/emscripten/packages/llvm/tag/linux_64bit/emscripten-llvm-e" + emscripten_version + ".tar.gz",
build_file_content = """
filegroup(
name = 'all',
@@ -488,6 +483,9 @@
srcs = glob(['**']),
)
""",
+ sha256 = "a0c2f2c5a897577f40af0fdf68dcf3cf65557ff20c081df26678c066a4fed4b1",
+ strip_prefix = "emscripten-llvm-e" + emscripten_version,
+ url = "https://s3.amazonaws.com/mozilla-games/emscripten/packages/llvm/tag/linux_64bit/emscripten-llvm-e" + emscripten_version + ".tar.gz",
)
# Fetch our Bazel dependencies that aren't distributed on npm
diff --git a/aos/BUILD b/aos/BUILD
index e918d7f..a2c7134 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -1,6 +1,5 @@
load("//tools:environments.bzl", "mcu_cpus")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-load("//aos/build:queues.bzl", "queue_library")
filegroup(
name = "prime_binaries",
@@ -116,100 +115,6 @@
visibility = ["//visibility:public"],
)
-genrule(
- name = "gen_print_field",
- outs = ["print_field.cc"],
- cmd = "$(location //aos/build/queues:print_field) $@",
- tools = ["//aos/build/queues:print_field"],
- visibility = ["//visibility:private"],
-)
-
-cc_library(
- name = "queue_types",
- srcs = [
- "print_field_helpers.h",
- "queue_types.cc",
- ":gen_print_field",
- ],
- hdrs = [
- "queue_types.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":generated_queue_headers",
- "//aos:byteorder",
- "//aos/ipc_lib:core_lib",
- "//aos/ipc_lib:shared_mem",
- "//aos/logging:printf_formats",
- "//aos/mutex",
- "//aos/time",
- ],
-)
-
-cc_test(
- name = "queue_types_test",
- srcs = [
- "queue_types_test.cc",
- ],
- deps = [
- ":queue_types",
- ":test_queue",
- "//aos/logging",
- "//aos/testing:googletest",
- "//aos/testing:test_logging",
- ],
-)
-
-cc_library(
- name = "queues",
- hdrs = [
- "queue.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- "//aos:queue",
- "//aos/ipc_lib:queue",
- "//aos/messages",
- ],
-)
-
-queue_library(
- name = "test_queue",
- srcs = [
- "test_queue.q",
- ],
-)
-
-genrule(
- name = "gen_queue_primitives",
- outs = ["queue_primitives.h"],
- cmd = "$(location //aos/build/queues:queue_primitives) $@",
- tools = ["//aos/build/queues:queue_primitives"],
- visibility = ["//visibility:private"],
-)
-
-cc_library(
- name = "generated_queue_headers",
- hdrs = [
- ":gen_queue_primitives",
- ],
- visibility = ["//aos/logging:__pkg__"],
-)
-
-cc_test(
- name = "queue_test",
- srcs = [
- "queue_test.cc",
- ],
- deps = [
- ":test_queue",
- "//aos:die",
- "//aos/testing:googletest",
- "//aos/testing:test_shm",
- "//aos/util:thread",
- ],
-)
-
cc_library(
name = "unique_malloc_ptr",
hdrs = [
@@ -229,8 +134,8 @@
visibility = ["//visibility:public"],
deps = [
"//aos/ipc_lib:aos_sync",
- "//aos/logging",
"//aos/mutex",
+ "@com_github_google_glog//:glog",
],
)
@@ -291,8 +196,8 @@
visibility = ["//visibility:public"],
deps = [
"//aos/ipc_lib:aos_sync",
- "//aos/logging",
"//aos/time",
+ "@com_github_google_glog//:glog",
],
)
@@ -322,14 +227,6 @@
)
cc_library(
- name = "queue",
- hdrs = [
- "queue-tmpl.h",
- ],
- visibility = ["//aos:__pkg__"],
-)
-
-cc_library(
name = "complex_thread_local",
srcs = [
"complex_thread_local.cc",
@@ -370,15 +267,31 @@
],
visibility = ["//visibility:public"],
deps = [
+ ":realtime",
"//aos:die",
"//aos/ipc_lib:shared_mem",
"//aos/logging:implementations",
],
)
+cc_library(
+ name = "realtime",
+ srcs = [
+ "realtime.cc",
+ ],
+ hdrs = [
+ "realtime.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_google_glog//:glog",
+ ],
+)
+
flatbuffer_cc_library(
- name = "configuration_flatbuffer",
+ name = "configuration_fbs",
srcs = ["configuration.fbs"],
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -391,13 +304,12 @@
],
visibility = ["//visibility:public"],
deps = [
- ":configuration_flatbuffer",
+ ":configuration_fbs",
":flatbuffer_merge",
":flatbuffers",
":json_to_flatbuffer",
"@com_google_absl//absl/base",
"//aos:unique_malloc_ptr",
- "//aos/logging",
"//aos/util:file",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/container:btree",
@@ -443,6 +355,7 @@
srcs = ["json_tokenizer.cc"],
hdrs = ["json_tokenizer.h"],
deps = [
+ "@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
],
)
@@ -451,11 +364,11 @@
name = "json_to_flatbuffer",
srcs = ["json_to_flatbuffer.cc"],
hdrs = ["json_to_flatbuffer.h"],
+ visibility = ["//visibility:public"],
deps = [
":flatbuffer_utils",
":flatbuffers",
":json_tokenizer",
- "//aos/logging",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
@@ -479,6 +392,7 @@
srcs = ["flatbuffer_merge.cc"],
hdrs = ["flatbuffer_merge.h"],
copts = ["-Wno-cast-align"],
+ visibility = ["//visibility:public"],
deps = [
":flatbuffer_utils",
":flatbuffers",
@@ -511,6 +425,7 @@
deps = [
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings",
],
)
@@ -520,11 +435,14 @@
"configuration_test.cc",
],
data = [
+ "testdata/backwards.json",
"testdata/config1.json",
"testdata/config1_bad.json",
"testdata/config2.json",
"testdata/config3.json",
"testdata/expected.json",
+ "//aos/events:config.fb.json",
+ "//aos/events:pingpong.bfbs",
],
deps = [
":configuration",
@@ -532,3 +450,17 @@
"//aos/testing:test_logging",
],
)
+
+cc_binary(
+ name = "config_flattener",
+ srcs = [
+ "config_flattener.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":configuration",
+ ":init",
+ "//aos/util:file",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/aos/actions/BUILD b/aos/actions/BUILD
index 25cc6e3..fe79a4a 100644
--- a/aos/actions/BUILD
+++ b/aos/actions/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_library(
name = "action_lib",
@@ -13,30 +13,22 @@
"actor.h",
],
deps = [
- "//aos:queues",
+ ":actions_fbs",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
"//aos/util:phased_loop",
],
)
-queue_library(
- name = "action_queue",
- srcs = [
- "actions.q",
- ],
+flatbuffer_cc_library(
+ name = "actions_fbs",
+ srcs = ["actions.fbs"],
)
-queue_library(
- name = "test_action_queue",
- srcs = [
- "test_action.q",
- ],
- deps = [
- ":action_queue",
- ],
+flatbuffer_cc_library(
+ name = "test_action_fbs",
+ srcs = ["test_action.fbs"],
)
cc_test(
@@ -44,15 +36,14 @@
srcs = [
"action_test.cc",
],
+ data = ["action_test_config.json"],
deps = [
":action_lib",
- ":action_queue",
- ":test_action_queue",
+ ":actions_fbs",
+ ":test_action_fbs",
"//aos:event",
- "//aos:queues",
"//aos/events:simulated_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/testing:googletest",
"//aos/testing:test_shm",
"//aos/time",
diff --git a/aos/actions/action_test.cc b/aos/actions/action_test.cc
index 178b2b0..aa0e6d2 100644
--- a/aos/actions/action_test.cc
+++ b/aos/actions/action_test.cc
@@ -7,11 +7,10 @@
#include "gtest/gtest.h"
#include "aos/actions/actions.h"
-#include "aos/actions/actions.q.h"
+#include "aos/actions/actions_generated.h"
#include "aos/actions/actor.h"
-#include "aos/actions/test_action.q.h"
-#include "aos/events/simulated-event-loop.h"
-#include "aos/queue.h"
+#include "aos/actions/test_action_generated.h"
+#include "aos/events/simulated_event_loop.h"
#include "aos/testing/test_logging.h"
#include "aos/testing/test_shm.h"
@@ -20,24 +19,24 @@
namespace actions {
namespace testing {
-
namespace chrono = ::std::chrono;
class TestActorIndex
- : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ : public aos::common::actions::ActorBase<actions::TestActionGoal> {
public:
- typedef TypedActionFactory<actions::TestActionQueueGroup> Factory;
+ typedef TypedActionFactory<actions::TestActionGoal> Factory;
explicit TestActorIndex(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actions::TestActionQueueGroup>(
- event_loop, ".aos.common.actions.test_action") {}
+ : aos::common::actions::ActorBase<actions::TestActionGoal>(
+ event_loop, "/test_action") {}
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".aos.common.actions.test_action");
+ return Factory(event_loop, "/test_action");
}
- bool RunAction(const uint32_t &new_index) override {
- index = new_index;
+ bool RunAction(const UInt *new_index) override {
+ VLOG(1) << "New index " << FlatbufferToJson(new_index);
+ index = new_index->val();
return true;
}
@@ -45,35 +44,35 @@
};
class TestActorNOP
- : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ : public aos::common::actions::ActorBase<actions::TestActionGoal> {
public:
- typedef TypedActionFactory<actions::TestActionQueueGroup> Factory;
+ typedef TypedActionFactory<actions::TestActionGoal> Factory;
explicit TestActorNOP(::aos::EventLoop *event_loop)
- : actions::ActorBase<actions::TestActionQueueGroup>(
- event_loop, ".aos.common.actions.test_action") {}
+ : actions::ActorBase<actions::TestActionGoal>(
+ event_loop, "/test_action") {}
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".aos.common.actions.test_action");
+ return Factory(event_loop, "/test_action");
}
- bool RunAction(const uint32_t &) override { return true; }
+ bool RunAction(const UInt *) override { return true; }
};
class TestActorShouldCancel
- : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ : public aos::common::actions::ActorBase<actions::TestActionGoal> {
public:
- typedef TypedActionFactory<actions::TestActionQueueGroup> Factory;
+ typedef TypedActionFactory<actions::TestActionGoal> Factory;
explicit TestActorShouldCancel(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actions::TestActionQueueGroup>(
- event_loop, ".aos.common.actions.test_action") {}
+ : aos::common::actions::ActorBase<actions::TestActionGoal>(
+ event_loop, "/test_action") {}
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".aos.common.actions.test_action");
+ return Factory(event_loop, "/test_action");
}
- bool RunAction(const uint32_t &) override {
+ bool RunAction(const UInt *) override {
while (!ShouldCancel()) {
AOS_LOG(FATAL, "NOT CANCELED!!\n");
}
@@ -82,37 +81,41 @@
};
class TestActor2Nop
- : public aos::common::actions::ActorBase<actions::TestAction2QueueGroup> {
+ : public aos::common::actions::ActorBase<actions::TestAction2Goal> {
public:
- typedef TypedActionFactory<actions::TestAction2QueueGroup> Factory;
+ typedef TypedActionFactory<actions::TestAction2Goal> Factory;
explicit TestActor2Nop(::aos::EventLoop *event_loop)
- : actions::ActorBase<actions::TestAction2QueueGroup>(
- event_loop, ".aos.common.actions.test_action2") {}
+ : actions::ActorBase<actions::TestAction2Goal>(
+ event_loop, "/test_action2") {}
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".aos.common.actions.test_action2");
+ return Factory(event_loop, "/test_action2");
}
- bool RunAction(const actions::MyParams &) { return true; }
+ bool RunAction(const actions::MyParams *) { return true; }
};
class ActionTest : public ::testing::Test {
protected:
ActionTest()
- : actor1_event_loop_(event_loop_factory_.MakeEventLoop()),
+ : configuration_(
+ configuration::ReadConfig("aos/actions/action_test_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ actor1_event_loop_(event_loop_factory_.MakeEventLoop()),
actor2_event_loop_(event_loop_factory_.MakeEventLoop()),
test_event_loop_(event_loop_factory_.MakeEventLoop()) {
::aos::testing::EnableTestLogging();
}
+ FlatbufferDetachedBuffer<Configuration> configuration_;
+
// Bring up and down Core.
::aos::SimulatedEventLoopFactory event_loop_factory_;
::std::unique_ptr<::aos::EventLoop> actor1_event_loop_;
::std::unique_ptr<::aos::EventLoop> actor2_event_loop_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
-
};
// Tests that the the actions exist in a safe state at startup.
@@ -130,11 +133,10 @@
TEST_F(ActionTest, StartWithOldGoal) {
::std::unique_ptr<::aos::EventLoop> test2_event_loop =
event_loop_factory_.MakeEventLoop();
- ::aos::Sender<TestActionQueueGroup::Goal> goal_sender =
- test2_event_loop->MakeSender<TestActionQueueGroup::Goal>(
- ".aos.common.actions.test_action.goal");
- ::aos::Fetcher<Status> status_fetcher = test2_event_loop->MakeFetcher<Status>(
- ".aos.common.actions.test_action.status");
+ ::aos::Sender<TestActionGoal> goal_sender =
+ test2_event_loop->MakeSender<TestActionGoal>("/test_action");
+ ::aos::Fetcher<Status> status_fetcher =
+ test2_event_loop->MakeFetcher<Status>("/test_action");
TestActorIndex::Factory nop_actor_factory =
TestActorNOP::MakeFactory(test_event_loop_.get());
@@ -142,9 +144,14 @@
ActionQueue action_queue;
{
- auto goal_message = goal_sender.MakeMessage();
- goal_message->run = 971;
- ASSERT_TRUE(goal_message.Send());
+ ::aos::Sender<TestActionGoal>::Builder builder =
+ goal_sender.MakeBuilder();
+
+ TestActionGoal::Builder goal_builder =
+ builder.MakeBuilder<TestActionGoal>();
+
+ goal_builder.add_run(971);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
TestActorNOP nop_act(actor1_event_loop_.get());
@@ -154,10 +161,14 @@
event_loop_factory_.RunFor(chrono::seconds(1));
ASSERT_TRUE(status_fetcher.Fetch());
- EXPECT_EQ(0u, status_fetcher->running);
- EXPECT_EQ(0u, status_fetcher->last_running);
+ EXPECT_EQ(0u, status_fetcher->running());
+ EXPECT_EQ(0u, status_fetcher->last_running());
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ }
// We started an action and it should be running.
EXPECT_TRUE(action_queue.Running());
@@ -189,7 +200,11 @@
action_queue.Tick();
EXPECT_FALSE(action_queue.Running());
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ }
// We started an action and it should be running.
EXPECT_TRUE(action_queue.Running());
@@ -223,8 +238,12 @@
// Enqueue two actions to test both cancel. We can have an action and a next
// action so we want to test that.
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ }
action_queue.Tick();
@@ -274,7 +293,11 @@
action_queue.Tick();
// Enqueue blocking action.
- action_queue.EnqueueAction(cancel_action_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(cancel_action_factory.Make(uint));
+ }
action_queue.Tick();
EXPECT_TRUE(action_queue.Running());
@@ -306,7 +329,11 @@
EXPECT_FALSE(action_queue.Running());
// Enqueue action to be canceled.
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ }
action_queue.Tick();
// Should still be running as the actor could not have signalled.
@@ -325,7 +352,11 @@
ASSERT_NE(0u, nop_actor_id);
// Add the next action which should ensure the first stopped.
- action_queue.EnqueueAction(nop_actor_factory.Make(0));
+ {
+ UIntT uint;
+ uint.val = 0;
+ action_queue.EnqueueAction(nop_actor_factory.Make(uint));
+ }
// id for the second run.
uint32_t nop_actor2_id = 0;
@@ -382,13 +413,17 @@
EXPECT_FALSE(action_queue.Running());
// Enqueue action to post index.
- action_queue.EnqueueAction(test_actor_index_factory.Make(5));
- ::aos::Fetcher<actions::TestActionQueueGroup::Goal> goal_fetcher_ =
- test_event_loop_->MakeFetcher<actions::TestActionQueueGroup::Goal>(
- ".aos.common.actions.test_action.goal");
+ {
+ UIntT uint;
+ uint.val = 5;
+ action_queue.EnqueueAction(test_actor_index_factory.Make(uint));
+ }
+ ::aos::Fetcher<actions::TestActionGoal> goal_fetcher_ =
+ test_event_loop_->MakeFetcher<actions::TestActionGoal>(
+ "/test_action");
ASSERT_TRUE(goal_fetcher_.Fetch());
- EXPECT_EQ(5u, goal_fetcher_->params);
+ EXPECT_EQ(5u, goal_fetcher_->params()->val());
EXPECT_EQ(0u, idx_actor.index);
action_queue.Tick();
@@ -400,9 +435,13 @@
EXPECT_EQ(5u, idx_actor.index);
// Enqueue action to post index.
- action_queue.EnqueueAction(test_actor_index_factory.Make(3));
+ {
+ UIntT uint;
+ uint.val = 3;
+ action_queue.EnqueueAction(test_actor_index_factory.Make(uint));
+ }
ASSERT_TRUE(goal_fetcher_.Fetch());
- EXPECT_EQ(3u, goal_fetcher_->params);
+ EXPECT_EQ(3u, goal_fetcher_->params()->val());
// Run the next action so it can accomplish signal completion.
event_loop_factory_.RunFor(chrono::seconds(1));
@@ -423,7 +462,7 @@
action_queue.Tick();
EXPECT_FALSE(action_queue.Running());
- actions::MyParams p;
+ actions::MyParamsT p;
p.param1 = 5.0;
p.param2 = 7;
diff --git a/aos/actions/action_test_config.json b/aos/actions/action_test_config.json
new file mode 100644
index 0000000..e9b6f16
--- /dev/null
+++ b/aos/actions/action_test_config.json
@@ -0,0 +1,20 @@
+{
+ "channels": [
+ {
+ "name": "/test_action",
+ "type": "aos.common.actions.TestActionGoal"
+ },
+ {
+ "name": "/test_action",
+ "type": "aos.common.actions.Status"
+ },
+ {
+ "name": "/test_action2",
+ "type": "aos.common.actions.TestAction2Goal"
+ },
+ {
+ "name": "/test_action2",
+ "type": "aos.common.actions.Status"
+ }
+ ]
+}
diff --git a/aos/actions/actions.fbs b/aos/actions/actions.fbs
new file mode 100644
index 0000000..0ce41fc
--- /dev/null
+++ b/aos/actions/actions.fbs
@@ -0,0 +1,24 @@
+namespace aos.common.actions;
+
+table Status {
+ // The run value of the instance we're currently running or 0.
+ running:uint;
+ // A run value we were previously running or 0.
+ last_running:uint;
+ // If false the action failed to complete and may be in a bad state,
+ // this is a critical problem not a cancellation.
+ success:bool;
+}
+
+table DoubleParam {
+ val:double;
+}
+
+table Goal {
+ // The unique value to put into status.running while running this instance or
+ // 0 to cancel.
+ run:uint;
+ // Default parameter. The more useful thing to do would be to define your own
+ // goal type to change param to a useful structure.
+ params:DoubleParam;
+}
diff --git a/aos/actions/actions.h b/aos/actions/actions.h
index e2b9ca8..d01b3f5 100644
--- a/aos/actions/actions.h
+++ b/aos/actions/actions.h
@@ -9,10 +9,10 @@
#include <atomic>
#include <memory>
-#include "aos/events/event-loop.h"
+#include "aos/actions/actions_generated.h"
+#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/queue.h"
namespace aos {
namespace common {
@@ -105,31 +105,28 @@
class TypedAction : public Action {
public:
// A convenient way to refer to the type of our goals.
+ typedef T GoalType;
typedef typename std::remove_reference<decltype(
- *(static_cast<T*>(nullptr)->goal.MakeMessage().get()))>::type GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<T*>(nullptr)->status.MakeMessage().get()))>::type StatusType;
- typedef typename std::remove_reference<
- decltype(static_cast<GoalType*>(nullptr)->params)>::type ParamType;
+ *static_cast<GoalType *>(nullptr)->params())>::type ParamType;
- TypedAction(typename ::aos::Fetcher<StatusType> *status_fetcher,
+ TypedAction(typename ::aos::Fetcher<Status> *status_fetcher,
typename ::aos::Sender<GoalType> *goal_sender,
- const ParamType ¶ms)
+ const typename ParamType::NativeTableType ¶ms)
: status_fetcher_(status_fetcher),
goal_sender_(goal_sender),
- goal_(goal_sender_->MakeMessage()),
// This adds 1 to the counter (atomically because it's potentially
// shared across threads) and then bitwise-ORs the bottom of the PID to
// differentiate it from other processes's values (ie a unique id).
run_value_(run_counter_.fetch_add(1, ::std::memory_order_relaxed) |
((getpid() & 0xFFFF) << 16)),
params_(params) {
- AOS_LOG(DEBUG, "Action %" PRIx32 " created on queue %s\n", run_value_,
- goal_sender_->name());
+ AOS_LOG(DEBUG, "Action %" PRIx32 " created on queue %.*s\n", run_value_,
+ static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
// Clear out any old status messages from before now.
status_fetcher_->Fetch();
if (status_fetcher_->get()) {
- AOS_LOG_STRUCT(DEBUG, "have status", *status_fetcher_->get());
+ VLOG(1) << "have status" << FlatbufferToJson(status_fetcher_->get());
}
}
@@ -164,9 +161,8 @@
if (old_run_value != nullptr) *old_run_value = old_run_value_;
}
- typename ::aos::Fetcher<StatusType> *status_fetcher_;
+ typename ::aos::Fetcher<Status> *status_fetcher_;
typename ::aos::Sender<GoalType> *goal_sender_;
- typename ::aos::Sender<GoalType>::Message goal_;
// Track if we have seen a response to the start message.
bool has_started_ = false;
@@ -182,7 +178,7 @@
const uint32_t run_value_;
// flag passed to action in order to have differing types
- const ParamType params_;
+ const typename ParamType::NativeTableType params_;
// The old value for running that we may have seen. If we see any value other
// than this or run_value_, somebody else got in the way and we're done. 0 if
@@ -196,20 +192,18 @@
template <typename T>
class TypedActionFactory {
public:
+ typedef T GoalType;
typedef typename std::remove_reference<decltype(
- *(static_cast<T*>(nullptr)->goal.MakeMessage().get()))>::type GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<T*>(nullptr)->status.MakeMessage().get()))>::type StatusType;
- typedef typename std::remove_reference<decltype(
- static_cast<GoalType *>(nullptr)->params)>::type ParamType;
+ *static_cast<GoalType *>(nullptr)->params())>::type ParamType;
explicit TypedActionFactory(::aos::EventLoop *event_loop,
const ::std::string &name)
: name_(name),
- status_fetcher_(event_loop->MakeFetcher<StatusType>(name + ".status")),
- goal_sender_(event_loop->MakeSender<GoalType>(name + ".goal")) {}
+ status_fetcher_(event_loop->MakeFetcher<Status>(name)),
+ goal_sender_(event_loop->MakeSender<GoalType>(name)) {}
- ::std::unique_ptr<TypedAction<T>> Make(const ParamType ¶m) {
+ ::std::unique_ptr<TypedAction<T>> Make(
+ const typename ParamType::NativeTableType ¶m) {
return ::std::unique_ptr<TypedAction<T>>(
new TypedAction<T>(&status_fetcher_, &goal_sender_, param));
}
@@ -221,7 +215,7 @@
private:
const ::std::string name_;
- typename ::aos::Fetcher<StatusType> status_fetcher_;
+ typename ::aos::Fetcher<Status> status_fetcher_;
typename ::aos::Sender<GoalType> goal_sender_;
};
@@ -231,25 +225,31 @@
template <typename T>
void TypedAction<T>::DoCancel() {
if (!sent_started_) {
- AOS_LOG(INFO, "Action %" PRIx32 " on queue %s was never started\n",
- run_value_, goal_sender_->name());
+ AOS_LOG(INFO, "Action %" PRIx32 " on queue %.*s was never started\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
} else {
if (interrupted_) {
AOS_LOG(INFO,
"Action %" PRIx32
- " on queue %s was interrupted -> not cancelling\n",
- run_value_, goal_sender_->name());
+ " on queue %.*s was interrupted -> not cancelling\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
} else {
if (sent_cancel_) {
- AOS_LOG(DEBUG, "Action %" PRIx32 " on queue %s already cancelled\n",
- run_value_, goal_sender_->name());
+ AOS_LOG(DEBUG, "Action %" PRIx32 " on queue %.*s already cancelled\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
} else {
- AOS_LOG(DEBUG, "Canceling action %" PRIx32 " on queue %s\n", run_value_,
- goal_sender_->name());
+ AOS_LOG(DEBUG, "Canceling action %" PRIx32 " on queue %.*s\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
{
- auto goal_message = goal_sender_->MakeMessage();
- goal_message->run = 0;
- goal_message.Send();
+ auto builder = goal_sender_->MakeBuilder();
+ typename GoalType::Builder goal_builder =
+ builder.template MakeBuilder<GoalType>();
+ goal_builder.add_run(0);
+ builder.Send(goal_builder.Finish());
}
sent_cancel_ = true;
}
@@ -268,7 +268,7 @@
CheckInterrupted();
} else {
while (status_fetcher_->FetchNext()) {
- AOS_LOG_STRUCT(DEBUG, "got status", *status_fetcher_->get());
+ VLOG(1) << "got status" << FlatbufferToJson(status_fetcher_->get());
CheckStarted();
if (has_started_) CheckInterrupted();
}
@@ -278,7 +278,7 @@
// yet.
if (!has_started_) return true;
return status_fetcher_->get() &&
- status_fetcher_->get()->running == run_value_;
+ status_fetcher_->get()->running() == run_value_;
}
template <typename T>
@@ -289,11 +289,11 @@
if (!status_fetcher_->FetchNext()) {
return false;
}
- AOS_LOG_STRUCT(DEBUG, "got status", *status_fetcher_->get());
+ VLOG(1) << "got status" << FlatbufferToJson(status_fetcher_->get());
CheckStarted();
CheckInterrupted();
if (has_started_ && (status_fetcher_->get() &&
- status_fetcher_->get()->running != run_value_)) {
+ status_fetcher_->get()->running() != run_value_)) {
return true;
}
return false;
@@ -303,23 +303,24 @@
void TypedAction<T>::CheckStarted() {
if (has_started_) return;
if (status_fetcher_->get()) {
- if (status_fetcher_->get()->running == run_value_ ||
- (status_fetcher_->get()->running == 0 &&
- status_fetcher_->get()->last_running == run_value_)) {
+ if (status_fetcher_->get()->running() == run_value_ ||
+ (status_fetcher_->get()->running() == 0 &&
+ status_fetcher_->get()->last_running() == run_value_)) {
// It's currently running our instance.
has_started_ = true;
- AOS_LOG(DEBUG, "Action %" PRIx32 " on queue %s has been started\n",
- run_value_, goal_sender_->name());
+ AOS_LOG(DEBUG, "Action %" PRIx32 " on queue %.*s has been started\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
} else if (old_run_value_ != 0 &&
- status_fetcher_->get()->running == old_run_value_) {
+ status_fetcher_->get()->running() == old_run_value_) {
AOS_LOG(DEBUG, "still running old instance %" PRIx32 "\n",
old_run_value_);
} else {
AOS_LOG(WARNING,
- "Action %" PRIx32 " on queue %s interrupted by %" PRIx32
+ "Action %" PRIx32 " on queue %.*s interrupted by %" PRIx32
" before starting\n",
- run_value_, goal_sender_->name(),
- status_fetcher_->get()->running);
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data(), status_fetcher_->get()->running());
has_started_ = true;
interrupted_ = true;
}
@@ -331,13 +332,13 @@
template <typename T>
void TypedAction<T>::CheckInterrupted() {
if (!interrupted_ && has_started_ && status_fetcher_->get()) {
- if (status_fetcher_->get()->running != 0 &&
- status_fetcher_->get()->running != run_value_) {
+ if (status_fetcher_->get()->running() != 0 &&
+ status_fetcher_->get()->running() != run_value_) {
AOS_LOG(WARNING,
- "Action %" PRIx32 " on queue %s interrupted by %" PRIx32
+ "Action %" PRIx32 " on queue %.*s interrupted by %" PRIx32
" after starting\n",
- run_value_, goal_sender_->name(),
- status_fetcher_->get()->running);
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data(), status_fetcher_->get()->running());
}
}
}
@@ -346,29 +347,38 @@
void TypedAction<T>::DoStart() {
if (!sent_started_) {
AOS_LOG(DEBUG, "Starting action %" PRIx32 "\n", run_value_);
- goal_->run = run_value_;
- goal_->params = params_;
+ auto builder = goal_sender_->MakeBuilder();
+ auto params_offset = ParamType::Pack(*builder.fbb(), ¶ms_);
+
+ auto goal_builder = builder.template MakeBuilder<GoalType>();
+ goal_builder.add_params(params_offset);
+ goal_builder.add_run(run_value_);
+
sent_started_ = true;
- if (!goal_.Send()) {
- AOS_LOG(ERROR, "sending goal for action %" PRIx32 " on queue %s failed\n",
- run_value_, goal_sender_->name());
+ if (!builder.Send(goal_builder.Finish())) {
+ AOS_LOG(ERROR,
+ "sending goal for action %" PRIx32 " on queue %.*s failed\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
// Don't wait to see a message with it.
has_started_ = true;
}
status_fetcher_->FetchNext();
if (status_fetcher_->get()) {
- AOS_LOG_STRUCT(DEBUG, "got status", *status_fetcher_->get());
+ VLOG(1) << "got status" << FlatbufferToJson(status_fetcher_->get());
}
- if (status_fetcher_->get() && status_fetcher_->get()->running != 0) {
- old_run_value_ = status_fetcher_->get()->running;
- AOS_LOG(INFO, "Action %" PRIx32 " on queue %s already running\n",
- old_run_value_, goal_sender_->name());
+ if (status_fetcher_->get() && status_fetcher_->get()->running() != 0) {
+ old_run_value_ = status_fetcher_->get()->running();
+ AOS_LOG(INFO, "Action %" PRIx32 " on queue %.*s already running\n",
+ old_run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
} else {
old_run_value_ = 0;
}
} else {
- AOS_LOG(WARNING, "Action %" PRIx32 " on queue %s already started\n",
- run_value_, goal_sender_->name());
+ AOS_LOG(WARNING, "Action %" PRIx32 " on queue %.*s already started\n",
+ run_value_, static_cast<int>(goal_sender_->name().size()),
+ goal_sender_->name().data());
}
}
diff --git a/aos/actions/actions.q b/aos/actions/actions.q
deleted file mode 100644
index e3046c9..0000000
--- a/aos/actions/actions.q
+++ /dev/null
@@ -1,35 +0,0 @@
-package aos.common.actions;
-
-interface StatusInterface {
- // 0 if the action isn't running or the value from goal.run.
- uint32_t running;
-};
-
-interface GoalInterface {
- // 0 to stop or an arbitrary value to put in status.running.
- uint32_t run;
-};
-
-message Status {
- // The run value of the instance we're currently running or 0.
- uint32_t running;
- // A run value we were previously running or 0.
- uint32_t last_running;
- // If false the action failed to complete and may be in a bad state,
- // this is a critical problem not a cancellation.
- bool success;
-};
-
-message Goal {
- // The unique value to put into status.running while running this instance or
- // 0 to cancel.
- uint32_t run;
- // Default parameter. The more useful thing to do would be to define your own
- // goal type to change param to a useful structure.
- double params;
-};
-
-interface ActionQueueGroup {
- queue Status status;
- queue Goal goal;
-};
diff --git a/aos/actions/actor.h b/aos/actions/actor.h
index a9b75e6..f5629a6 100644
--- a/aos/actions/actor.h
+++ b/aos/actions/actor.h
@@ -7,9 +7,9 @@
#include <chrono>
#include <functional>
+#include "aos/actions/actions_generated.h"
#include "aos/controls/control_loop.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
@@ -20,29 +20,27 @@
template <class T>
class ActorBase {
public:
+ typedef T GoalType;
typedef typename std::remove_reference<decltype(
- *(static_cast<T *>(nullptr)->goal.MakeMessage().get()))>::type GoalType;
- typedef typename std::remove_reference<decltype(*(
- static_cast<T *>(nullptr)->status.MakeMessage().get()))>::type StatusType;
- typedef typename std::remove_reference<decltype(
- static_cast<GoalType *>(nullptr)->params)>::type ParamType;
+ *static_cast<GoalType *>(nullptr)->params())>::type ParamType;
ActorBase(::aos::EventLoop *event_loop, const ::std::string &name)
: event_loop_(event_loop),
- status_sender_(event_loop->MakeSender<StatusType>(name + ".status")),
- goal_fetcher_(event_loop->MakeFetcher<GoalType>(name + ".goal")) {
+ status_sender_(event_loop->MakeSender<Status>(name)),
+ goal_fetcher_(event_loop->MakeFetcher<GoalType>(name)) {
AOS_LOG(INFO, "Constructing action %s\n", name.c_str());
- event_loop->MakeWatcher(name + ".goal",
+ event_loop->MakeWatcher(name,
[this](const GoalType &goal) { HandleGoal(goal); });
// Send out an inital status saying we aren't running to wake up any users
// who might be waiting forever for the previous action.
event_loop->OnRun([this]() {
- auto status_message = status_sender_.MakeMessage();
- status_message->running = 0;
- status_message->last_running = 0;
- status_message->success = !abort_;
- if (!status_message.Send()) {
+ auto builder = status_sender_.MakeBuilder();
+ Status::Builder status_builder = builder.template MakeBuilder<Status>();
+ status_builder.add_running(0);
+ status_builder.add_last_running(0);
+ status_builder.add_success(!abort_);
+ if (!builder.Send(status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send the status.\n");
}
});
@@ -99,7 +97,7 @@
// Will return true if finished or asked to cancel.
// Will return false if it failed accomplish its goal
// due to a problem with the system.
- virtual bool RunAction(const ParamType& params) = 0;
+ virtual bool RunAction(const ParamType *params) = 0;
void HandleGoal(const GoalType &goal);
@@ -110,7 +108,7 @@
uint32_t current_id_ = 0;
- ::aos::Sender<StatusType> status_sender_;
+ ::aos::Sender<Status> status_sender_;
::aos::Fetcher<GoalType> goal_fetcher_;
State state_ = State::WAITING_FOR_ACTION;
@@ -118,48 +116,51 @@
template <class T>
void ActorBase<T>::HandleGoal(const GoalType &goal) {
- AOS_LOG_STRUCT(DEBUG, "action goal", goal);
+ VLOG(1) << "action goal " << FlatbufferToJson(&goal);
switch (state_) {
case State::WAITING_FOR_ACTION:
- if (goal.run) {
+ if (goal.run()) {
state_ = State::RUNNING_ACTION;
} else {
- auto status_message = status_sender_.MakeMessage();
- status_message->running = 0;
- status_message->last_running = 0;
- status_message->success = !abort_;
- if (!status_message.Send()) {
+ auto builder = status_sender_.MakeBuilder();
+ Status::Builder status_builder = builder.template MakeBuilder<Status>();
+ status_builder.add_running(0);
+ status_builder.add_last_running(0);
+ status_builder.add_success(!abort_);
+ if (!builder.Send(status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send the status.\n");
}
break;
}
case State::RUNNING_ACTION: {
++running_count_;
- const uint32_t running_id = goal.run;
+ const uint32_t running_id = goal.run();
current_id_ = running_id;
AOS_LOG(INFO, "Starting action %" PRIx32 "\n", running_id);
{
- auto status_message = status_sender_.MakeMessage();
- status_message->running = running_id;
- status_message->last_running = 0;
- status_message->success = !abort_;
- if (!status_message.Send()) {
+ auto builder = status_sender_.MakeBuilder();
+ Status::Builder status_builder = builder.template MakeBuilder<Status>();
+ status_builder.add_running(running_id);
+ status_builder.add_last_running(0);
+ status_builder.add_success(!abort_);
+ if (!builder.Send(status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send the status.\n");
}
}
- AOS_LOG_STRUCT(INFO, "goal", goal);
- abort_ = !RunAction(goal.params);
+ VLOG(1) << "goal " << FlatbufferToJson(&goal);
+ abort_ = !RunAction(goal.params());
AOS_LOG(INFO, "Done with action %" PRIx32 "\n", running_id);
current_id_ = 0u;
{
- auto status_message = status_sender_.MakeMessage();
- status_message->running = 0;
- status_message->last_running = running_id;
- status_message->success = !abort_;
+ auto builder = status_sender_.MakeBuilder();
+ Status::Builder status_builder = builder.template MakeBuilder<Status>();
+ status_builder.add_running(0);
+ status_builder.add_last_running(running_id);
+ status_builder.add_success(!abort_);
- if (!status_message.Send()) {
+ if (!builder.Send(status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send the status.\n");
} else {
AOS_LOG(INFO, "Sending Done status %" PRIx32 "\n", running_id);
@@ -171,7 +172,7 @@
running_id);
} break;
case State::WAITING_FOR_STOPPED:
- if (goal.run == 0) {
+ if (goal.run() == 0) {
AOS_LOG(INFO, "Action stopped.\n");
state_ = State::WAITING_FOR_ACTION;
}
@@ -211,9 +212,9 @@
template <class T>
bool ActorBase<T>::ShouldCancel() {
if (goal_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "goal queue", *goal_fetcher_);
+ VLOG(1) << "goal queue " << FlatbufferToJson(goal_fetcher_.get());
}
- bool ans = !goal_fetcher_->run || goal_fetcher_->run != current_id_;
+ bool ans = !goal_fetcher_->run() || goal_fetcher_->run() != current_id_;
if (ans) {
AOS_LOG(INFO, "Time to stop action\n");
}
diff --git a/aos/actions/test_action.fbs b/aos/actions/test_action.fbs
new file mode 100644
index 0000000..f7f8052
--- /dev/null
+++ b/aos/actions/test_action.fbs
@@ -0,0 +1,20 @@
+namespace aos.common.actions;
+
+table UInt {
+ val:uint;
+}
+
+table TestActionGoal {
+ run:uint;
+ params:UInt;
+}
+
+table MyParams {
+ param1:double;
+ param2:int;
+}
+
+table TestAction2Goal {
+ run:uint;
+ params:MyParams;
+}
diff --git a/aos/actions/test_action.q b/aos/actions/test_action.q
deleted file mode 100644
index f2d268d..0000000
--- a/aos/actions/test_action.q
+++ /dev/null
@@ -1,32 +0,0 @@
-package aos.common.actions;
-
-import "aos/actions/actions.q";
-
-queue_group TestActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- uint32_t params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-struct MyParams {
- double param1;
- int32_t param2;
-};
-
-queue_group TestAction2QueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- MyParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/aos/build/BUILD b/aos/build/BUILD
deleted file mode 100644
index e69de29..0000000
--- a/aos/build/BUILD
+++ /dev/null
diff --git a/aos/build/queues.bzl b/aos/build/queues.bzl
deleted file mode 100644
index 07a91e4..0000000
--- a/aos/build/queues.bzl
+++ /dev/null
@@ -1,117 +0,0 @@
-def _single_queue_file_impl(ctx):
- args = [
- '-h_file_path', ctx.outputs.header.path,
- '-cc_file_path', ctx.outputs.cc.path,
- '-src_filename', ctx.file.src.short_path,
- '-I', '.',
- ctx.file.src.path,
- ]
- ctx.action(
- outputs = [
- ctx.outputs.header,
- ctx.outputs.cc,
- ],
- inputs = [ ctx.file.src ] + ctx.attr.q_deps.transitive_q_files,
- executable = ctx.executable._queue_compiler,
- arguments = args,
- mnemonic = 'QGen',
- progress_message = 'Generating C++ code for %s' % ctx.file.src.short_path,
- )
-
-def _single_queue_file_outputs(src):
- return {
- 'header': src.name + '.h',
- 'cc': src.name + '.cc',
- }
-
-_single_queue_file = rule(
- attrs = {
- "src": attr.label(
- mandatory = True,
- single_file = True,
- allow_files = [".q"],
- ),
- "q_deps": attr.label(
- providers = ["transitive_q_files"],
- mandatory = True,
- ),
- "package_name": attr.string(
- mandatory = True,
- ),
- "_queue_compiler": attr.label(
- executable = True,
- default = Label("//aos/build/queues:compiler"),
- cfg = "host",
- ),
- },
- output_to_genfiles = True,
- outputs = _single_queue_file_outputs,
- implementation = _single_queue_file_impl,
-)
-
-def _q_deps_impl(ctx):
- transitive_q_files = ctx.files.srcs
- for dep in ctx.attr.deps:
- transitive_q_files = transitive_q_files + dep.transitive_q_files
- return struct(transitive_q_files = transitive_q_files)
-
-_q_deps = rule(
- attrs = {
- "srcs": attr.label_list(
- mandatory = True,
- non_empty = True,
- allow_files = [".q"],
- ),
- "deps": attr.label_list(
- mandatory = True,
- non_empty = False,
- providers = ["transitive_q_files"],
- ),
- },
- implementation = _q_deps_impl,
-)
-
-"""Creates a C++ library from a set of .q files.
-
-Attrs:
- srcs: A list of .q files.
- deps: Other queue_library rules this one depends on.
-"""
-
-def queue_library(name, srcs, deps = [],
- compatible_with = None, restricted_to = None,
- visibility = None):
- q_deps = _q_deps(
- name = name + '__q_deps',
- srcs = srcs,
- deps = [dep + '__q_deps' for dep in deps],
- visibility = visibility,
- compatible_with = compatible_with,
- restricted_to = restricted_to,
- )
-
- for src in srcs:
- _single_queue_file(
- name = name + '_' + src,
- src = src,
- q_deps = ':%s__q_deps' % name,
- package_name = native.package_name(),
- visibility = ['//visibility:private'],
- compatible_with = compatible_with,
- restricted_to = restricted_to,
- )
-
- native.cc_library(
- name = name,
- srcs = [src + '.cc' for src in srcs],
- hdrs = [src + '.h' for src in srcs],
- deps = deps + [
- '//aos:once',
- '//aos:queues',
- '//aos:queue_types',
- '//aos/logging:printf_formats',
- ],
- visibility = visibility,
- compatible_with = compatible_with,
- restricted_to = restricted_to,
- )
diff --git a/aos/build/queues/BUILD b/aos/build/queues/BUILD
deleted file mode 100644
index 0ff6a87..0000000
--- a/aos/build/queues/BUILD
+++ /dev/null
@@ -1,45 +0,0 @@
-load("//tools/build_rules:ruby.bzl", "ruby_library", "ruby_binary")
-
-ruby_library(
- name = "lib",
- srcs = [
- "load.rb",
- "write_iff_changed.rb",
- ] + glob(["*/*.rb"]),
-)
-
-ruby_binary(
- name = "compiler",
- srcs = [
- "compiler.rb",
- ],
- data = [
- "@clang_3p6_repo//:clang-format",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":lib",
- ],
-)
-
-ruby_binary(
- name = "queue_primitives",
- srcs = [
- "queue_primitives.rb",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":lib",
- ],
-)
-
-ruby_binary(
- name = "print_field",
- srcs = [
- "print_field.rb",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":lib",
- ],
-)
diff --git a/aos/build/queues/compiler.rb b/aos/build/queues/compiler.rb
deleted file mode 100644
index 6ca141d..0000000
--- a/aos/build/queues/compiler.rb
+++ /dev/null
@@ -1,152 +0,0 @@
-require_relative "load.rb"
-
-def parse_args(globals,args)
- i = 0
- while(i < args.length)
- if(args[i] == "-I")
- args.delete_at(i)
- if(!args[i])
- $stderr.puts "hey! -I is followed by nothing."
- $stderr.puts "\tnot a supported usage..."
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- path = args.delete_at(i)
- globals.add_path(path)
- elsif(args[i] == "-cpp_out")
- args.delete_at(i)
- path = args.delete_at(i)
- if(path =~ /\./)
- $stderr.puts "hey! path #{path} has a \".\" char which is "
- $stderr.puts "\tnot a supported usage..."
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- elsif(!path)
- $stderr.puts "hey! No cpp_out path provided."
- $stderr.puts "\tumm, you could try -cpp_out \"\""
- $stderr.puts "\tThat might do the trick"
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- $cpp_out = path.split(/\\|\//)
- elsif(args[i] == "-cpp_base")
- args.delete_at(i)
- path = args.delete_at(i)
- $cpp_base = File.expand_path(path)
- if(!File.exists?($cpp_base))
- $stderr.puts "output directory #{$cpp_base.inspect} does not exist."
- $stderr.puts "\tI'm not going to make that! sheesh, who do you think I am?"
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- elsif(args[i] == "-src_filename")
- args.delete_at(i)
- $src_filename = args.delete_at(i)
- elsif(args[i] == "-h_file_path")
- args.delete_at(i)
- path = args.delete_at(i)
- $h_file_path = File.expand_path(path)
- if(!File.exists?(File.dirname($h_file_path)))
- $stderr.puts "directory of output #{$h_file_path.inspect} does not exist."
- $stderr.puts "\tI'm not going to make that! sheesh, who do you think I am?"
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- elsif(args[i] == "-cc_file_path")
- args.delete_at(i)
- path = args.delete_at(i)
- $cc_file_path = File.expand_path(path)
- if(!File.exists?(File.dirname($cc_file_path)))
- $stderr.puts "directory of output #{$cc_file_path.inspect} does not exist."
- $stderr.puts "\tI'm not going to make that! sheesh, who do you think I am?"
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- elsif(args[i] =~ /^-/)
- $stderr.puts "hey! unknown argument #{args[i]}."
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- else
- i += 1
- end
- end
- if ($cpp_base && $cpp_out) == ($src_filename && $h_file_path && $cc_file_path)
- $stderr.puts "hey! I'm not sure where to write the output files!"
- exit!(-1)
- end
-end
-def format_pipeline(output)
- read_in, write_in = IO.pipe()
- # TODO(phil): Is there a better way to use the sandboxed clang-format here?
- child = Process.spawn({'LD_LIBRARY_PATH' => './bazel-out/host/bin/aos/build/queues/compiler.runfiles/org_frc971/external/clang_3p6_repo/usr/lib/x86_64-linux-gnu'},
- './bazel-out/host/bin/aos/build/queues/compiler.runfiles/org_frc971/external/clang_3p6_repo/usr/bin/clang-format-3.6 --style=google',
- {:in=>read_in, write_in=>:close,
- :out=>output.fileno})
- read_in.close
- [child, write_in]
-end
-def build(filename,globals_template)
- globals = Globals.new()
- globals_template.paths.each do |path|
- globals.add_path(path)
- end
- filename = File.expand_path(filename)
- q_file = QFile.parse(filename)
- output_file = q_file.q_eval(globals)
- q_filename = File.basename(filename)
-
- if $cpp_base && $cpp_out
- $src_filename = ($cpp_out + [q_filename]).join("/")
- $h_file_path = $cpp_base + "/" + $src_filename + ".h"
- $cc_file_path = $cpp_base + "/" + $src_filename + ".cc"
- FileUtils.mkdir_p(Pathname.new($cpp_base) + $cpp_out.join("/"))
- end
-
- cpp_tree = output_file.make_cpp_tree($src_filename)
-
- cpp_tree.add_cc_include(($src_filename + ".h").inspect)
- cpp_tree.add_cc_include("aos/byteorder.h".inspect)
- cpp_tree.add_cc_include("<inttypes.h>")
- cpp_tree.add_cc_include("aos/queue_types.h".inspect)
- cpp_tree.add_cc_include("aos/once.h".inspect)
- cpp_tree.add_cc_include("aos/logging/printf_formats.h".inspect)
- cpp_tree.add_cc_using("::aos::to_network")
- cpp_tree.add_cc_using("::aos::to_host")
-
- header_file = WriteIffChanged.new($h_file_path)
- cc_file = WriteIffChanged.new($cc_file_path)
- header_child, header_output = format_pipeline(header_file)
- cc_child, cc_output = format_pipeline(cc_file)
- cpp_tree.write_header_file($cpp_base,header_output)
- cpp_tree.write_cc_file($cpp_base,cc_output)
- header_output.close()
- cc_output.close()
- if !Process.wait2(cc_child)[1].success?
- $stderr.puts "Formatting cc file failed."
- exit 1
- end
- if !Process.wait2(header_child)[1].success?
- $stderr.puts "Formatting header file failed."
- exit 1
- end
- header_file.close()
- cc_file.close()
-end
-begin
- args = ARGV.dup
- globals = Globals.new()
- parse_args(globals,args)
- if(args.length == 0)
- $stderr.puts "hey! you want me to do something,"
- $stderr.puts "\tbut you gave me no q files to build!"
- $stderr.puts "\tWot. Wot."
- exit!(-1)
- end
- args.each do |filename|
- build(filename,globals)
- end
- exit(0)
-rescue QError => e
- $stderr.print(e.to_s)
- exit!(-1)
-end
diff --git a/aos/build/queues/cpp_pretty_print/auto_gen.rb b/aos/build/queues/cpp_pretty_print/auto_gen.rb
deleted file mode 100644
index d3fb480..0000000
--- a/aos/build/queues/cpp_pretty_print/auto_gen.rb
+++ /dev/null
@@ -1,306 +0,0 @@
-module CPP
-end
-class CPP::Comment
- def initialize(text)
- @text = text
- end
- def pp(state)
- state.needs_semi = false
- if(@text.include?("\n"))
- state.print("/* #{@text} */")
- else
- state.print("// #{@text}")
- end
- end
-end
-class CPP::TODO < CPP::Comment
- def initialize(owner,text)
- @text = "TODO(#{owner}): #{text}"
- end
-end
-class CPP::StaticVar
- class ForwardDec
- def initialize(func) ; @func = func ; end
- def pp(state) ; @func.pp_forward_dec(state) ; end
- end
- attr_accessor :args, :static
- def initialize(type_class, val_type, name, args = CPP::Args.new())
- @type_class = type_class
- @val_type = val_type
- @name = name
- @args = args
- @static = true
- end
- def forward_dec() ; ForwardDec.new(self) ; end
- def pp_forward_dec(state)
- if (@static)
- state.print("static ")
- end
- state.print("#{@val_type} #{@name}")
- end
- def pp(state)
- state.print("#{@val_type} #{@type_class.chop_method_prefix}#{@name}(")
- state.pp(@args)
- state.print(")")
- end
- alias_method :pp_header_file, :pp_forward_dec
- alias_method :pp_cc_file, :pp
-end
-class CPP::MemberFunc
- class ForwardDec
- def initialize(func) ; @func = func ; end
- def pp(state) ; @func.pp_forward_dec(state) ; end
- end
- attr_accessor :args,:suite,:return_type,:name,:pre_func_types,:const,:static,:virtual
- def initialize(type_class,return_type,name)
- @type_class = type_class
- @return_type = return_type
- @name = name
- @const = false
- @static = false
- @virtual = false
- @args = CPP::Args.new()
- @suite = CPP::Suite.new()
- end
- attr_accessor :inline
- def forward_dec() ; ForwardDec.new(self) ; end
- def pp_forward_dec(state)
- return self.pp_inline(state) if(@inline)
- if (@static)
- state.print("static ")
- elsif (@virtual)
- state.print("virtual ")
- end
- state.print("#{@return_type} #{@pre_func_types}#{@name}(")
- state.pp(@args)
- state.print(")")
- if (@const)
- state.print(" const")
- end
- end
- def pp_inline(state)
- if (@static)
- state.print("static ")
- elsif (@virtual)
- state.print("virtual ")
- end
- state.print("#{@return_type} #{@pre_func_types}#{@name}(")
- state.pp(@args)
- state.print(") ")
- if (@const)
- state.print(" const")
- end
- @suite.pp_one_line(state)
- end
- def pp(state)
- return if(@inline)
- state.print("#{@return_type} #{@pre_func_types}#{@type_class.chop_method_prefix}#{@name}(")
- state.pp(@args)
- state.print(") ")
- if (@const)
- state.print("const ")
- end
- state.pp(@suite)
- state.v_pad(2)
- end
- alias_method :pp_header_file, :pp_forward_dec
- alias_method :pp_cc_file, :pp
-
-end
-class CPP::Constructor
- class ForwardDec
- def initialize(func) ; @func = func ; end
- def pp(state) ; @func.pp_forward_dec(state) ; end
- end
- attr_accessor :args,:suite,:return_type,:name
- def initialize(type_class)
- @type_class = type_class
- @args = CPP::Args.new()
- @suite = CPP::Suite.new()
- @var_cons = CPP::Args.new()
- end
- def forward_dec() ; ForwardDec.new(self) ; end
- def add_cons(*args)
- @var_cons.push(CPP::FuncCall.build(*args))
- end
- def pp_forward_dec(state)
- state.print("#{@type_class.name}(")
- state.pp(@args)
- state.print(")")
- end
- def pp_inline(state)
- pp(state,false)
- end
- def pp(state,prefix = true)
- state.needs_semi = false
- state.print(@type_class.chop_method_prefix) if(prefix)
- state.print("#{@type_class.name}(")
- state.pp(@args)
- if(@var_cons.length >= 1)
- state.print(")")
- state.endline()
- state.indent += 4
- state.print(": ")
- state.pp(@var_cons)
- state.indent -= 4
- state.print(" ")
- else
- state.print(") ")
- end
- state.pp(@suite)
- state.v_pad(2)
- end
- alias_method :pp_header_file, :pp_forward_dec
- alias_method :pp_cc_file, :pp
-end
-class CPP::Destructor
- class ForwardDec
- def initialize(func) ; @func = func ; end
- def pp(state) ; @func.pp_forward_dec(state) ; end
- end
- attr_accessor :args,:suite,:return_type,:name
- def initialize(type_class)
- @type_class = type_class
- @args = CPP::Args.new()
- @suite = CPP::Suite.new()
- end
- def forward_dec() ; ForwardDec.new(self) ; end
- def pp_forward_dec(state)
- state.print("~#{@type_class.name}(")
- state.pp(@args)
- state.print(")")
- end
- def pp(state)
- state.print("#{@type_class.chop_method_prefix}~#{@type_class.name}(")
- state.pp(@args)
- state.print(") ")
- state.pp(@suite)
- state.v_pad(2)
- end
- alias_method :pp_header_file, :pp_forward_dec
- alias_method :pp_cc_file, :pp
-end
-class CPP::Include
- attr_accessor :filename
- def initialize(filename)
- @filename = filename
- end
- def pp(state)
- state.needs_semi = false
- state.suppress_indent()
- state.print("#include #{@filename}")
- state.endline()
- end
-end
-class CPP::IncludeGuard
- attr_accessor :name,:suite
- def initialize(suite = CPP::Suite.new())
- @suite = suite
- end
- def self.rand_name(len = 40)
- str = ""
- len.times { str += ((rand(26) + ?A).chr)}
- return str
- end
- def pp(state)
- @name ||= IncludeGuard.rand_name
- state.needs_semi = false
- state.suppress_indent()
- state.print("#ifndef #{@name}")
- state.endline()
- state.suppress_indent()
- state.print("#define #{@name}")
- state.endline()
- if(@suite.respond_to?(:pp_no_braces))
- @suite.pp_no_braces(state)
- else
- state.pp(@suite)
- end
- state.endline()
- state.needs_semi = false
- state.suppress_indent()
- state.print("#endif // #{@name}")
- state.endline()
- end
-end
-class CPP::IfnDef
- attr_accessor :name,:suite
- def initialize(suite = CPP::Suite.new())
- @suite = suite
- end
- def pp(state)
- state.needs_semi = false
- state.suppress_indent()
- state.print("#ifndef #{@name}")
- state.endline()
- if(@suite.respond_to?(:pp_no_braces))
- @suite.pp_no_braces(state)
- else
- state.pp(@suite)
- end
- state.endline()
- state.needs_semi = false
- state.suppress_indent()
- state.print("#endif // #{@name}")
- state.endline()
- end
-end
-class CPP::PreprocessorIf
- attr_accessor :name,:suite
- def initialize(ifsuite, elsesuite)
- @ifsuite = ifsuite
- @elsesuite = elsesuite
- end
- def write_if(state)
- state.needs_semi = false
- state.suppress_indent()
- state.print("#if #{@name}")
- state.endline()
- end
- def write_else(state)
- state.needs_semi = false
- state.suppress_indent()
- state.print("#else // #{@name}")
- state.endline()
- end
- def write_endif(state)
- state.needs_semi = false
- state.suppress_indent()
- state.print("#endif // #{@name}")
- state.endline()
- end
- def pp_inline(state)
- self.write_if(state)
- @ifsuite.pp_inline(state)
- if(@elsesuite != nil)
- self.write_else(state)
- @elsesuite.pp_inline(state)
- end
- self.write_endif(state)
- end
- def pp(state)
- self.write_if(state)
- if(@ifsuite.respond_to?(:pp_no_braces))
- @ifsuite.pp_no_braces(state)
- else
- state.pp(@ifsuite)
- end
- if(@elsesuite != nil)
- self.write_else(state)
- if(@elsesuite.respond_to?(:pp_no_braces))
- @elsesuite.pp_no_braces(state)
- else
- state.pp(@elsesuite)
- end
- end
- self.write_endif(state)
- end
-end
-class CPP::Using
- def initialize(using)
- @using = using
- end
- def pp(state)
- state.print("using #{@using}")
- end
-end
diff --git a/aos/build/queues/cpp_pretty_print/dep_file_pair.rb b/aos/build/queues/cpp_pretty_print/dep_file_pair.rb
deleted file mode 100644
index 02233d4..0000000
--- a/aos/build/queues/cpp_pretty_print/dep_file_pair.rb
+++ /dev/null
@@ -1,470 +0,0 @@
-class GroupElement
- def add_group_dep(group_dep)
- @group_dep = group_dep
- end
- def adjust_group(state,old_group)
- if(@group_dep != old_group)
- @group_dep.adjust_from(state,old_group)
- end
- return @group_dep
- end
-end
-class DepMask < GroupElement
- def initialize(elem)
- @elem = elem
- @deps = []
- end
- def add_dep(dep)
- @deps << dep
- self
- end
- def method_missing(method,*args,&blk)
- @elem.send(method,*args,&blk)
- end
- alias_method :old_respond_to, :respond_to?
- def respond_to?(method)
- old_respond_to(method) || @elem.respond_to?(method)
- end
-end
-
-class MemberElementHeader < GroupElement
- def initialize(elem)
- @elem = elem
- end
- def pp(state)
- if(@elem.respond_to?(:pp_header_file))
- @elem.pp_header_file(state)
- else
- state.pp(@elem)
- end
- end
- def self.check(plan,elem)
- plan.push(self.new(elem))
- end
-end
-class MemberElementInline < GroupElement
- def initialize(elem)
- @elem = elem
- end
- def pp(state)
- if(@elem.respond_to?(:pp_inline))
- @elem.pp_inline(state)
- else
- state.pp(@elem)
- end
- end
-end
-class MemberElementCC < GroupElement
- attr_accessor :pp_override
- def initialize(elem)
- @elem = elem
- end
- def pp(state)
- return state.pp(@elem) if(@pp_override)
- @elem.pp_cc_file(state)
- end
- def self.check(plan,elem)
- plan.push(self.new(elem)) if(elem.respond_to?(:pp_cc_file))
- end
-end
-class ImplicitName
- attr_accessor :parent
- def initialize(name,parent)
- @name = name
- @parent = parent
- end
- def close(state)
- state.endline()
- state.needs_semi = false
- state.print("} // namespace #{@name}\n")
- end
- def name()
- if(@parent)
- return @parent.name + "." + @name
- else
- return "." + @name
- end
- end
- def open(state)
- state.endline()
- state.needs_semi = false
- state.print("namespace #{@name} {\n")
- end
- def adjust_from(state,old_group)
- close_grps = []
- grp = old_group
- while(grp)
- close_grps << grp
- grp = grp.parent
- end
- open_grps = []
- grp = self
- while(grp)
- open_grps << grp
- grp = grp.parent
- end
- while(open_grps[-1] == close_grps[-1] && close_grps[-1])
- close_grps.pop()
- open_grps.pop()
- end
- close_grps.each do |grp|
- grp.close(state)
- end
- open_grps.reverse.each do |grp|
- grp.open(state)
- end
- end
- def adjust_to(state,new_group)
- grp = self
- while(grp)
- grp.close(state)
- grp = grp.parent
- end
- end
-end
-class GroupPlan < GroupElement
- attr_accessor :implicit
- def initialize(group_elem,members = [])
- @group_elem = group_elem
- @members = CPP::Suite.new(members)
- end
- def push(mem)
- mem.add_group_dep(@implicit) if(@implicit)
- @members << mem
- end
- def pp(state)
- if(@group_elem)
- @group_elem.open(state)
- end
- group = nil
- @members.each do |member|
- group = member.adjust_group(state,group)
- #puts "at:#{group.name}" if(group.respond_to?(:name))
- state.endline()
- state.needs_semi = true
- state.pp(member)
- state.endline()
- end
- group.adjust_to(state,nil) if(group)
- if(@group_elem)
- @group_elem.close(state)
- end
- end
-end
-class CC_Mask
- def initialize(elem)
- @elem = elem
- end
- def plan_cc(plan)
- elem = MemberElementCC.new(@elem)
- elem.pp_override = true
- plan.push(elem)
- end
- def plan_header(plan);
- end
-end
-module Types
- class TypeDef
- def initialize(type,name)
- @type = type
- @name = name
- end
- def pp(state)
- state.pp("typedef #{@type.name} #{@name}")
- end
- end
- class Type
- attr_accessor :name,:static,:dec,:space
- def initialize(namespace,name)
- @space = namespace
- @name = name
- @members = []
- @protections = {}
- @deps = []
- end
- def parent_class
- @parent.split(' ')[1] if @parent
- end
- class ProtectionGroup
- def initialize(name)
- @name = name
- end
- def adjust_from(state,other)
- state.indent -= 1
- state.pp("#@name:")
- state.endline
- state.indent += 1
- end
- def adjust_to(state,other)
- other.adjust_from(state,self) if other
- end
- end
- ProtectionGroups = { :public => ProtectionGroup.new(:public),
- :protected => ProtectionGroup.new(:protected),
- :private => ProtectionGroup.new(:private)}
- def set_parent(parent)
- @parent = parent
- end
- def chop_method_prefix()
- @space.chop_method_prefix + "#@name::"
- end
- def def_func(*args)
- func = CPP::MemberFunc.new(self,*args)
- @protections[func] = @protection
- @members << func
- return func
- end
- def add_cc_comment(*args)
- args.each do |arg|
- @members.push(CC_Mask.new(CPP::Comment.new(arg)))
- end
- end
- def plan_cc(plan)
- @members.each do |member|
- if(member.respond_to?(:plan_cc))
- member.plan_cc(plan)
- elsif(member.respond_to?(:pp_cc_file))
- plan.push(MemberElementCC.new(member))
- end
- end
- end
- def plan_header(plan)
- group_plan = GroupPlan.new(self)
- plan.push(group_plan)
- @members.each do |member|
- group_plan.implicit = ProtectionGroups[@protections[member]]
- if(member.respond_to?(:plan_header))
- member.plan_header(group_plan)
- else
- group_plan.push(MemberElementHeader.new(member))
- end
- #member.plan_cc(plan)
- end
- end
- def open(state)
- state.needs_semi = false
- state.v_pad(2)
- if(@static)
- state.print("static ")
- end
- self.template_header(state) if(self.respond_to?(:template_header))
- state.print("#{self.class.type_name} #{@name}")
- self.template_spec_args(state) if(self.respond_to?(:template_spec_args))
- if(@parent)
- state.print(" : #{@parent} {\n")
- else
- state.print(" {\n")
- end
- state.indent += 2
- end
- def close(state)
- state.indent -= 2
- state.needs_semi = true
- if(@dec)
- state.print("\n} #{@dec}")
- else
- state.print("\n}")
- end
- state.v_pad(2)
- end
- def add_dep(other)
- @deps << other
- self
- end
- end
- class Class < Type
- def add_member(protection,member)
- @protection = protection
- @members.push(member)
- @protections[member] = protection
- return member
- end
- def add_struct(*args)
- add_member(:public, Types::Struct.new(self,*args))
- end
- def public() ; @protection = :public ; end
- def protected() ; @protection = :protected ; end
- def private() ; @protection = :private ; end
- def self.type_name() ; "class" ; end
- end
- class PreprocessorIf < Type
- def initialize(*args)
- super(*args)
- end
- def add_member(member)
- @members.push(member)
- return member
- end
- def self.type_name() ; "#if" ; end
- def open(state)
- state.needs_semi = false
- state.print("\n#if #{@name}")
- end
- def close(state)
- state.needs_semi = false
- state.print("\n#endif // #{@name}")
- end
- end
- class Struct < Type
- attr_accessor :members
- def add_member(member)
- @members.push(member)
- return member
- end
- def self.type_name() ; "struct" ; end
- end
- class TemplateClass < Class
- def initialize(*args)
- super(*args)
- @t_args = CPP::Args.new()
- end
- def spec_args()
- return @spec_args ||= CPP::Args.new()
- end
- def template_header(state)
- if(@t_args.length > 0)
- state.pp("template < ")
- state.pp(@t_args)
- else
- state.pp("template < ")
- end
- state.pp(">\n")
- end
- def plan_cc(plan)
-
- end
- def plan_header(plan)
- group_plan = GroupPlan.new(self)
- plan.push(group_plan)
- @members.each do |member|
- group_plan.implicit = ProtectionGroups[@protections[member]]
- if(member.respond_to?(:plan_header))
- member.plan_header(group_plan)
- else
- group_plan.push(MemberElementInline.new(member))
- end
- end
- end
- def template_spec_args(state)
- if(@spec_args)
- state.pp("< ")
- state.pp(@spec_args)
- state.pp(">")
- end
- end
- end
-end
-class DepFilePair
- class NameSpace
- def initialize(name,parent)
- @name,@parent = name,parent
- @members = []
- end
- def add_struct(*args)
- add Types::Struct.new(self,*args)
- end
- def add_template(*args)
- add Types::TemplateClass.new(self,*args)
- end
- def add_class(*args)
- add Types::Class.new(self,*args)
- end
- def add_cc(arg)
- @members.push(CC_Mask.new(arg))
- end
- def chop_method_prefix()
- ""
- end
- def class_comment(*args)
- add CPP::Comment.new(*args)
- end
- def var_dec_comment(*args)
- add CPP::Comment.new(*args)
- end
- def add_var_dec(arg)
- add DepMask.new(arg)
- end
- def plan_cc(plan)
- plan.implicit = ImplicitName.new(@name,plan.implicit)
- @members.each do |member|
- if(member.respond_to?(:plan_cc))
- member.plan_cc(plan)
- else
- MemberElementCC.check(plan,member)
- end
- end
- plan.implicit = plan.implicit.parent
- end
- def plan_header(plan)
- plan.implicit = ImplicitName.new(@name,plan.implicit)
- @members.each do |member|
- if(member.respond_to?(:plan_header))
- member.plan_header(plan)
- else
- MemberElementHeader.check(plan,member)
- end
- end
- plan.implicit = plan.implicit.parent
- end
- def add val
- @members << val
- val
- end
- end
- def initialize(rel_path)
- @rel_path = rel_path
- @cc_includes = []
- @header_includes = []
- @spaces = []
- @cc_usings = []
- @cache = {}
- end
- def add_cc_include(inc_path)
- @cc_includes << CPP::Include.new(inc_path)
- end
- def add_cc_using(using)
- @cc_usings << CPP::Using.new(using)
- end
- def add_header_include(inc_path)
- @header_includes << CPP::Include.new(inc_path)
- end
- def add_namespace(name,parent)
- space = NameSpace.new(name,parent)
- if(parent != self)
- parent.add(space)
- else
- @spaces.push(space)
- end
- return space
- end
- def set(type,cached)
- @cache[type] = cached
- end
- def get(type)
- cached = @cache[type]
- return cached if cached
- cached = type.create(self)
- cached_check = @cache[type]
- return cached_check if cached_check
- set(type,cached)
- return cached
- end
- def write_cc_file(cpp_base,cc_file)
- plan_cc = GroupPlan.new(nil,elems_cc = [])
- @spaces.each do |space|
- space.plan_cc(plan_cc)
- end
- suite = CPP::Suite.new(@cc_includes + @cc_usings + [plan_cc])
- CPP.pretty_print(suite,cc_file)
- end
- def write_header_file(cpp_base,header_file)
- plan_header = GroupPlan.new(nil,elems_cc = [])
- @spaces.each do |space|
- space.plan_header(plan_header)
- end
- suite = CPP::Suite.new(@header_includes + [plan_header])
- include_guard = CPP::IncludeGuard.new(suite)
- include_guard.name = @rel_path.upcase.gsub(/[\.\/\\]/,"_") + "_H_"
- CPP.pretty_print(include_guard,header_file)
- end
-end
diff --git a/aos/build/queues/cpp_pretty_print/file_pair_types.rb b/aos/build/queues/cpp_pretty_print/file_pair_types.rb
deleted file mode 100644
index 25ce4fe..0000000
--- a/aos/build/queues/cpp_pretty_print/file_pair_types.rb
+++ /dev/null
@@ -1,204 +0,0 @@
-module CPP
-end
-class CPP::TypePair
- attr_accessor :name,:static,:dec
- def initialize(namespace,name)
- @space,@name = namespace,name
- @members = []
- @protections = {}
- @dont_sort = {}
- end
- ProtectionTable = { :public => 0,:protected => 1,:private => 2 }
- def comp(m1,m2)
- if(!(@dont_sort[m1] || @dont_sort[m2]))
- if(@protections[m1] && @protections[m2])
- comp = ProtectionTable[@protections[m1]] <=> ProtectionTable[@protections[m2]]
- return comp if(comp != 0)
- end
- comp = TypeTable[m1.class] <=> TypeTable[m2.class]
- return comp if(comp != 0)
- end
- return @stable[m1] <=> @stable[m2]
- end
- def set_parent(parent)
- @parent = parent
- end
- def add_class(name)
- return add_member(:public,CPP::ClassPair.new(self,name))
- end
- def add_struct(name)
- return add_member(:public,CPP::StructPair.new(self,name))
- end
- def add_cc_comment(*strs)
- strs.each do |str|
- @members << comment = CPP::CCMemberWrapper.new(CPP::Comment.new(str))
- @protections[comment] = @protection
- end
- #@dont_sort[comment] = true
- end
- def method_prefix()
- @space.method_prefix + "#@name::"
- end
- def chop_method_prefix()
- @space.chop_method_prefix + "#@name::"
- end
- def pp(state)
- @stable = {}
- @members.each_with_index { |mem,i| @stable[mem] = i }
- members = @members.sort { |m1,m2| comp(m1,m2) }
-
- state.needs_semi = false
- state.v_pad(2)
- if(@static)
- state.print("static ")
- end
- if(@parent)
- state.print("#{self.class.type_name} #{@name} : #{@parent} {\n")
- else
- state.print("#{self.class.type_name} #{@name} {\n")
- end
- state.indent += 2
- protection = nil
- members.each do |member|
- if(protection != @protections[member])
- state.indent -= 1
- state.needs_semi = false
- state.v_pad(2) if(protection)
- protection = @protections[member]
- state.print("#{protection}:\n")
- state.indent += 1
- state.endline()
- end
- state.endline()
- state.needs_semi = true
- if(member.respond_to?(:pp_header_file))
- member.pp_header_file(state)
- else
- state.pp(member)
- end
- state.endline()
- end
- state.indent -= 2
- state.needs_semi = true
- if(@dec)
- state.print("\n} #{@dec}")
- else
- state.print("\n}")
- end
- state.v_pad(2)
- end
- def pp_header_file(state)
- pp(state)
- end
- def pp_cc_file(state)
- @members.each do |member|
- state.needs_semi = true
- member.pp_cc_file(state) if(member.respond_to?(:pp_cc_file))
- state.endline()
- end
- end
- def def_func(*args)
- func = CPP::MemberFunc.new(self,*args)
- @protections[func] = @protection
- @members << func
- return func
- end
-end
-class CPP::CCMemberWrapper
- def initialize(member)
- @member = member
- end
- def pp_header_file(state) ; end
- def pp_cc_file(state)
- state.pp(@member)
- end
-end
-class CPP::ClassPair < CPP::TypePair
- attr_accessor :name
- def initialize(namespace,name)
- super(namespace,name)
- @protection = :public #default to public
- end
- def add_member(protection,member)
- @protection = protection
- @members << member
- @protections[member] = protection
- return member
- end
- def public() ; @protection = :public ; end
- def protected() ; @protection = :protected ; end
- def private() ; @protection = :private ; end
- def self.type_name() ; "class" ; end
-end
-class CPP::StructPair < CPP::TypePair
- def self.type_name() ; "struct" ; end
- def add_member(member)
- @members << member
- return member
- end
-end
-CPP::TypePair::TypeTable = { CPP::StructPair => 0, CPP::ClassPair => 1,CPP::Constructor => 2 }
-CPP::TypePair::TypeTable.default = 3
-class CPP::TemplateClass < CPP::ClassPair
- def initialize(namespace,name)
- super(namespace,name)
- @t_args = CPP::Args.new()
- end
- def spec_args()
- return @spec_args ||= CPP::Args.new()
- end
- def pp(state)
- @stable = {}
- @members.each_with_index { |mem,i| @stable[mem] = i }
- members = @members.sort { |m1,m2| comp(m1,m2) }
- state.needs_semi = false
- state.v_pad(2)
- if(@t_args.length > 0)
- state.pp("template < ")
- state.pp(@t_args)
- else
- state.pp("template < ")
- end
- state.pp(">\n")
- state.pp("class #@name")
- if(@spec_args)
- state.pp("< ")
- state.pp(@spec_args)
- state.pp("> {")
- else
- state.pp(" {")
- end
- state.endline()
- state.indent += 2
- protection = nil
- members.each do |member|
- if(protection != @protections[member])
- state.indent -= 1
- state.needs_semi = false
- state.v_pad(2) if(protection)
- protection = @protections[member]
- state.print("#{protection}:\n")
- state.indent += 1
- state.endline()
- end
- state.endline()
- state.needs_semi = true
- if(member.respond_to?(:pp_inline))
- member.pp_inline(state)
- else
- state.pp(member)
- end
- state.endline()
- end
- state.indent -= 2
- state.needs_semi = true
- if(@dec)
- state.print("\n} #{@dec}")
- else
- state.print("\n}")
- end
- state.v_pad(2)
- end
- def pp_cc_file(state); end
-end
-
diff --git a/aos/build/queues/cpp_pretty_print/standard_types.rb b/aos/build/queues/cpp_pretty_print/standard_types.rb
deleted file mode 100644
index 997059a..0000000
--- a/aos/build/queues/cpp_pretty_print/standard_types.rb
+++ /dev/null
@@ -1,365 +0,0 @@
-module CPP
- class Class
-
- end
- class Funct
- attr_accessor :retval,:name,:args,:suite
- def initialize(retval = nil,name = nil,args = Args.new(),suite = Suite.new())
- @retval,@name,@args,@suite = retval,name,args,suite
- end
- def pp(state)
- state.pp(@retval)
- state.print(" ")
- state.pp(@name)
- state.print("(")
- state.pp(@args)
- state.print(")")
- state.pp(@suite)
- end
- end
- class If
- attr_accessor :cond,:suite,:else_suite
- def initialize(cond = nil,suite = Suite.new(),else_suite = nil)
- @cond,@suite,@else_suite = cond,suite,else_suite
- end
- def pp(state)
- state.print("if (")
- state.pp(@cond)
- state.print(") ")
- state.needs_semi = false
- state.pp(@suite)
- if(@else_suite)
- state.print(" else ")
- state.pp(@else_suite)
- end
- end
- def else_suite()
- return(@else_suite ||= Suite.new())
- end
- end
- class For
- attr_accessor :init,:cond,:update,:suite
- def initialize(init = nil,cond = nil,update = nil,suite = Suite.new())
- @init,@cond,@update,@suite = init,cond,update,suite
- end
- def pp(state)
- state.print("for (")
- Args.new([@init,@cond,@update]).pp(state,";")
- state.print(") ")
- state.needs_semi = false
- state.pp(@suite)
- end
- end
- class Args < Array
- attr_accessor :dont_wrap
- def pp(state,sep = ",")
- pos = start = state.col
- self.each_with_index do |arg,i|
- #puts "#{state.col - start} , #{start}"
- state.print(sep) if(i != 0)
- if(pos > 80)
- state.wrap(state.indent + 4)
- pos = start = state.col
- elsif(state.col * 2 - pos > 80 && !@dont_wrap)
- state.wrap(start)
- start = pos
- else
- state.print(" ") if(i != 0)
- pos = state.col
- end
- state.pp(arg)
- end
- end
- end
- class Suite < Array
- def pp(state)
- state.print("{")
- state.>>
- state.needs_semi = false
- self.pp_no_braces(state)
- state.<<
- state.print("}")
- end
- def pp_no_braces(state)
- self.each do |arg,i|
- state.endline()
- state.needs_semi = true
- state.pp(arg)
- state.endline()
- end
- end
- def pp_one_line(state)
- if(self.length == 1)
- state.needs_semi = true
- state.print("{ ")
- state.pp(self[0])
- state.print(";") if(state.needs_semi)
- state.needs_semi = false
- state.print(" }")
- else
- self.pp(state)
- end
- end
- end
- class FuncCall
- attr_accessor :name,:args
- def initialize(name = nil,args = Args.new())
- @name,@args = name,args
- end
- def self.build(name,*args)
- self.new(name,Args.new(args))
- end
- def pp(state)
- state.pp(@name)
- state.print("(")
- state.pp(@args)
- state.print(")")
- end
- end
- class BIT_OR
- attr_accessor :val1,:val2
- def initialize(val1,val2)
- @val1,@val2 = val1,val2
- end
- def pp(state)
- state.pp(@val1)
- state.print(" | ")
- state.pp(@val2)
- end
- end
- class LT
- attr_accessor :val1,:val2
- def initialize(val1,val2)
- @val1,@val2 = val1,val2
- end
- def pp(state)
- state.pp(@val1)
- state.print(" < ")
- state.pp(@val2)
- end
- end
- class Div
- attr_accessor :val1,:val2
- def initialize(val1,val2)
- @val1,@val2 = val1,val2
- end
- def pp(state)
- state.pp(@val1)
- state.print(" / ")
- state.pp(@val2)
- end
- end
- class Add
- attr_accessor :val1,:val2
- def initialize(val1,val2)
- @val1,@val2 = val1,val2
- end
- def pp(state)
- state.pp(@val1)
- state.print(" + ")
- state.pp(@val2)
- end
- end
- class Member
- attr_accessor :obj,:name
- def initialize(obj,name)
- @obj,@name = obj,name
- end
- def pp(state)
- state.pp(@obj)
- state.print(".")
- state.pp(@name)
- end
- end
- class PointerMember
- attr_accessor :obj,:name
- def initialize(obj,name)
- @obj,@name = obj,name
- end
- def pp(state)
- state.pp(@obj)
- state.print("->")
- state.pp(@name)
- end
- end
- class Minus
- attr_accessor :val
- def initialize(val)
- @val = val
- end
- def pp(state)
- state.print("-")
- state.pp(@val)
- end
- end
- class Not
- attr_accessor :val
- def initialize(val)
- @val = val
- end
- def pp(state)
- state.print("!")
- state.pp(@val)
- end
- end
- class Address
- attr_accessor :val
- def initialize(val)
- @val = val
- end
- def pp(state)
- state.print("&")
- state.pp(@val)
- end
- end
- class Paran
- attr_accessor :val
- def initialize(val)
- @val = val
- end
- def pp(state)
- state.print("(")
- state.pp(@val)
- state.print(")")
- end
- end
- class Assign
- attr_accessor :lval,:rval
- def initialize(lval = nil,rval = Args.new())
- @lval,@rval = lval,rval
- end
- def pp(state)
- state.pp(@lval)
- state.print(" = ")
- state.pp(@rval)
- end
- end
- class PointerDeref
- attr_accessor :val
- def initialize(val)
- @val = val
- end
- def pp(state)
- state.print("*")
- state.pp(@val)
- end
- end
- class Cast
- attr_accessor :to,:val
- def initialize(to,val)
- @to,@val = to,val
- end
- def pp(state)
- state.print("(")
- state.pp(@to)
- state.print(")")
- state.pp(@val)
- end
- end
- class VarDec
- attr_accessor :type,:name
- def initialize(type = nil,name = nil)
- @type,@name = type,name
- end
- def pp(state)
- state.pp(@type)
- state.print(" ")
- state.pp(@name)
- end
- end
- class Return
- attr_accessor :val
- def initialize(val = nil)
- @val = val
- end
- def pp(state)
- state.print("return ")
- state.pp(@val)
- end
- end
- class TraversalState
- attr_accessor :needs_semi,:indent,:col
- def initialize(file)
- @file = file
- @indent = 0
- @not_indented = true
- @just_endlined = true
- @hold_points = []
- @col = 0
- end
- def set_hold_point()
- @hold_points.push(@col)
- end
- def wrap(col)
- @file.print("\n")
- @file.print(" " * col)
- @col = col
- end
- def suppress_indent()
- @not_indented = false
- end
- def <<()
- @indent -= 2
- end
- def >>()
- @indent += 2
- end
- def pp(ast)
- return ast.pp(self) if(ast.respond_to?(:pp) &&
- ast.class != String &&
- ast.class != Integer)
- self.print(ast)
- end
- def print(chars)
- return print_no_split(chars) if(!chars.respond_to?(:split))
- chars.split(/\n/,-1).each_with_index do |line,i|
- endline() if(i != 0)
- print_no_split(line) if(line && line.length > 0)
- end
- end
- def print_no_split(chars)
- if(@not_indented)
- @file.print(" "*@indent)
- @col += @indent
- @not_indented = false
- end
- if(chars)
- chars = chars.to_s
- if(chars.length > 0)
- @col += chars.length
- @file.print(chars)
- @just_endlined = false
- @v_pad = 0
- end
- end
- end
- def endline()
- return if(@just_endlined)
- @file.print(";") if(@needs_semi)
- @needs_semi = false
- @file.print("\n")
- @not_indented = true
- @just_endlined = true
- @hold_points.clear()
- @v_pad += 1
- @col = 0
- end
- def v_pad(n)
- while(@v_pad < n)
- force_endline()
- end
- end
- def force_endline()
- @just_endlined = false
- endline()
- end
- end
- def self.pretty_print(ast,file)
- state = TraversalState.new(file)
- if(ast.respond_to?(:pp_no_braces))
- ast.pp_no_braces(state)
- else
- state.pp(ast)
- end
- end
-end
diff --git a/aos/build/queues/load.rb b/aos/build/queues/load.rb
deleted file mode 100644
index 50d59f8..0000000
--- a/aos/build/queues/load.rb
+++ /dev/null
@@ -1,17 +0,0 @@
-$LOAD_PATH.unshift(".")
-
-["tokenizer.rb","q_file.rb","queue_group.rb","queue.rb","namespaces.rb",
- "interface.rb","errors.rb", "q_struct.rb"].each do |name|
- require_relative "objects/" + name
-end
-["standard_types.rb","auto_gen.rb","file_pair_types.rb",
- "dep_file_pair.rb"].each do |name|
- require_relative "cpp_pretty_print/" + name
-end
-["q_file.rb","message_dec.rb","queue_dec.rb", "q_struct.rb"].each do |name|
- require_relative "output/" + name
-end
-require_relative "write_iff_changed.rb"
-
-require "fileutils"
-require "pathname"
diff --git a/aos/build/queues/objects/errors.rb b/aos/build/queues/objects/errors.rb
deleted file mode 100644
index d776a22..0000000
--- a/aos/build/queues/objects/errors.rb
+++ /dev/null
@@ -1,43 +0,0 @@
-class QError < Exception
- def initialize(msg)
- super()
- @msg = msg
- @qstacktrace = []
- end
- def self.set_name(name)
- @pretty_name = name
- end
- def self.pretty_name()
- @pretty_name
- end
- def to_s
- msg = "Error:(#{self.class.pretty_name})\n\t"
- msg += @msg
- msg += "\n" if(msg[-1] != "\n")
- @qstacktrace.each do |part|
- part = part.q_stack_name if(part.respond_to?(:q_stack_name))
- msg += "\tfrom: #{part}\n"
- end
- return msg
- end
- set_name("Base Level Exception.")
- attr_accessor :qstacktrace
-end
-class QSyntaxError < QError
- def initialize(msg)
- super(msg)
- end
- set_name("Syntax Error")
-end
-class QNamespaceCollision < QError
- def initialize(msg)
- super(msg)
- end
- set_name("Namespace Collision")
-end
-class QImportNotFoundError < QError
- def initialize(msg)
- super(msg)
- end
- set_name("Couldn't Find Target of Import Statement")
-end
diff --git a/aos/build/queues/objects/interface.rb b/aos/build/queues/objects/interface.rb
deleted file mode 100644
index 52332b6..0000000
--- a/aos/build/queues/objects/interface.rb
+++ /dev/null
@@ -1,69 +0,0 @@
-class MessageElementReq
- def initialize(type,name)
- @type = type
- @name = name
- end
- def self.parse(tokens)
- type = tokens.expect(:tWord).data
- name = tokens.expect(:tWord).data
- tokens.expect(:tSemi)
- return self.new(type,name)
- end
-end
-class QueueReq
- def initialize(name,type = nil)
- @name = name
- @type = type
- end
- def self.parse(tokens)
- type_or_name = tokens.expect(:tWord).data
- if(tokens.peak == :tSemi)
- tokens.expect(:tSemi)
- return self.new(type_or_name)
- else
- name = tokens.expect(:tWord).data
- tokens.expect(:tSemi)
- return self.new(name,type_or_name)
- end
- end
-end
-class InterfaceStmt < QStmt
- def initialize(name,elements)
- @name = name
- @elements = elements
- end
- def q_eval(locals)
-
- end
- def self.check_type(tokens,new_type,old_type)
- return new_type if(old_type == nil)
- if(new_type != old_type)
- tokens.qError(<<ERROR_MSG)
-error: intermixing queue definitions (a queue_group feature)
- and type definitions (a message_group feature)
- this results in an undefined type value.
- Wot. Wot.
-ERROR_MSG
- end
- return old_type
- end
- def self.parse(tokens)
- name = tokens.expect(:tWord).data
- values = []
- type = nil
- tokens.expect(:tOpenB)
- while(tokens.peak != :tCloseB)
- if(tokens.peak.data == "queue")
- tokens.expect(:tWord)
- values << QueueReq.parse(tokens)
- type = check_type(tokens,:queue_group,type)
- else
- values << MessageElementReq.parse(tokens)
- type = check_type(tokens,:message_group,type)
- end
- end
- tokens.expect(:tCloseB)
- tokens.expect(:tSemi)
- self.new(name,values)
- end
-end
diff --git a/aos/build/queues/objects/namespaces.rb b/aos/build/queues/objects/namespaces.rb
deleted file mode 100644
index ea3aa80..0000000
--- a/aos/build/queues/objects/namespaces.rb
+++ /dev/null
@@ -1,211 +0,0 @@
-class LocalSituation
- attr_accessor :globals,:local
- def initialize(globals)
- @globals = globals
- @local = nil
- end
- def package=(qualified)
- if(@local)
- raise QSyntaxError.new(<<ERROR_MSG)
-You are redefining the package path.
- Stuff might break if you do that.
- Other options include: using another header file, and just using the same namespace.
- If you are confident that you need this you can remove this check at
- #{__FILE__}:#{__LINE__}.
- Or file a feature request.
- But that would be weird...
- Wot. Wot.
-ERROR_MSG
- end
- @local = @globals.space
- qualified.names.each do |name|
- @local = @local.get_make(name)
- end
- end
- def register(value)
- if(!@local)
- raise QError.new(<<ERROR_MSG)
-There is no package path defined, This is a big problem because
- we are kindof expecting you to have a package path...
- use a :
- package my_super.cool.project;
- statement to remedy this situation. (or file a feature request)
- Wot. Wot.
-ERROR_MSG
- end
- @local[value.name] = value
- value.parent = @local if(value.respond_to?(:parent=))
- value.loc = @local
- end
- def bind(bind_to)
- return BoundSituation.new(self,bind_to)
- end
-end
-class BoundSituation < LocalSituation
- def initialize(locals,bind_to)
- @globals = locals.globals
- @local = bind_to
- end
-end
-class NameSpace
- attr_accessor :parent,:name
- def initialize(name = nil,parent = nil)
- @name = name
- @parent = parent
- @spaces = {}
- end
- def []=(key,val)
- if(old_val = @spaces[key])
- old_val = old_val.created_by if(old_val.respond_to?(:created_by) && old_val.created_by)
- if(old_val.respond_to?(:q_stack_name))
- old_val = old_val.q_stack_name
- else
- old_val = "eh, it is a #{old_val.class} thats all I know..."
- end
- raise QNamespaceCollision.new(<<ERROR_MSG)
-Woah! The name #{queue_name(key).inspect} is already taken by some chap at #{old_val}.
-\tFind somewhere else to peddle your wares.
-\tWot. Wot.
-ERROR_MSG
- end
- @spaces[key] = val
- end
- def to_cpp_id(name)
- txt = @name + "::" + name
- return @parent.to_cpp_id(txt) if(@parent && parent.name)
- return "::" + txt
- end
- def queue_name(queue)
- get_name() + "." + queue
- end
- def [](key)
- #puts "getting #{get_name}.#{key}"
- @spaces[key]
- end
- def get_make(name)
- @spaces[name] ||= self.class.new(name,self)
- end
- def get_name()
- if(@parent)
- return "#{@parent.get_name}.#{@name}"
- else
- return "#{@name}"
- end
- end
- def create(cpp_tree)
- if(@parent && @parent.name)
- parent = cpp_tree.get(@parent)
- else
- parent = cpp_tree
- end
- return cpp_tree.add_namespace(@name,parent)
- end
- def to_s()
- "<NameSpace: #{get_name()}>"
- end
- def inspect()
- "<NameSpace: #{get_name()}>"
- end
- def root()
- return self if(@parent == nil)
- return @parent.root
- end
-end
-class Globals
- attr_accessor :space
- def initialize()
- @space = NameSpace.new()
- @space.get_make("aos")
- @include_paths = []
- end
- def paths()
- @include_paths
- end
- def add_path(path)
- @include_paths << path
- end
- def find_file(filename)
- @include_paths.each do |path_name|
- new_path = File.expand_path(path_name) + "/" + filename
- if(File.exists?(new_path))
- return new_path
- end
- end
- raise QImportNotFoundError.new(<<ERROR_MSG)
-Problem Loading:#{filename.inspect} I looked in:
-\t#{(@include_paths.collect {|name| name.inspect}).join("\n\t")}
-\tbut alas, it was nowhere to be found.
-\tIt is popular to include the top of the repository as the include path start,
-\tand then reference off of that.
-\tI would suggest doing that and then trying to build again.
-\tWot. Wot.
-ERROR_MSG
- end
-end
-class QualifiedName
- attr_accessor :names,:off_root
- def initialize(names,off_root = false)
- @names = names
- @off_root = off_root
- end
- def test_lookup(namespace)
- @names.each do |name|
- return nil if(!namespace.respond_to?(:[]))
- namespace = namespace[name]
- return nil if(!namespace)
- end
- return namespace
- end
- def is_simple?()
- return !@off_root && @names.length == 1
- end
- def to_simple()
- return @names[-1]
- end
- def to_s()
- if(@off_root)
- return ".#{@names.join(".")}"
- else
- return @names.join(".")
- end
- end
- def lookup(locals)
- if(@off_root)
- local = locals.globals.space
- else
- local = locals.local
- end
- target = nil
- while(!target && local)
- target = test_lookup(local)
- local = local.parent
- end
- return target if(target)
- if(@off_root)
- raise QError.new(<<ERROR_MSG)
-I was looking for .#{@names.join(".")}, but alas, it was not under
-\tthe root namespace.
-\tI'm really sorry old chap.
-\tWot. Wot.
-ERROR_MSG
- else
- raise QError.new(<<ERROR_MSG)
-I was looking for #{@names.join(".")}, but alas, I could not find
-\tit in #{locals.local.get_name} or any parent namespaces.
-\tI'm really sorry old chap.
-\tWot. Wot.
-ERROR_MSG
- end
- end
- def self.parse(tokens)
- names = []
- off_root = (tokens.peak == :tDot)
- tokens.next if(off_root)
- names << tokens.expect(:tWord).data
- while(tokens.peak == :tDot)
- tokens.next
- names << tokens.expect(:tWord).data
- end
- return self.new(names,off_root)
- end
-end
diff --git a/aos/build/queues/objects/q_file.rb b/aos/build/queues/objects/q_file.rb
deleted file mode 100644
index f73b638..0000000
--- a/aos/build/queues/objects/q_file.rb
+++ /dev/null
@@ -1,150 +0,0 @@
-class QStmt
- def set_line(val)
- @file_line = val
- return self
- end
- def q_stack_name()
- @file_line
- end
- def self.method_added(name)
- @wrapped ||= {}
- return if(name != :q_eval && name != :q_eval_extern)
- return if(@wrapped[name])
- @wrapped[name] = true
- method = self.instance_method(name)
- define_method(name) do |*args,&blk|
- begin
- method.bind(self).call(*args,&blk)
- rescue QError => e
- e.qstacktrace << self
- raise e
- end
- end
- end
-
-end
-class ImportStmt < QStmt
- def initialize(filename)
- @filename = filename
- end
- def q_eval(locals)
- filename = locals.globals.find_file(@filename)
- #puts "importing #{filename.inspect}"
- q_file = QFile.parse(filename)
- q_output = q_file.q_eval_extern(locals.globals)
- q_output.extern = true
- return Target::QInclude.new(@filename + ".h")
- end
- def self.parse(tokens)
- line = tokens.pos
- to_import = (tokens.expect(:tString) do |token|
- <<ERROR_MSG
-I found a #{token.humanize} at #{token.pos}.
-\tI was really looking for a "filename" for this import statement.
-\tSomething like: import "super_cool_file";
-\tWot.Wot
-ERROR_MSG
- end).data
- tokens.expect(:tSemi) do |token|
- <<ERROR_MSG
-I found a #{token.humanize} at #{token.pos}.
-\tI was really looking for a ";" to finish off this import statement
-\tat line #{line};
-\tSomething like: import #{to_import.inspect};
-\tWot.Wot #{" "*to_import.inspect.length}^
-ERROR_MSG
- end
- return self.new(to_import).set_line(line)
- end
-end
-class PackageStmt < QStmt
- def initialize(name)
- @name = name
- end
- def q_eval(locals)
- locals.package = @name
- return nil
- end
- def self.parse(tokens)
- line = tokens.pos
- qualified_name = QualifiedName.parse(tokens)
- tokens.expect(:tSemi)
- return self.new(qualified_name).set_line(line)
- end
-end
-class QFile
- attr_accessor :namespace
- def initialize(filename,suite)
- @filename,@suite = filename,suite
- end
- def q_eval(globals = Globals.new())
- local_pos = LocalSituation.new(globals)
- q_file = Target::QFile.new()
- @suite.each do |name|
- val = name.q_eval(local_pos)
- if(val)
- if(val.respond_to?(:create))
- q_file.add_type(val)
- end
- end
- end
- @namespace = local_pos.local
- return q_file
- end
- def q_eval_extern(globals)
- local_pos = LocalSituation.new(globals)
- q_file = Target::QFile.new()
- @suite.each do |name|
- if(name.respond_to?(:q_eval_extern))
- val = name.q_eval_extern(local_pos)
- else
- val = name.q_eval(local_pos)
- end
- if(val)
- if(val.respond_to?(:create))
- q_file.add_type(val)
- end
- end
- end
- return q_file
- end
- def self.parse(filename)
- tokens = Tokenizer.new(filename)
- suite = []
- while(tokens.peak != :tEnd)
- token = tokens.expect(:tWord) do |token| #symbol
- <<ERROR_MSG
-I found a #{token.humanize} at #{token.pos}.
-\tI was really looking for a "package", "import", "queue_group",
-\t"message", or "queue" statement to get things moving.
-\tSomething like: import "super_cool_file";
-\tWot.Wot
-ERROR_MSG
- end
- case token.data
- when "package"
- suite << PackageStmt.parse(tokens)
- when "import"
- suite << ImportStmt.parse(tokens)
- when "queue_group"
- suite << QueueGroupStmt.parse(tokens)
- when "message"
- suite << MessageStmt.parse(tokens)
- when "queue"
- suite << QueueStmt.parse(tokens)
- when "interface"
- suite << InterfaceStmt.parse(tokens)
- when "struct"
- suite << StructStmt.parse(tokens)
- else
- tokens.qError(<<ERROR_MSG)
-expected a "package","import","queue","struct","queue_group", or "message" statement rather
- than a #{token.data.inspect}, (whatever that is?)
- oh! no! a feature request!?
- Wot. Wot.
-ERROR_MSG
- end
- end
- return self.new(filename,suite)
- end
-end
diff --git a/aos/build/queues/objects/q_struct.rb b/aos/build/queues/objects/q_struct.rb
deleted file mode 100644
index cd47fe6..0000000
--- a/aos/build/queues/objects/q_struct.rb
+++ /dev/null
@@ -1,37 +0,0 @@
-
-class StructStmt
- def initialize(name,suite)
- @name = name
- @suite = suite
- end
- def q_eval(locals)
- group = Target::StructDec.new(@name)
- locals.register(group)
- @suite.each do |stmt|
- stmt.q_eval(locals.bind(group))
- end
- return group
- end
- def self.parse(tokens)
- name = tokens.expect(:tWord).data
- values = []
- tokens.expect(:tOpenB)
- while(tokens.peak != :tCloseB)
- values << MessageElementStmt.parse(tokens)
- end
- names = {}
- values.each do |val|
- if(names[val.name])
- raise QSyntaxError.new(<<ERROR_MSG)
-Hey! duplicate name #{val.name.inspect} in your message declaration statement (message #{name}).
-\tI found them at: #{names[val.name].q_stack_name()} and #{val.q_stack_name()}.
-\tWot. Wot.
-ERROR_MSG
- end
- names[val.name] = val
- end
- tokens.expect(:tCloseB)
- tokens.expect(:tSemi)
- self.new(name,values)
- end
-end
diff --git a/aos/build/queues/objects/queue.rb b/aos/build/queues/objects/queue.rb
deleted file mode 100644
index 7841411..0000000
--- a/aos/build/queues/objects/queue.rb
+++ /dev/null
@@ -1,173 +0,0 @@
-class MessageElementStmt < QStmt
- attr_accessor :name
- def initialize(type_name,name,length = nil) #lengths are for arrays
- @type_name = type_name
- @type = type_name.to_s
- @type = '::aos::monotonic_clock::time_point' if @type == 'Time'
- @name = name
- @length = length
- end
- CommonMistakes = {"short" => "int16_t","int" => "int32_t","long" => "int64_t","time" => "Time"}
- def check_type_error(locals)
- if(!(Sizes[@type] || (@length != nil && @type == "char")) )
- if(correction = CommonMistakes[@type])
- raise QError.new(<<ERROR_MSG)
-Hey! you have a \"#{@type}\" in your message statement.
-\tplease use #{correction} instead. Your type is not supported because we
-\twant to guarantee that the sizes of the messages stay the same across platforms.
-\tWot. Wot.
-ERROR_MSG
- elsif(@type == "char")
- raise QError.new(<<ERROR_MSG)
-Hey! you have a \"#{@type}\" in your message statement.
-\tyou need your declaration to be a char array like: char[10].
-\tor, please use int8_t or uint8_t.
-\tWot. Wot.
-ERROR_MSG
- else
- @is_struct_type = true
- return if(lookup_type(locals))
- raise QError.new(<<ERROR_MSG)
-Hey! you have a \"#{@type}\" in your message statement.
-\tThat is not in the list of supported types.
-\there is the list of supported types:
-\tint{8,16,32,64}_t,uint{8,16,32,64}_t,bool,float,double
-\tWot. Wot.
-ERROR_MSG
- end
- end
- end
-
- PrintFormat = {"bool" => "%c",
- "float" => "%f",
- "char" => "%c",
- "double" => "%f",
- "uint8_t" => "%\" PRIu8 \"",
- "uint16_t" => "%\" PRIu16 \"",
- "uint32_t" => "%\" PRIu32 \"",
- "uint64_t" => "%\" PRIu64 \"",
- "int8_t" => "%\" PRId8 \"",
- "int16_t" => "%\" PRId16 \"",
- "int32_t" => "%\" PRId32 \"",
- "int64_t" => "%\" PRId64 \"",
- "::aos::monotonic_clock::time_point" => "\" AOS_TIME_FORMAT \""}
- def toPrintFormat()
- if(format = PrintFormat[@type])
- return format;
- end
- raise QError.new(<<ERROR_MSG)
-Somehow this slipped past me, but
-\tI couldn't find the print format of #{@type}. Really, my bad.
-\tWot. Wot.
-ERROR_MSG
- end
-
- Sizes = {"bool" => 1, "float" => 4,"double" => 8,"::aos::monotonic_clock::time_point" => 8}
- [8,16,32,64].each do |len|
- Sizes["int#{len}_t"] = len / 8
- Sizes["uint#{len}_t"] = len / 8
- end
- Zero = {"float" => "0.0f","double" => "0.0","bool" => "false","::aos::monotonic_clock::time_point" => "::aos::monotonic_clock::epoch()"}
- def size()
- if(size = Sizes[@type]); return size; end
- return 1 if(@type == "char")
- raise QError.new(<<ERROR_MSG)
-Somehow this slipped past me, but
-\tI couldn't find the size of #{@type}. Really, my bad.
-\tWot. Wot.
-ERROR_MSG
- end
- def lookup_type(locals)
- return @type_name.lookup(locals)
- end
- def q_eval(locals)
- check_type_error(locals)
- if(@is_struct_type)
- tval = lookup_type(locals)
- member = Target::MessageStructElement.new(tval, @name)
- if (@length != nil)
- inner_member = member
- member = Target::MessageArrayElement.new(inner_member, @length)
- end
- else
- member = Target::MessageElement.new(@type,@name)
- member.size = size()
- member.zero = Zero[@type] || "0";
- member.printformat = toPrintFormat()
-
- if(@length != nil)
- inner_member = member
- member = Target::MessageArrayElement.new(inner_member,@length)
- end
- end
- locals.local.add_member(member)
- end
- def self.parse(tokens)
- line = tokens.pos
- #type = tokens.expect(:tWord).data
- type_name = QualifiedName.parse(tokens)
- len = nil
- if(tokens.peak == :tOpenB)
- tokens.expect(:tOpenB)
- len = tokens.expect(:tNumber).data
- tokens.expect(:tCloseB)
- end
- name = tokens.expect(:tWord).data
- tokens.expect(:tSemi)
- return self.new(type_name,name,len).set_line(line)
- end
-end
-class MessageStmt < QStmt
- def initialize(name,suite)
- @name = name
- @suite = suite
- end
- def q_eval(locals)
- group = Target::MessageDec.new(@name)
- locals.register(group)
- @suite.each do |stmt|
- stmt.q_eval(locals.bind(group))
- end
- return group
- end
- def self.parse(tokens)
- name = tokens.expect(:tWord).data
- values = []
- tokens.expect(:tOpenB)
- while(tokens.peak != :tCloseB)
- values << MessageElementStmt.parse(tokens)
- end
- names = {}
- values.each do |val|
- if(names[val.name])
- raise QSyntaxError.new(<<ERROR_MSG)
-Hey! duplicate name #{val.name.inspect} in your message declaration statement (message #{name}).
-\tI found them at: #{names[val.name].q_stack_name()} and #{val.q_stack_name()}.
-\tWot. Wot.
-ERROR_MSG
- end
- names[val.name] = val
- end
- tokens.expect(:tCloseB)
- tokens.expect(:tSemi)
- self.new(name,values)
- end
-end
-class QueueStmt < QStmt
- def initialize(type,name)
- @type,@name = type,name
- end
- def q_eval(locals)
- queue = Target::QueueDec.new(@type.lookup(locals),@name)
- locals.register(queue)
- locals.local.add_queue(queue) if(locals.local.respond_to?(:add_queue))
- return queue
- end
- def self.parse(tokens)
- line = tokens.pos
- type_name = QualifiedName.parse(tokens)
- name = tokens.expect(:tWord).data
- tokens.expect(:tSemi)
- return self.new(type_name,name).set_line(line)
- end
-end
diff --git a/aos/build/queues/objects/queue_group.rb b/aos/build/queues/objects/queue_group.rb
deleted file mode 100644
index 136834f..0000000
--- a/aos/build/queues/objects/queue_group.rb
+++ /dev/null
@@ -1,115 +0,0 @@
-class QueueGroupTypeStmt < QStmt
- def initialize(name,suite)
- @name,@suite = name,suite
- end
- def q_eval(locals)
- group = Target::QueueGroupDec.new(@name)
- group.created_by = self
- locals.register(group)
- @suite.each do |stmt|
- stmt.q_eval(locals.bind(group))
- end
- return group
- end
-end
-class ImplementsStmt < QStmt
- def initialize(name)
- @name = name
- end
- def q_eval(locals)
-
- end
- def self.parse(tokens)
- name = QualifiedName.parse(tokens)
- tokens.expect(:tSemi)
- return self.new(name)
- end
-end
-class QueueGroupStmt < QStmt
- def initialize(type,name)
- @type,@name = type,name
- end
- def q_eval(locals)
- group = Target::QueueGroup.new(@type.lookup(locals),@name)
- group.created_by = self
- locals.register(group)
- return group
- end
- def self.parse(tokens)
- line = tokens.pos
- type_name = QualifiedName.parse(tokens)
- if(type_name.names.include?("queue_group"))
- tokens.qError(<<ERROR_MSG)
-I was looking at the identifier you gave
-\tfor the queue group type between line #{line} and #{tokens.pos}
-\tThere shouldn't be a queue_group type called queue_group
-\tor including queue_group in it's path, it is a reserved keyword.
-\tWot. Wot.
-ERROR_MSG
- end
- if(tokens.peak == :tOpenB)
- if(type_name.is_simple?())
- type_name = type_name.to_simple
- else
- tokens.qError(<<ERROR_MSG)
-You gave the name: "#{type_name.to_s}" but you're only allowed to
-\thave simple names like "#{type_name.names[-1]}" in queue_group definitions
-\ttry something like:
-\tqueue_group ControlLoop { }
-\tWot. Wot.
-ERROR_MSG
- end
- tokens.expect(:tOpenB)
- suite = []
- while(tokens.peak != :tCloseB)
- token = tokens.expect(:tWord) do |token|
- <<ERROR_MSG
-I'm a little confused, I found a #{token.humanize} at #{token.pos}
-\tbut what I really wanted was an identifier signifying a nested
-\tmessage declaration, or queue definition, or an impliments statement.
-\tWot.Wot
-ERROR_MSG
- end
- case token.data
- when "message"
- suite << MessageStmt.parse(tokens)
- when "queue"
- suite << QueueStmt.parse(tokens)
- when "implements"
- suite << ImplementsStmt.parse(tokens)
- else
- tokens.qError(<<ERROR_MSG)
-expected a "queue","implements" or "message" statement rather
-\tthan a #{token.data.inspect}.
-\tWot. Wot.
-ERROR_MSG
- end
- end
- tokens.expect(:tCloseB)
- obj = QueueGroupTypeStmt.new(type_name,suite).set_line(line)
- else
- name = (tokens.expect(:tWord) do |token|
- <<ERROR_MSG
-I found a #{token.humanize} at #{token.pos}
-\tbut I was in the middle of parsing a queue_group statement, and
-\twhat I really wanted was an identifier to store the queue group.
-\tSomething like: queue_group control_loops.Drivetrain my_cool_group;
-\tWot.Wot
-ERROR_MSG
- end).data
- obj = QueueGroupStmt.new(type_name,name).set_line(line)
- if(tokens.peak == :tDot)
- tokens.qError(<<ERROR_MSG)
-Hey! It looks like you're trying to use a complex identifier at: #{tokens.pos}
-\tThats not going to work. Queue Group definitions have to be of the form:
-\tqueue_group ComplexID SimpleID
-\tWot. Wot.
-ERROR_MSG
- end
- end
- tokens.expect(:tSemi) do |token|
- token.pos
- end
- return obj
- end
-end
diff --git a/aos/build/queues/objects/tokenizer.rb b/aos/build/queues/objects/tokenizer.rb
deleted file mode 100644
index 2a33b90..0000000
--- a/aos/build/queues/objects/tokenizer.rb
+++ /dev/null
@@ -1,213 +0,0 @@
-class BufferedReader
- def initialize(file)
- @file = file
- @line = 1
- @chars = []
- @col_nums = []
- @col = 0
- end
- def filename
- return File.basename(@file.path)
- end
- def pos()
- "#{filename()}:#{lineno()},#{@col}"
- end
- def clear_comment()
- @file.gets
- @line += 1
- end
- def lineno()
- return @line
- return @file.lineno + 1
- end
- def pop_char()
- val = @chars.pop() || @file.read(1)
- @col_nums[@line] = @col += 1
- if(val == "\n")
- @line += 1
- @col = 0
- end
-
- return val
- end
- def unpop_char(char)
- if(char == "\n")
- @line -= 1
- @col = @col_nums[@line]
- end
- @col -= 1
- @chars.push(char)
- end
-end
-class Tokenizer
- TOKEN_TYPES = {"{" => :tOpenB,"}"=> :tCloseB,";" => :tSemi,"," => :tComma,
- "(" => :tOpenParan,")" => :tCloseParan,"=" => :tAssign,"." => :tDot,
- "<<"=> :tLShift,"*" => :tMult,"+" => :tAdd,"[" => :tOpenB,
- "]" => :tCloseB}
- Humanize = TOKEN_TYPES.invert
- class Token
- attr_accessor :type,:data,:pos
- def to_s
- if(@type == :tString)
- val = @data.inspect.to_s
- elsif(@type == :tWord)
- val = @data.to_s
- else
- val = @data.to_s
- end
- return "#{val.ljust(50)}:#{@type}"
- end
- def humanize()
- if(@type == :tString)
- return "#{@data.inspect.to_s} string"
- elsif(@type == :tWord)
- return "#{@data.inspect} identifier"
- end
- return Humanize[@type].inspect
- end
- def inspect()
- data = ""
- data = " #{@data.inspect}" if(@data)
- "<Token :#{@type}#{data} at #{@pos}>"
- end
- def ==(other)
- if(other.class == Symbol)
- return @type == other
- elsif(other.class == self.class)
- return @type == other.type && @data == other.data
- else
- return nil
- end
- end
- end
- def initialize(file)
- file = File.open(file,"r") if(file.class == String)
- @read = BufferedReader.new(file)
- end
- def qError(error)
- syntax_error(error)
- end
- def syntax_error(msg)
- err = QSyntaxError.new(msg)
- err.qstacktrace << "#{@read.lineno} of #{@read.filename}"
- raise err
- end
- def peak_token()
- @peak_token = next_token()
- end
- def peak()
- peak_token()
- end
- def next()
- next_token()
- end
- def pos()
- @read.pos
- end
- def tokenize(string_token)
- token = Token.new()
- token.type = TOKEN_TYPES[string_token]
- return token
- end
- def next_token()
- if(token = @peak_token)
- @peak_token = nil
- return token
- end
- token = next_token_cache()
- pos = self.pos()
- token.pos = pos
- return token
- end
- def next_token_cache()
- token = Token.new()
- token.data = ""
- while (char = @read.pop_char())
- #puts "#{char.inspect}:#{token.inspect}"
- if(char == "/")
- if(@read.pop_char == "/")
- @read.clear_comment()
- else
- syntax_error("unexpected #{char.inspect}")
- end
- elsif(char == "#")
- @read.clear_comment()
- elsif(char =~ /[\s\r\n]/)
- if(token.type)
- return token
- end
- elsif(char =~ /\"/)
- token.type = :tString
- token.data = ""
- while((char = @read.pop_char) != "\"")
- token.data += char
- end
- return token
- elsif(char =~ /[1-9]/)
- token.type = :tNumber
- token.data = char.to_i
- while(char != ".")
- char = @read.pop_char
- if(char =~ /[0-9]/)
- token.data = char.to_i + token.data * 10
- elsif(char == ".")
- else
- @read.unpop_char(char)
- return token
- end
- end
- second_char = 0
- man = 0
- while(true)
- char = @read.pop_char
- if(char =~ /[0-9]/)
- second_char = char.to_i + second_char * 10
- man = man * 10
- else
- @read.unpop_char(char)
- token.data += second_char / man.to_f
- return token
- end
- end
- elsif(char == ":")
- if(@read.pop_char == "=")
- return tokenize(":=")
- end
- syntax_error("unexpected \":\"")
- elsif(char =~ /[;\{\}=()\",\*\+\.\[\]]/)
- return(tokenize(char))
- elsif(char =~ /[a-zA-Z_]/)
- token.type = :tWord
- token.data = char
- while(true)
- char = @read.pop_char()
- if(char && char =~ /\w/)
- token.data += char
- else
- @read.unpop_char(char)
- return token
- end
- end
- elsif(char == "<")
- if((char = @read.pop_char()) == "<")
- return tokenize("<<")
- else
- @read.unpop_char(char)
- return tokenize("<")
- end
- else
- syntax_error("unexpected #{char.inspect}")
- end
- end
- token.type = :tEnd
- return token
- end
- def expect(type,&blk)
- token = self.next
- if(token != type)
- syntax_error(blk.call(token)) if(blk)
- syntax_error("unexpected: #{token.type}")
- end
- return token
- end
-end
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
deleted file mode 100644
index 0f5694a..0000000
--- a/aos/build/queues/output/message_dec.rb
+++ /dev/null
@@ -1,434 +0,0 @@
-begin
- require "sha1"
-rescue LoadError
- require "digest/sha1"
-end
-
-class Target::StructBase < Target::Node
- def create_DoGetType(type_class, cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"const ::aos::MessageType*","DoGetType")
- member_func.static = true
- fields = []
- register_members = []
- @members.each do |member|
- tId = member.getTypeID()
- fieldName = member.name.inspect
- if(member.respond_to?(:add_TypeRegister))
- register_members.push(member)
- end
- fields << "new ::aos::MessageType::Field{#{tId}, #{member.respond_to?(:length) ? member.length : 0}, #{fieldName}}"
- end
- register_members.uniq do |member|
- member.type
- end.each do |member|
- member.add_TypeRegister(cpp_tree, type_class, member_func)
- end
- id = getTypeID()
- member_func.suite << ("static const ::aos::MessageType kMsgMessageType(#{type_class.parent_class ? type_class.parent_class + '::Size()' : 0}, #{id}, #{(@loc.respond_to?(:queue_name) ? @loc.queue_name(@name) : "#{@loc.get_name()}.#{@name}").inspect}, {" +
- "#{fields.join(", ")}})");
- type_class.add_member(member_func)
- member_func.suite << "::aos::type_cache::Add(kMsgMessageType)"
- member_func.suite << CPP::Return.new("&kMsgMessageType")
- end
- def create_InOrderConstructor(type_class, cpp_tree)
- if @members.empty?
- return
- end
- cons = CPP::Constructor.new(type_class)
- type_class.add_member(cons)
- @members.each do |member|
- if member.respond_to?(:type_name)
- type_name = "#{member.type_name(cpp_tree)} #{member.name}_in"
- else
- type_name = member.create_usage(cpp_tree, "#{member.name}_in")
- end
-
- cons.args << type_name
- cons.add_cons(member.name, member.name + '_in')
- end
- end
- def create_DefaultConstructor(type_class, cpp_tree)
- cons = CPP::Constructor.new(type_class)
- type_class.add_member(cons)
- cons.add_cons(type_class.parent_class) if type_class.parent_class
- cons.suite << CPP::FuncCall.build('Zero')
- end
- def create_Zero(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"void","Zero")
- type_class.add_member(member_func)
- @members.each do |elem|
- elem.zeroCall(member_func.suite)
- end
- member_func.suite << CPP::FuncCall.new(type_class.parent_class + '::Zero') if type_class.parent_class
- end
- def create_Size(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"size_t","Size")
- member_func.inline = true
- member_func.static = true
- type_class.add_member(member_func.forward_dec)
- size = 0
- @members.each do |elem|
- size += elem.size
- end
- if type_class.parent_class
- member_func.suite << CPP::Return.new(CPP::Add.new(size,
- "#{type_class.parent_class}::Size()"))
- else
- member_func.suite << CPP::Return.new(size)
- end
- end
- def create_Serialize(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"size_t","Serialize")
- type_class.add_member(member_func)
- member_func.args << "char *buffer"
- member_func.suite << "#{type_class.parent_class}::Serialize(buffer)" if type_class.parent_class
- member_func.const = true
- offset = type_class.parent_class ? type_class.parent_class + '::Size()' : '0'
- @members.each do |elem|
- elem.toNetwork(offset,member_func.suite)
- offset += " + #{elem.size}";
- end
- member_func.suite << CPP::Return.new(CPP::FuncCall.new('Size'))
- end
- def create_Deserialize(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"size_t","Deserialize")
- type_class.add_member(member_func)
- member_func.args << "const char *buffer"
- member_func.suite << "#{type_class.parent_class}::Deserialize(buffer)" if type_class.parent_class
- offset = type_class.parent_class ? type_class.parent_class + '::Size()' : '0'
- @members.each do |elem|
- elem.toHost(offset,member_func.suite)
- offset += " + #{elem.size}";
- end
- member_func.suite << CPP::Return.new(CPP::FuncCall.new('Size'))
- end
- def create_EqualsNoTime(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"bool","EqualsNoTime")
- member_func.const = true
- type_class.add_member(member_func)
- member_func.args << "const #{type_class.name} &other"
- member_func.suite << "if (!#{type_class.parent_class}::EqualsNoTime(other)) return false;" if type_class.parent_class
- @members.each do |elem|
- if elem.respond_to? :create_EqualsNoTime
- member_func.suite << "if (!other.#{elem.name}.EqualsNoTime(#{elem.name})) return false;"
- elsif elem.respond_to?(:length) && elem.member_type.respond_to?(:create_EqualsNoTime)
- 0.upto(elem.length - 1) do |i|
- member_func.suite << "if (!other.#{elem.name}[#{i}].EqualsNoTime(#{elem.name}[#{i}])) return false;"
- end
- else
- member_func.suite << "if (other.#{elem.name} != #{elem.name}) return false;"
- end
- end
- member_func.suite << CPP::Return.new('true')
- end
- def simpleStr()
- return "{\n" + @members.collect() { |elem| elem.simpleStr() + "\n"}.join("") + "}"
- end
- def getTypeID()
- return "0x" + (((Digest::SHA1.hexdigest(simpleStr())[0..3].to_i(16)) << 16) | size).to_s(16)
- end
- def add_member(member)
- @members << member
- end
- def size()
- return @size if(@size)
- @size = 0
- @members.each do |elem|
- @size += elem.size
- end
- return @size
- end
-end
-
-class Target::MessageDec < Target::StructBase
- attr_accessor :name,:loc,:parent,:msg_hash
- def initialize(name)
- @name = name
- @members = []
- end
- def extern=(value)
- @extern=value
- end
- def get_name()
- if(@parent)
- return "#{@parent.get_name}.#{@name}"
- else
- return "#{@name}"
- end
- end
- def create_Print(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"size_t","Print")
- type_class.add_member(member_func)
- member_func.args << "char *buffer"
- member_func.args << "size_t length"
- member_func.const = true
- format = "\""
- args = []
- @members.each do |elem|
- format += ", "
- format += elem.toPrintFormat()
- elem.fetchPrintArgs(args)
- end
- format += "\""
- member_func.suite << "size_t super_size = ::aos::Message::Print(buffer, length)"
- member_func.suite << "buffer += super_size"
- member_func.suite << "length -= super_size"
- if !args.empty?
- member_func.suite << "return super_size + snprintf(buffer, length, " + ([format] + args).join(", ") + ")";
- else
- # snprintf will return zero.
- member_func.suite << "return super_size"
- end
- end
- def create_GetType(type_class, cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"const ::aos::MessageType*","GetType")
- type_class.add_member(member_func)
- member_func.static = true
- member_func.suite << "static ::aos::Once<const ::aos::MessageType> getter(#{type_class.name}::DoGetType)"
- member_func.suite << CPP::Return.new("getter.Get()")
- end
- def self.builder_loc(loc)
- return @builder_loc if(@builder_loc)
- return @builder_loc = loc.root.get_make("aos")
- end
- def type_name(builder_loc,name)
- if(builder_loc == @loc) #use relative name
- return name
- else #use full name
- return @loc.to_cpp_id(name)
- end
- end
- def create(cpp_tree)
- return self if(@extern)
- orig_namespace = namespace = cpp_tree.get(@loc)
- name = ""
- if(namespace.class < Types::Type) #is nested
- name = namespace.name + "_" + name
- namespace = namespace.space
- end
- type_class = namespace.add_struct(name + @name)
- if(name.length > 0)
- orig_namespace.add_member(:public, Types::TypeDef.new(type_class,@name))
- end
- cpp_tree.set(self,type_class)
- type_class.set_parent("public ::aos::Message")
- ts = self.simpleStr()
- self.msg_hash = "0x#{Digest::SHA1.hexdigest(ts)[-8..-1]}"
- type_class.add_member("enum {kQueueLength = 100, kHash = #{self.msg_hash}}")
- @members.each do |elem|
- type_class.add_member(elem.create_usage(cpp_tree))
- end
-
- create_Serialize(type_class,cpp_tree)
- create_Deserialize(type_class,cpp_tree)
- create_Zero(type_class,cpp_tree)
- create_Size(type_class,cpp_tree)
- create_Print(type_class,cpp_tree)
- create_GetType(type_class, cpp_tree)
- create_DoGetType(type_class, cpp_tree)
- create_DefaultConstructor(type_class, cpp_tree)
- create_InOrderConstructor(type_class, cpp_tree)
- create_EqualsNoTime(type_class, cpp_tree)
-
- b_namespace = cpp_tree.get(b_loc = self.class.builder_loc(@loc))
-
- template = b_namespace.add_template("MessageBuilder")
- template.spec_args << t = @loc.to_cpp_id(@name)
- msg_ptr_t = "ScopedMessagePtr< #{t}>"
- msg_bld_t = "MessageBuilder< #{t}>"
- template.add_member(:private,"#{msg_ptr_t} msg_ptr_")
- template.add_member(:private,"#{msg_bld_t}(const #{msg_bld_t}&)")
- template.add_member(:private,"void operator=(const #{msg_bld_t}&)")
- template.add_member(:private,"friend class ::aos::Queue< #{t}>")
-
- cons = CPP::Constructor.new(template)
- template.add_member(:private,cons)
- cons.args << "RawQueue *queue"
- cons.args << "#{t} *msg"
- cons.add_cons("msg_ptr_","queue","msg")
- template.public
- DefineMembers(cpp_tree, template, msg_bld_t)
- end
- def DefineMembers(cpp_tree, template, msg_bld_t)
- send = template.def_func("bool","Send")
- send.suite << CPP::Return.new("msg_ptr_.Send()")
- @members.each do |elem|
-=begin
- MessageBuilder<frc971::control_loops::Drivetrain::Goal> &steering(
- double steering) {
- msg_ptr_->steering = steering;
- return *this;
- }
-=end
- setter = template.def_func(msg_bld_t,"&#{elem.name}")
- setter.args << elem.create_usage(cpp_tree)
- elem.set_message_builder(setter.suite)
- setter.suite << CPP::Return.new("*this")
-
- end
- end
-end
-class Target::MessageElement < Target::Node
- attr_accessor :name,:loc,:size,:zero,:type,:printformat
- def initialize(type,name)
- @type,@name = type,name
- end
- def type()
- return @type
- end
- def type_name(cpp_tree)
- return type()
- end
- def toPrintFormat()
- return printformat
- end
- def create_usage(cpp_tree, this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- "#{@type} #{this_name}"
- end
- def toNetwork(offset,suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- suite << f_call = CPP::FuncCall.build("to_network",
- "&#{parent}#{this_name}",
- "&buffer[#{offset}]")
- f_call.args.dont_wrap = true
- end
- def toHost(offset,suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- suite << f_call = CPP::FuncCall.build("to_host",
- "&buffer[#{offset}]",
- "&#{parent}#{this_name}")
- f_call.args.dont_wrap = true
- end
- def getTypeID()
- '0x' + ((Digest::SHA1.hexdigest(@type)[0..3].to_i(16) << 16) |
- 0x2000 | # marks it as primitive
- size).to_s(16)
- end
- def simpleStr()
- "#{@type} #{@name}"
- end
- def set_message_builder(suite, this_name=nil)
- this_name ||= @name
- suite << "msg_ptr_->#{this_name} = #{this_name}"
- end
-
- def zeroCall(suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- suite << CPP::Assign.new(parent + this_name,@zero)
- end
- def fetchPrintArgs(args, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- if (@type == 'bool')
- args.push("#{parent}#{this_name} ? 'T' : 'f'")
- elsif (@type == '::aos::monotonic_clock::time_point')
- args.push(["AOS_TIME_ARGS(static_cast<int32_t>(",
- "::std::chrono::duration_cast<::std::chrono::seconds>(",
- "#{parent}#{this_name}.time_since_epoch()).count()), ",
- "static_cast<uint32_t>(::std::chrono::duration_cast<::std::chrono::nanoseconds>(",
- "#{parent}#{this_name}.time_since_epoch() - ",
- "::std::chrono::duration_cast<::std::chrono::seconds>(",
- "#{parent}#{this_name}.time_since_epoch())).count()))"].join(''))
- else
- args.push("#{parent}#{this_name}")
- end
- end
-end
-class Target::MessageArrayElement < Target::Node
- attr_accessor :loc, :length
- def initialize(type,length)
- @type,@length = type,length
- end
- def zero()
- return @type.zero()
- end
- def name()
- return @type.name()
- end
-
- def add_TypeRegister(*args)
- if @type.respond_to?(:add_TypeRegister)
- @type.add_TypeRegister(*args)
- end
- end
-
- def toPrintFormat()
- return ([@type.toPrintFormat] * @length).join(", ")
- end
- def create_usage(cpp_tree, this_name=nil)
- if (this_name == nil)
- this_name = name
- end
- return "::std::array< #{@type.type_name(cpp_tree)}, #{@length}> #{this_name}"
- end
- def type()
- return "::std::array< #{@type.type()}, #{@length}>"
- end
- def member_type()
- return @type
- end
- def simpleStr()
- return "#{@type.type} #{@name}[#{@length}]"
- end
- def getTypeID()
- return @type.getTypeID()
- end
- def size()
- return @type.size * @length
- end
- def toNetwork(offset, suite, parent="", this_name=nil)
- if (this_name == nil)
- this_name = name
- end
- offset = (offset == 0) ? "" : "#{offset} + "
- for_loop_var = getForLoopVar()
- suite << for_stmt = CPP::For.new("int #{for_loop_var} = 0","#{for_loop_var} < #{@length}","#{for_loop_var}++")
- @type.toNetwork("#{offset} (#{for_loop_var} * #{@type.size})", for_stmt.suite, parent, "#{this_name}[#{for_loop_var}]")
- end
- def toHost(offset, suite, parent="", this_name=nil)
- if (this_name == nil)
- this_name = name
- end
- offset = (offset == 0) ? "" : "#{offset} + "
- for_loop_var = getForLoopVar()
- suite << for_stmt = CPP::For.new("int #{for_loop_var} = 0","#{for_loop_var} < #{@length}","#{for_loop_var}++")
- @type.toHost("#{offset} (#{for_loop_var} * #{@type.size})", for_stmt.suite, parent, "#{this_name}[#{for_loop_var}]")
- end
- def set_message_builder(suite, this_name=nil)
- if (this_name == nil)
- this_name = name
- end
-
- for_loop_var = getForLoopVar()
- suite << for_stmt = CPP::For.new("int #{for_loop_var} = 0","#{for_loop_var} < #{@length}","#{for_loop_var}++")
- @type.set_message_builder(for_stmt.suite, "#{this_name}[#{for_loop_var}]")
- end
- def zeroCall(suite, parent="", this_name=nil)
- if (this_name == nil)
- this_name = name
- end
-
- offset = (offset == 0) ? "" : "#{offset} + "
- for_loop_var = getForLoopVar()
- suite << for_stmt = CPP::For.new("int #{for_loop_var} = 0","#{for_loop_var} < #{@length}","#{for_loop_var}++")
- @type.zeroCall(for_stmt.suite, parent, "#{this_name}[#{for_loop_var}]")
- end
- def fetchPrintArgs(args, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = name
- end
- for i in 0..(@length-1)
- @type.fetchPrintArgs(args, parent, "#{this_name}[#{i}]")
- end
- end
-end
diff --git a/aos/build/queues/output/q_file.rb b/aos/build/queues/output/q_file.rb
deleted file mode 100644
index 3bbdaed..0000000
--- a/aos/build/queues/output/q_file.rb
+++ /dev/null
@@ -1,200 +0,0 @@
-begin
- require "sha1"
-rescue LoadError
- require "digest/sha1"
-end
-
-$i = 0
-def getForLoopVar()
- $i = $i + 1
- return "_autogen_index_#{$i}"
-end
-
-module Target
-end
-class Target::Node
- attr_accessor :created_by
- def get_name()
- if(@parent)
- return "#{@parent.get_name}.#{@name}"
- else
- return "#{@name}"
- end
- end
-end
-class Target::QFile < Target::Node
- def initialize() #needs to know repo_path,
- @class_types = []
- end
- def add_type(type)
- @class_types << type
- end
- def extern=(value)
- @class_types.each do |type|
- type.extern=value
- end
- end
- def make_cpp_tree(rel_path)
- cpp_tree = DepFilePair.new(rel_path)
- cpp_tree.add_header_include("<array>")
- cpp_tree.add_header_include("\"aos/macros.h\"")
- cpp_tree.add_header_include("\"aos/queue.h\"")
- @class_types.each do |type|
- cpp_tree.get(type)
- end
- return cpp_tree
- end
-end
-class Target::QInclude < Target::Node
- def initialize(path)
- @path = path
- end
- def extern=(value)
- @extern=value
- end
- def create(cpp_tree)
- return if (@extern)
-# inc = cpp_tree.header.add_include("\"#{@path}\"")
- inc = cpp_tree.add_header_include("\"#{@path}\"")
- cpp_tree.set(self,inc)
- end
-end
-class Target::QueueGroupDec < Target::Node
- attr_accessor :name,:loc,:parent,:queues
- def initialize(name)
- @name = name
- @queues = []
- @subs = {}
- end
- def root()
- @parent.root()
- end
- def []=(key,val)
- #puts "#{key}= #{val}"
- @subs[key] = val
- end
- def extern=(value)
- @extern=value
- end
- def get_name()
- if(@parent)
- return "#{@parent.get_name}.#{@name}"
- else
- return "#{@name}"
- end
- end
- def to_cpp_id(id)
- name = "#{@name}::#{id}"
- return @parent.to_cpp_id(name) if(@parent)
- return name
- end
- def [](key)
- #puts "#{key}"
- @subs[key]
- end
- def add_queue(queue)
- @queues.push(queue)
- end
- def hash_with_name(name)
- ts = (@queues.collect { |queue|
- queue.msg_hash()
- }).join("") + name
- return "0x#{Digest::SHA1.hexdigest(ts)[-8..-1]}"
- end
- def create(cpp_tree)
- return if(@extern)
- namespace = cpp_tree.get(@loc)
- type_class = Types::Class.new(namespace,@name)
- cpp_tree.set(self,type_class) #breaks infinite recursion
- type_class.set_parent("public ::aos::QueueGroup")
- @queues.each do |queue|
- type_class.add_member(:public,queue.create_usage(cpp_tree))
- end
- create_Constructor(type_class,cpp_tree)
- namespace.add(type_class)
- end
- def create_Constructor(type_class,cpp_tree)
- member_func = CPP::Constructor.new(type_class)
- type_class.add_member(:public,member_func)
- #cpp_tree.cc_file.add_funct(member_func)
- member_func.args << "const char *name";
- member_func.add_cons("::aos::QueueGroup","name")
- @queues.each do |queue|
- member_func.args << "const char *#{queue.name}_name";
- member_func.add_cons(queue.name,"#{queue.name}_name")
- end
- end
-end
-class Target::QueueGroup < Target::Node
- attr_accessor :name,:loc
- def initialize(type,name)
- @type,@name = type,name
- end
- def type_name()
- if(@type.loc == @loc) #use relative name
- return @type.name
- else #use full name
- return @type.loc.to_cpp_id(@type.name)
- end
- end
- def create(cpp_tree)
- namespace = cpp_tree.get(@loc)
- type = cpp_tree.get(@type)
- cpp_tree.set(self,self)
- namespace.add_cc("static #{type_name} *#{@name}_ptr")
- counter = "#{@name}_counter"
- namespace.add_cc("static int #{counter}")
-
- str = <<COMMENT_END
-Schwarz counter to construct and destruct the #{@name} object.
-Must be constructed before &#{@name} is constructed so that #{@name}_ptr
-is valid so we can store a reference to the object.
-COMMENT_END
- str.split(/\n/).each do |str_sec|
- namespace.class_comment(str_sec)
- end
- init_class = namespace.add_class("InitializerFor_" + @name).add_dep(type)
-
-
- init_class.add_cc_comment(
- "The counter is initialized at load-time, i.e., before any of the static",
- "objects are initialized.")
-
-
- cons = CPP::Constructor.new(init_class)
- init_class.add_member(:public,cons)
- cons.suite << if_stmt = CPP::If.new("0 == #{counter}++")
-
- cons_call = CPP::FuncCall.new("new #{type_name}")
- cons_call.args.push(@loc.queue_name(@name).inspect)
-
- @type.queues.collect do |queue|
- cons_call.args.push(@loc.queue_name(@name + "." + queue.name).inspect)
- end
- if_stmt.suite << CPP::Assign.new("#{@name}_ptr",cons_call)
-
-
- destruct = CPP::Destructor.new(init_class)
- destruct.suite << if_stmt = CPP::If.new("0 == --#{counter}")
- if_stmt.suite << "delete #{@name}_ptr"
- if_stmt.suite << CPP::Assign.new("#{@name}_ptr","NULL")
-
- init_class.add_member(:public,destruct)
- init_class.static = true
- init_class.dec = @name + "_initializer";
-
- str = <<COMMENT_END
-Create a reference to the new object in the pointer. Since we have already
-created the initializer
-COMMENT_END
- str.split(/\n/).map{|str_sec| CPP::Comment.new(str_sec)}.each do |comment|
- namespace.add(comment)
- end
- namespace.add("static UNUSED_VARIABLE #{type_name} &#{@name}" +
- " = #{@name}_initializer.get()")
-
- get = init_class.def_func(type_name,"get") #.add_dep(type)
- get.pre_func_types = "&"
- get.suite << CPP::Return.new("*#{@name}_ptr")
- end
-end
diff --git a/aos/build/queues/output/q_struct.rb b/aos/build/queues/output/q_struct.rb
deleted file mode 100644
index f538bb3..0000000
--- a/aos/build/queues/output/q_struct.rb
+++ /dev/null
@@ -1,156 +0,0 @@
-class Target::StructDec < Target::StructBase
- attr_accessor :name,:loc,:parent, :extern
- def initialize(name)
- @name = name
- @members = []
- end
- def add_member(member)
- @members << member
- end
- def create_GetType(type_class, cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"const ::aos::MessageType*","GetType")
- type_class.add_member(member_func)
- member_func.static = true
- member_func.suite << "static ::aos::Once<const ::aos::MessageType> getter(#{type_class.name}::DoGetType)"
- member_func.suite << CPP::Return.new("getter.Get()")
- end
- def create(cpp_tree)
- return self if(@extern)
- orig_namespace = namespace = cpp_tree.get(@loc)
- name = ""
- if(namespace.class < Types::Type) #is nested
- name = namespace.name + "_" + name
- namespace = namespace.space
- end
- type_class = namespace.add_struct(name + @name)
-
- @members.each do |elem|
- type_class.add_member(elem.create_usage(cpp_tree))
- end
- create_DoGetType(type_class, cpp_tree)
- create_GetType(type_class, cpp_tree)
- create_DefaultConstructor(type_class, cpp_tree)
- create_InOrderConstructor(type_class, cpp_tree)
- create_Zero(type_class, cpp_tree)
- create_Size(type_class, cpp_tree)
- create_Serialize(type_class, cpp_tree)
- create_Deserialize(type_class, cpp_tree)
- create_EqualsNoTime(type_class, cpp_tree)
- return type_class
- end
- def getPrintFormat()
- return "{" + @members.collect { |elem| elem.toPrintFormat() }.join(", ") + "}"
- end
- def fetchPrintArgs(args, parent = "")
- @members.each do |elem|
- elem.fetchPrintArgs(args, parent)
- end
- end
- def toHost(offset, suite, parent)
- @members.each do |elem|
- elem.toHost(offset, suite, parent)
- offset += " + #{elem.size()}"
- end
- end
- def toNetwork(offset, suite, parent)
- @members.each do |elem|
- elem.toNetwork(offset, suite, parent)
- offset += " + #{elem.size()}"
- end
- end
- def zeroCall(suite, parent)
- @members.each do |elem|
- elem.zeroCall(suite, parent)
- end
- end
-end
-class Target::MessageStructElement < Target::Node
- attr_accessor :name,:loc
- def initialize(type,name)
- @type, @name = type, name
- end
- def type()
- return @type.get_name
- end
- def create_EqualsNoTime(type_class,cpp_tree)
- member_func = CPP::MemberFunc.new(type_class,"bool","EqualsNoTime")
- member_func.const = true
- type_class.add_member(member_func)
- member_func.args << "const #{type_class.name} &other"
- member_func.suite << "if (!#{type_class.parent_class}::EqualsNoTime(other)) return false;" if type_class.parent_class
- @members.each do |elem|
- if elem.respond_to? :create_EqualsNoTime
- member_func.suite << "if (!other.#{elem.name}.EqualsNoTime(#{elem.name})) return false;"
- elsif elem.respond_to?(:length) && elem.member_type.respond_to?(:create_EqualsNoTime)
- 0.upto(elem.length - 1) do |i|
- member_func.suite << "if (!other.#{elem.name}[#{i}].EqualsNoTime(#{elem.name}[#{i}])) return false;"
- end
- else
- member_func.suite << "if (other.#{elem.name} != #{elem.name}) return false;"
- end
- end
- member_func.suite << CPP::Return.new('true')
- end
- def type_name(cpp_tree)
- type = cpp_tree.get(@type)
- if(@type.loc == @loc) #use relative name
- return type.name
- else #use full name
- return @type.loc.to_cpp_id(type.name)
- end
- end
- def size()
- return @type.size()
- end
- def toPrintFormat()
- @type.getPrintFormat()
- end
- def create_usage(cpp_tree, this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- return "#{type_name(cpp_tree)} #{this_name}"
- end
- def add_TypeRegister(cpp_tree, o_type, member_func)
- type = cpp_tree.get(@type)
- tName = @type.loc.to_cpp_id(type.name)
- member_func.suite << "#{tName}::GetType()"
- end
- def fetchPrintArgs(args, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- @type.fetchPrintArgs(args, parent + "#{this_name}.")
- end
- def toNetwork(offset,suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- @type.toNetwork(offset, suite, parent + "#{this_name}.")
- end
- def toHost(offset,suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- @type.toHost(offset, suite, parent + "#{this_name}.")
- end
- def set_message_builder(suite, this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- suite << "msg_ptr_->#{this_name} = #{this_name}"
- end
-
- def zeroCall(suite, parent = "", this_name=nil)
- if (this_name == nil)
- this_name = @name
- end
- @type.zeroCall(suite, parent + "#{this_name}.")
- end
- def simpleStr()
- "#{@type.simpleStr()} #{@name}"
- end
- def getTypeID()
- return @type.getTypeID()
- end
-end
diff --git a/aos/build/queues/output/queue_dec.rb b/aos/build/queues/output/queue_dec.rb
deleted file mode 100644
index 031c033..0000000
--- a/aos/build/queues/output/queue_dec.rb
+++ /dev/null
@@ -1,89 +0,0 @@
-class Target::QueueDec < Target::Node
- attr_accessor :name,:loc
- def initialize(type,name)
- @type,@name = type,name
- end
- def msg_hash()
- return @type.msg_hash
- end
- def full_message_name(cpp_tree)
- type = cpp_tree.get(@type)
- return @type.loc.to_cpp_id(type.name)
- end
- def full_builder_name(cpp_tree)
- return "::aos::MessageBuilder< #{full_message_name(cpp_tree)}>"
- end
- def full_type_name(cpp_tree)
- return "::aos::Queue< #{full_message_name(cpp_tree)}>"
- end
- def type_name(cpp_tree,queue_type = "::aos::Queue")
- type = cpp_tree.get(@type)
- if(@type.loc == @loc) #use relative name
- return "#{queue_type}<#{type.name}>"
- else #use full name
- return "#{queue_type}< #{@type.loc.to_cpp_id(type.name)}>"
- end
- end
- def create_usage(cpp_tree)
- "#{type_name(cpp_tree)} #{@name}"
- end
- def create(cpp_tree)
- namespace = cpp_tree.get(@loc)
- type = cpp_tree.get(@type)
- cpp_tree.set(self,self)
- type_name = type_name(cpp_tree)
- full_type_name = full_type_name(cpp_tree)
- namespace.add_cc("static #{type_name} *#{@name}_ptr")
- counter = "#{@name}_counter"
- namespace.add_cc("static int #{counter}")
-
- str = <<COMMENT_END
-Schwarz counter to construct and destruct the #{@name} queue.
-Must be constructed before &#{@name} is constructed so that #{@name}_ptr
-is valid so we can store a reference to the object.
-COMMENT_END
- str.split(/\n/).each do |str_sec|
- namespace.class_comment(str_sec)
- end
- init_class = namespace.add_class("InitializerFor_" + @name)
-
- init_class.add_cc_comment(
- "The counter is initialized at load-time, i.e., before any of the static",
- "objects are initialized.")
-
-
- cons = CPP::Constructor.new(init_class)
- init_class.add_member(:public,cons)
- cons.suite << if_stmt = CPP::If.new("0 == #{counter}++")
-
- cons_call = CPP::FuncCall.new("new #{type_name}")
- cons_call.args.push(@loc.queue_name(@name).inspect)
- if_stmt.suite << CPP::Assign.new("#{@name}_ptr",cons_call)
-
-
- destruct = CPP::Destructor.new(init_class)
- init_class.add_member(:public,destruct)
- destruct.suite << if_stmt = CPP::If.new("0 == --#{counter}")
- if_stmt.suite << "delete #{@name}_ptr"
- if_stmt.suite << CPP::Assign.new("#{@name}_ptr","NULL")
-
- init_class.static = true
- init_class.dec = @name + "_initializer";
-
- str = <<COMMENT_END
-Create a reference to the new object in the pointer. Since we have already
-created the initializer
-COMMENT_END
- str.split(/\n/).map{|str_sec| CPP::Comment.new(str_sec)}.each do |comment|
- namespace.add(comment)
- end
- namespace.add("static UNUSED_VARIABLE #{type_name} &#{@name}" +
- " = #{@name}_initializer.get()")
-
- get = init_class.def_func(full_type_name,"get")
- get.pre_func_types = "&"
- get.suite << CPP::Return.new("*#{@name}_ptr")
-
- end
-end
-
diff --git a/aos/build/queues/print_field.rb b/aos/build/queues/print_field.rb
deleted file mode 100644
index 4f26d04..0000000
--- a/aos/build/queues/print_field.rb
+++ /dev/null
@@ -1,118 +0,0 @@
-require_relative 'load.rb'
-
-# TODO(brians): Special-case Time too and float/double if we can find a good way to do it.
-GenericTypeNames = ['float', 'double', 'char']
-TimeTypeNames = ['::aos::monotonic_clock::time_point']
-IntegerSizes = [8, 16, 32, 64]
-
-WriteIffChanged.open(ARGV[0]) do |output|
- output.puts <<END
-// This file is generated by #{File.expand_path(__FILE__)}.
-// DO NOT EDIT BY HAND!
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <inttypes.h>
-#include <stdio.h>
-
-#include "aos/byteorder.h"
-#include "aos/time/time.h"
-#include "aos/print_field_helpers.h"
-#include "aos/logging/printf_formats.h"
-
-namespace aos {
-
-bool PrintField(char *output, size_t *output_bytes, const void *input,
- size_t *input_bytes, uint32_t type) {
- switch (type) {
-#{GenericTypeNames.collect do |name|
- message_element = Target::MessageElement.new(name, 'value')
- statement = MessageElementStmt.new(name, 'value')
- message_element.size = statement.size
- print_args = []
- message_element.fetchPrintArgs(print_args)
- next <<END2
- case #{message_element.getTypeID()}:
- {
- if (*input_bytes < #{statement.size}) return false;
- *input_bytes -= #{statement.size};
- #{name} value;
- to_host(static_cast<const char *>(input), &value);
- int ret = snprintf(output, *output_bytes,
- "#{statement.toPrintFormat()}",
- #{print_args[0]});
- if (ret < 0) return false;
- if (static_cast<unsigned int>(ret) >= *output_bytes) return false;
- *output_bytes -= ret;
- return true;
- }
-END2
-end.join('')}
-#{TimeTypeNames.collect do |name|
- message_element = Target::MessageElement.new(name, 'value')
- statement = MessageElementStmt.new(name, 'value')
- message_element.size = statement.size
- print_args = []
- message_element.fetchPrintArgs(print_args)
- next <<END2
- case #{message_element.getTypeID()}:
- {
- if (*input_bytes < #{statement.size}) return false;
- *input_bytes -= #{statement.size};
- int32_t upper;
- uint32_t lower;
- to_host(static_cast<const char *>(input), &upper);
- to_host(static_cast<const char *>(input) + 4, &lower);
- #{name} value(::std::chrono::seconds(upper) + ::std::chrono::nanoseconds(lower));
- int ret = snprintf(output, *output_bytes,
- "#{statement.toPrintFormat()}",
- #{print_args[0]});
- if (ret < 0) return false;
- if (static_cast<unsigned int>(ret) >= *output_bytes) return false;
- *output_bytes -= ret;
- return true;
- }
-END2
-end.join('')}
-#{IntegerSizes.collect do |size|
- [true, false].collect do |signed|
- size_bytes = size / 8
- name = "#{signed ? '' : 'u'}int#{size}_t"
- message_element = Target::MessageElement.new(name, 'value')
- message_element.size = size_bytes
- next <<END2
- case #{message_element.getTypeID()}:
- {
- if (*input_bytes < #{size_bytes}) return false;
- *input_bytes -= #{size_bytes};
- #{name} value;
- to_host(static_cast<const char *>(input), &value);
- return PrintInteger<#{name}>(output, value, output_bytes);
- }
-END2
- end
-end.flatten.join('')}
-#{
- message_element = Target::MessageElement.new('bool', 'value')
- message_element.size = 1
- r = <<END2
- case #{message_element.getTypeID()}:
- {
- if (*input_bytes < 1) return false;
- if (*output_bytes < 1) return false;
- *input_bytes -= 1;
- bool value = static_cast<const char *>(input)[0];
- *output_bytes -= 1;
- *output = value ? 'T' : 'f';
- return true;
- }
-END2
-}
- default:
- return false;
- }
-}
-
-} // namespace aos
-END
-end
diff --git a/aos/build/queues/q_specs/nested_structs.q b/aos/build/queues/q_specs/nested_structs.q
deleted file mode 100644
index 7162620..0000000
--- a/aos/build/queues/q_specs/nested_structs.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package aos.test;
-
-import "q_specs/struct_test.q";
-
-
-message Position {
- int32_t a;
- int32_t[2] b;
- .aos.test2.ClawHalfPosition top;
- .aos.test2.ClawHalfPosition bottom;
- bool c;
-};
diff --git a/aos/build/queues/q_specs/struct_queue_group.q b/aos/build/queues/q_specs/struct_queue_group.q
deleted file mode 100644
index 2bc5046..0000000
--- a/aos/build/queues/q_specs/struct_queue_group.q
+++ /dev/null
@@ -1,20 +0,0 @@
-package aos.test;
-
-struct Claw {
- int32_t a;
- int32_t b;
-};
-
- message HPosition {
- Claw top;
- };
-
-// All angles here are 0 horizontal, positive up.
-queue_group ClawGroup {
-
- message Position {
- Claw top;
- };
- queue Position kjks;
-};
-
diff --git a/aos/build/queues/q_specs/struct_test.q b/aos/build/queues/q_specs/struct_test.q
deleted file mode 100644
index afb22ac..0000000
--- a/aos/build/queues/q_specs/struct_test.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package aos.test2;
-
-struct SubStruct {
- int32_t g;
-};
-
-struct ClawHalfPosition {
- double position;
- bool hall_effect;
- SubStruct clampy;
-};
-
diff --git a/aos/build/queues/queue_primitives.rb b/aos/build/queues/queue_primitives.rb
deleted file mode 100644
index a5f6624..0000000
--- a/aos/build/queues/queue_primitives.rb
+++ /dev/null
@@ -1,54 +0,0 @@
-require_relative 'load.rb'
-
-require 'fileutils'
-
-TypeNames = [8, 16, 32, 64].collect do |size|
- ["uint#{size}_t", "int#{size}_t"]
-end.flatten + ['bool', 'float', 'char', 'double', '::aos::monotonic_clock::time_point']
-
-FileUtils.mkdir_p(File.dirname(ARGV[0]))
-WriteIffChanged.open(ARGV[0]) do |output|
- output.puts <<END
-// This file is generated by #{File.expand_path(__FILE__)}.
-// DO NOT EDIT BY HAND!
-
-#include <stdint.h>
-
-#include "aos/time/time.h"
-
-namespace aos {
-namespace queue_primitive_types {
-#{TypeNames.collect do |name|
- message_element = Target::MessageElement.new(name, 'value')
- statement = MessageElementStmt.new(name, 'value')
- message_element.size = statement.size
- name = 'Time' if name == '::aos::monotonic_clock::time_point'
- next <<END2
- static const uint32_t #{name}_p = #{message_element.getTypeID()};
-END2
-end.join('')}
-} // namespace queue_primitive_types
-
-// A class for mapping an actual type to a type ID.
-// There are specializations for all of the actual primitive types.
-template<typename T>
-class TypeID {
- public:
- static const uint32_t id;
-};
-
-#{TypeNames.collect do |name|
- message_element = Target::MessageElement.new(name, 'value')
- statement = MessageElementStmt.new(name, 'value')
- message_element.size = statement.size
- next <<END2
-template<>
-class TypeID< #{name}> {
- public:
- static const uint32_t id = #{message_element.getTypeID()};
-};
-END2
-end.join('')}
-} // namespace aos
-END
-end
diff --git a/aos/build/queues/write_iff_changed.rb b/aos/build/queues/write_iff_changed.rb
deleted file mode 100644
index 74373a9..0000000
--- a/aos/build/queues/write_iff_changed.rb
+++ /dev/null
@@ -1,30 +0,0 @@
-require 'tempfile'
-
-# Writes a file like normal except doesn't do anything if the new contents are
-# the same as what's already there. Useful for helping build tools not rebuild
-# if generated files are re-generated but don't change.
-# Usable as a standard File, including to redirect to with IO.popen etc.
-class WriteIffChanged < Tempfile
- def initialize(filename)
- super('write_iff_changed')
- @filename = filename
- end
- def WriteIffChanged.open(filename)
- out = WriteIffChanged.new(filename)
- yield out
- out.close
- end
- def close
- flush
- contents = File.open(path, 'r') do |f|
- f.readlines(nil)
- end
- if !File.exists?(@filename) ||
- File.new(@filename, 'r').readlines(nil) != contents
- File.open(@filename, 'w') do |out|
- out.write(contents[0])
- end
- end
- super
- end
-end
diff --git a/aos/condition.cc b/aos/condition.cc
index 0c81e2c..e4276b6 100644
--- a/aos/condition.cc
+++ b/aos/condition.cc
@@ -4,9 +4,9 @@
#include <inttypes.h>
#include <time.h>
-#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/type_traits/type_traits.h"
+#include "glog/logging.h"
namespace aos {
@@ -28,7 +28,7 @@
const bool do_timeout = timeout != chrono::nanoseconds(0);
if (do_timeout) {
- AOS_PCHECK(clock_gettime(CLOCK_MONOTONIC, &end_time) == 0);
+ PCHECK(clock_gettime(CLOCK_MONOTONIC, &end_time) == 0);
timeout += chrono::nanoseconds(end_time.tv_nsec);
chrono::seconds timeout_seconds =
chrono::duration_cast<chrono::seconds>(timeout);
diff --git a/aos/config.bzl b/aos/config.bzl
new file mode 100644
index 0000000..3f78422
--- /dev/null
+++ b/aos/config.bzl
@@ -0,0 +1,60 @@
+load("//tools/build_rules:label.bzl", "expand_label")
+
+AosConfigInfo = provider(fields = ["transitive_flatbuffers", "transitive_src"])
+
+def aos_config(name, src, flatbuffers, deps = [], visibility = None):
+ _aos_config(
+ name = name,
+ src = src,
+ deps = deps,
+ flatbuffers = [expand_label(flatbuffer) + "_reflection_out" for flatbuffer in flatbuffers],
+ visibility = visibility,
+ )
+
+def _aos_config_impl(ctx):
+ flatbuffers_depset = depset(
+ ctx.files.flatbuffers,
+ transitive = [dep[AosConfigInfo].transitive_flatbuffers for dep in ctx.attr.deps],
+ )
+
+ src_depset = depset(
+ ctx.files.src,
+ transitive = [dep[AosConfigInfo].transitive_src for dep in ctx.attr.deps],
+ )
+
+ all_files = flatbuffers_depset.to_list() + src_depset.to_list()
+ ctx.actions.run(
+ outputs = [ctx.outputs.config],
+ inputs = all_files,
+ arguments = [ctx.outputs.config.path, ctx.files.src[0].path] + [f.path for f in flatbuffers_depset.to_list()],
+ progress_message = "Flattening config",
+ executable = ctx.executable._config_flattener,
+ )
+ return AosConfigInfo(
+ transitive_flatbuffers = flatbuffers_depset,
+ transitive_src = src_depset,
+ )
+
+_aos_config = rule(
+ attrs = {
+ "_config_flattener": attr.label(
+ executable = True,
+ cfg = "host",
+ default = Label("//aos:config_flattener"),
+ ),
+ "src": attr.label(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "deps": attr.label_list(
+ providers = [AosConfigInfo],
+ ),
+ "flatbuffers": attr.label_list(
+ mandatory = False,
+ ),
+ },
+ outputs = {
+ "config": "%{name}.json",
+ },
+ implementation = _aos_config_impl,
+)
diff --git a/aos/config_flattener.cc b/aos/config_flattener.cc
new file mode 100644
index 0000000..71cda32
--- /dev/null
+++ b/aos/config_flattener.cc
@@ -0,0 +1,41 @@
+#include <string>
+#include <vector>
+
+#include "aos/configuration.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+int Main(int argc, char **argv) {
+ CHECK_GE(argc, 3) << ": Too few arguments";
+
+ VLOG(1) << "Reading " << argv[2];
+ FlatbufferDetachedBuffer<Configuration> config =
+ configuration::ReadConfig(argv[2]);
+
+ std::vector<aos::FlatbufferString<reflection::Schema>> schemas;
+
+ for (int i = 3; i < argc; ++i) {
+ schemas.emplace_back(util::ReadFileToStringOrDie(argv[i]));
+ }
+
+ const std::string merged_config = FlatbufferToJson(
+ &configuration::MergeConfiguration(config, schemas).message(), true);
+
+ // TODO(austin): Figure out how to squash the schemas onto 1 line so it is
+ // easier to read?
+ VLOG(1) << "Flattened config is " << merged_config;
+ util::WriteStringToFileOrDie(argv[1], merged_config);
+ return 0;
+}
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ return aos::Main(argc, argv);
+}
diff --git a/aos/configuration.cc b/aos/configuration.cc
index cd92f20..bcb67ba 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -22,11 +22,6 @@
// Define the compare and equal operators for Channel and Application so we can
// insert them in the btree below.
-//
-// These are not in headers because they are only comparing part of the
-// flatbuffer, and it seems weird to expose that as *the* compare operator. And
-// I can't put them in an anonymous namespace because they wouldn't be found
-// that way by the btree.
bool operator<(const FlatbufferDetachedBuffer<Channel> &lhs,
const FlatbufferDetachedBuffer<Channel> &rhs) {
int name_compare = lhs.message().name()->string_view().compare(
@@ -154,8 +149,12 @@
FlatbufferDetachedBuffer<Configuration> ReadConfig(
const absl::string_view path, absl::btree_set<std::string> *visited_paths) {
- FlatbufferDetachedBuffer<Configuration> config(JsonToFlatbuffer(
- util::ReadFileToStringOrDie(path), ConfigurationTypeTable()));
+ flatbuffers::DetachedBuffer buffer = JsonToFlatbuffer(
+ util::ReadFileToStringOrDie(path), ConfigurationTypeTable());
+
+ CHECK_GT(buffer.size(), 0u) << ": Failed to parse JSON file";
+
+ FlatbufferDetachedBuffer<Configuration> config(std::move(buffer));
// Depth first. Take the following example:
//
// config1.json:
@@ -222,7 +221,54 @@
return config;
}
-// Remove duplicate entries, and handle overrides.
+// Compares (c < p) a channel, and a name, type tuple.
+bool CompareChannels(const Channel *c,
+ ::std::pair<absl::string_view, absl::string_view> p) {
+ int name_compare = c->name()->string_view().compare(p.first);
+ if (name_compare == 0) {
+ return c->type()->string_view() < p.second;
+ } else if (name_compare < 0) {
+ return true;
+ } else {
+ return false;
+ }
+};
+
+// Compares for equality (c == p) a channel, and a name, type tuple.
+bool EqualsChannels(const Channel *c,
+ ::std::pair<absl::string_view, absl::string_view> p) {
+ return c->name()->string_view() == p.first &&
+ c->type()->string_view() == p.second;
+}
+
+// Compares (c < p) an application, and a name;
+bool CompareApplications(const Application *a, absl::string_view name) {
+ return a->name()->string_view() < name;
+};
+
+// Compares for equality (c == p) an application, and a name;
+bool EqualsApplications(const Application *a, absl::string_view name) {
+ return a->name()->string_view() == name;
+}
+
+// Maps name for the provided maps. Modifies name.
+void HandleMaps(const flatbuffers::Vector<flatbuffers::Offset<aos::Map>> *maps,
+ absl::string_view *name) {
+ // For the same reason we merge configs in reverse order, we want to process
+ // maps in reverse order. That lets the outer config overwrite channels from
+ // the inner configs.
+ for (auto i = maps->rbegin(); i != maps->rend(); ++i) {
+ if (i->has_match() && i->match()->has_name() && i->has_rename() &&
+ i->rename()->has_name() && i->match()->name()->string_view() == *name) {
+ VLOG(1) << "Renamed \"" << *name << "\" to \""
+ << i->rename()->name()->string_view() << "\"";
+ *name = i->rename()->name()->string_view();
+ }
+ }
+}
+
+} // namespace
+
FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
const Flatbuffer<Configuration> &config) {
// Store all the channels in a sorted set. This lets us track channels we
@@ -316,54 +362,6 @@
return fbb.Release();
}
-// Compares (c < p) a channel, and a name, type tuple.
-bool CompareChannels(const Channel *c,
- ::std::pair<absl::string_view, absl::string_view> p) {
- int name_compare = c->name()->string_view().compare(p.first);
- if (name_compare == 0) {
- return c->type()->string_view() < p.second;
- } else if (name_compare < 0) {
- return true;
- } else {
- return false;
- }
-};
-
-// Compares for equality (c == p) a channel, and a name, type tuple.
-bool EqualsChannels(const Channel *c,
- ::std::pair<absl::string_view, absl::string_view> p) {
- return c->name()->string_view() == p.first &&
- c->type()->string_view() == p.second;
-}
-
-// Compares (c < p) an application, and a name;
-bool CompareApplications(const Application *a, absl::string_view name) {
- return a->name()->string_view() < name;
-};
-
-// Compares for equality (c == p) an application, and a name;
-bool EqualsApplications(const Application *a, absl::string_view name) {
- return a->name()->string_view() == name;
-}
-
-// Maps name for the provided maps. Modifies name.
-void HandleMaps(const flatbuffers::Vector<flatbuffers::Offset<aos::Map>> *maps,
- absl::string_view *name) {
- // For the same reason we merge configs in reverse order, we want to process
- // maps in reverse order. That lets the outer config overwrite channels from
- // the inner configs.
- for (auto i = maps->rbegin(); i != maps->rend(); ++i) {
- if (i->has_match() && i->match()->has_name() && i->has_rename() &&
- i->rename()->has_name() && i->match()->name()->string_view() == *name) {
- VLOG(1) << "Renamed \"" << *name << "\" to \""
- << i->rename()->name()->string_view() << "\"";
- *name = i->rename()->name()->string_view();
- }
- }
-}
-
-} // namespace
-
const char *GetRootDirectory() {
static char* root_dir;// return value
static absl::once_flag once_;
@@ -417,6 +415,9 @@
HandleMaps(config->maps(), &name);
}
+ VLOG(1) << "Acutally looking up { \"name\": \"" << name << "\", \"type\": \""
+ << type << "\" }";
+
// Then look for the channel.
auto channel_iterator =
std::lower_bound(config->channels()->cbegin(),
@@ -435,5 +436,96 @@
}
}
+FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
+ const Flatbuffer<Configuration> &config,
+ const std::vector<aos::FlatbufferString<reflection::Schema>> &schemas) {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Channel>>>
+ channels_offset;
+ if (config.message().has_channels()) {
+ std::vector<flatbuffers::Offset<Channel>> channel_offsets;
+ for (const Channel *c : *config.message().channels()) {
+ flatbuffers::FlatBufferBuilder channel_fbb;
+ channel_fbb.ForceDefaults(1);
+
+ // Search for a schema with a matching type.
+ const aos::FlatbufferString<reflection::Schema> *found_schema = nullptr;
+ for (const aos::FlatbufferString<reflection::Schema> &schema: schemas) {
+ if (schema.message().root_table() != nullptr) {
+ if (schema.message().root_table()->name()->string_view() ==
+ c->type()->string_view()) {
+ found_schema = &schema;
+ }
+ }
+ }
+
+ CHECK(found_schema != nullptr)
+ << ": Failed to find schema for " << FlatbufferToJson(c);
+
+ // The following is wasteful, but works.
+ //
+ // Copy it into a Channel object by creating an object with only the
+ // schema populated and merge that into the current channel.
+ flatbuffers::Offset<reflection::Schema> schema_offset =
+ CopyFlatBuffer<reflection::Schema>(&found_schema->message(),
+ &channel_fbb);
+ Channel::Builder channel_builder(channel_fbb);
+ channel_builder.add_schema(schema_offset);
+ channel_fbb.Finish(channel_builder.Finish());
+ FlatbufferDetachedBuffer<Channel> channel_schema_flatbuffer(
+ channel_fbb.Release());
+
+ FlatbufferDetachedBuffer<Channel> merged_channel(
+ MergeFlatBuffers(channel_schema_flatbuffer, CopyFlatBuffer(c)));
+
+ channel_offsets.emplace_back(
+ CopyFlatBuffer<Channel>(&merged_channel.message(), &fbb));
+ }
+ channels_offset = fbb.CreateVector(channel_offsets);
+ }
+
+ // Copy the applications and maps unmodified.
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Application>>>
+ applications_offset;
+ {
+ ::std::vector<flatbuffers::Offset<Application>> applications_offsets;
+ if (config.message().has_applications()) {
+ for (const Application *a : *config.message().applications()) {
+ applications_offsets.emplace_back(CopyFlatBuffer<Application>(a, &fbb));
+ }
+ }
+ applications_offset = fbb.CreateVector(applications_offsets);
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Map>>>
+ maps_offset;
+ {
+ ::std::vector<flatbuffers::Offset<Map>> map_offsets;
+ if (config.message().has_maps()) {
+ for (const Map *m : *config.message().maps()) {
+ map_offsets.emplace_back(CopyFlatBuffer<Map>(m, &fbb));
+ }
+ maps_offset = fbb.CreateVector(map_offsets);
+ }
+ }
+
+ // Now insert eerything else in unmodified.
+ ConfigurationBuilder configuration_builder(fbb);
+ if (config.message().has_channels()) {
+ configuration_builder.add_channels(channels_offset);
+ }
+ if (config.message().has_maps()) {
+ configuration_builder.add_maps(maps_offset);
+ }
+ if (config.message().has_applications()) {
+ configuration_builder.add_applications(applications_offset);
+ }
+
+ fbb.Finish(configuration_builder.Finish());
+ return fbb.Release();
+}
+
} // namespace configuration
} // namespace aos
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index d23c4b9..a5cb59f 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -1,3 +1,5 @@
+include "reflection/reflection.fbs";
+
namespace aos;
// Table representing a channel. Channels are where data is published and
@@ -12,6 +14,9 @@
// Max size of the data being published. (This will be automatically
// computed in the future.)
max_size:int = 1000;
+
+ // The schema for the data sent on this channel.
+ schema:reflection.Schema;
}
// Table to support renaming channel names.
diff --git a/aos/configuration.h b/aos/configuration.h
index 3cee8e7..e777f41 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -21,6 +21,16 @@
FlatbufferDetachedBuffer<Configuration> ReadConfig(
const absl::string_view path);
+// Sorts and merges entries in a config.
+FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
+ const Flatbuffer<Configuration> &config);
+
+// Adds schema definitions to a sorted and merged config from the provided
+// schema list.
+FlatbufferDetachedBuffer<Configuration> MergeConfiguration(
+ const Flatbuffer<Configuration> &config,
+ const std::vector<aos::FlatbufferString<reflection::Schema>> &schemas);
+
// Returns the resolved location for a name, type, and application name. Returns
// nullptr if none is found.
//
@@ -37,6 +47,8 @@
return GetChannel(&config.message(), name, type, application_name);
}
+// TODO(austin): GetSchema<T>(const Flatbuffer<Configuration> &config);
+
// Returns "our" IP address.
const in_addr &GetOwnIPAddress();
@@ -51,6 +63,13 @@
const char *GetLoggingDirectory();
} // namespace configuration
+
+// Compare and equality operators for Channel. Note: these only check the name
+// and type for equality.
+bool operator<(const FlatbufferDetachedBuffer<Channel> &lhs,
+ const FlatbufferDetachedBuffer<Channel> &rhs);
+bool operator==(const FlatbufferDetachedBuffer<Channel> &lhs,
+ const FlatbufferDetachedBuffer<Channel> &rhs);
} // namespace aos
#endif // AOS_CONFIGURATION_H_
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index 0f13041..e20b308 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -4,6 +4,8 @@
#include "aos/json_to_flatbuffer.h"
#include "aos/testing/test_logging.h"
#include "aos/util/file.h"
+#include "flatbuffers/reflection.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace aos {
@@ -25,7 +27,7 @@
TEST_F(ConfigurationTest, ConfigMerge) {
FlatbufferDetachedBuffer<Configuration> config =
ReadConfig("aos/testdata/config1.json");
- printf("Read: %s\n", FlatbufferToJson(config, true).c_str());
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
EXPECT_EQ(
absl::StripSuffix(
@@ -33,6 +35,19 @@
FlatbufferToJson(config, true));
}
+// Tests that we sort the entries in a config so we can look entries up.
+TEST_F(ConfigurationTest, UnsortedConfig) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/backwards.json");
+
+ LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+
+ EXPECT_EQ(FlatbufferToJson(GetChannel(config, ".aos.robot_state",
+ "aos.RobotState", "app1")),
+ "{ \"name\": \".aos.robot_state\", \"type\": \"aos.RobotState\", "
+ "\"max_size\": 5 }");
+}
+
// Tests that we die when a file is imported twice.
TEST_F(ConfigurationDeathTest, DuplicateFile) {
EXPECT_DEATH(
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index a612ecb..c45a345 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -1,23 +1,9 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
- name = "replay_control_loop",
- hdrs = [
- "replay_control_loop.h",
- ],
- deps = [
- ":control_loop",
- "//aos:queues",
- "//aos/logging:queue_logging",
- "//aos/logging:replay",
- "//aos/time",
- ],
-)
-
-cc_library(
name = "control_loop_test",
testonly = True,
srcs = [
@@ -27,11 +13,13 @@
"control_loop_test.h",
],
deps = [
+ "//aos:flatbuffers",
+ "//aos:json_to_flatbuffer",
"//aos/events:simulated_event_loop",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
+ "//aos/robot_state:robot_state_fbs",
"//aos/testing:googletest",
- "//aos/testing:test_shm",
+ "//aos/testing:test_logging",
"//aos/time",
],
)
@@ -43,7 +31,7 @@
],
restricted_to = mcu_cpus,
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -54,9 +42,9 @@
],
deps = [
"//aos/logging",
- "//aos/logging:matrix_logging",
"//third_party/cddlib",
- "//third_party/eigen",
+ "@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -69,14 +57,14 @@
":polytope",
"//aos/testing:googletest",
"//aos/testing:test_logging",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
-queue_library(
- name = "control_loop_queues",
+flatbuffer_cc_library(
+ name = "control_loop_fbs",
srcs = [
- "control_loops.q",
+ "control_loops.fbs",
],
)
@@ -90,13 +78,11 @@
"control_loop.h",
],
deps = [
- ":control_loop_queues",
- "//aos:queues",
- "//aos/events:event-loop",
- "//aos/events:shm-event-loop",
+ "//aos/events:event_loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//aos/util:log_interval",
],
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 7628c26..f937bb4 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -2,65 +2,63 @@
#include <inttypes.h>
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
namespace aos {
namespace controls {
// TODO(aschuh): Tests.
-template <class T>
-constexpr ::std::chrono::milliseconds ControlLoop<T>::kStaleLogInterval;
-template <class T>
-constexpr ::std::chrono::milliseconds ControlLoop<T>::kPwmDisableTime;
+template <class GoalType, class PositionType, class StatusType,
+ class OutputType>
+constexpr ::std::chrono::milliseconds ControlLoop<
+ GoalType, PositionType, StatusType, OutputType>::kStaleLogInterval;
+template <class GoalType, class PositionType, class StatusType,
+ class OutputType>
+constexpr ::std::chrono::milliseconds ControlLoop<
+ GoalType, PositionType, StatusType, OutputType>::kPwmDisableTime;
-template <class T>
-void ControlLoop<T>::ZeroOutputs() {
- typename ::aos::Sender<OutputType>::Message output =
- output_sender_.MakeMessage();
- Zero(output.get());
- output.Send();
+template <class GoalType, class PositionType, class StatusType,
+ class OutputType>
+void ControlLoop<GoalType, PositionType, StatusType, OutputType>::ZeroOutputs() {
+ typename ::aos::Sender<OutputType>::Builder builder =
+ output_sender_.MakeBuilder();
+ builder.Send(Zero(&builder));
}
-template <class T>
-void ControlLoop<T>::IteratePosition(const PositionType &position) {
+template <class GoalType, class PositionType, class StatusType,
+ class OutputType>
+void ControlLoop<GoalType, PositionType, StatusType,
+ OutputType>::IteratePosition(const PositionType &position) {
no_goal_.Print();
no_sensor_state_.Print();
motors_off_log_.Print();
- AOS_LOG_STRUCT(DEBUG, "position", position);
-
// Fetch the latest control loop goal. If there is no new
// goal, we will just reuse the old one.
goal_fetcher_.Fetch();
const GoalType *goal = goal_fetcher_.get();
- if (goal) {
- AOS_LOG_STRUCT(DEBUG, "goal", *goal);
- } else {
- AOS_LOG_INTERVAL(no_goal_);
- }
const bool new_robot_state = robot_state_fetcher_.Fetch();
if (!robot_state_fetcher_.get()) {
AOS_LOG_INTERVAL(no_sensor_state_);
return;
}
- if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
+ if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid()) {
AOS_LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
- robot_state_fetcher_->reader_pid, sensor_reader_pid_);
+ robot_state_fetcher_->reader_pid(), sensor_reader_pid_);
reset_ = true;
- sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
+ sensor_reader_pid_ = robot_state_fetcher_->reader_pid();
}
- bool outputs_enabled = robot_state_fetcher_->outputs_enabled;
+ bool outputs_enabled = robot_state_fetcher_->outputs_enabled();
// Check to see if we got a driver station packet recently.
if (new_robot_state) {
- if (robot_state_fetcher_->outputs_enabled) {
+ if (robot_state_fetcher_->outputs_enabled()) {
// If the driver's station reports being disabled, we're probably not
// actually going to send motor values regardless of what the FPGA
// reports.
- last_pwm_sent_ = robot_state_fetcher_->sent_time;
+ last_pwm_sent_ = robot_state_fetcher_.context().monotonic_sent_time;
}
}
@@ -69,35 +67,27 @@
const bool motors_off = monotonic_now >= kPwmDisableTime + last_pwm_sent_;
joystick_state_fetcher_.Fetch();
if (motors_off) {
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled()) {
AOS_LOG_INTERVAL(motors_off_log_);
}
outputs_enabled = false;
}
- typename ::aos::Sender<StatusType>::Message status =
- status_sender_.MakeMessage();
- if (status.get() == nullptr) {
- return;
- }
-
+ typename ::aos::Sender<StatusType>::Builder status =
+ status_sender_.MakeBuilder();
if (outputs_enabled) {
- typename ::aos::Sender<OutputType>::Message output =
- output_sender_.MakeMessage();
- RunIteration(goal, &position, output.get(), status.get());
+ typename ::aos::Sender<OutputType>::Builder output =
+ output_sender_.MakeBuilder();
+ RunIteration(goal, &position, &output, &status);
- output->SetTimeToNow();
- AOS_LOG_STRUCT(DEBUG, "output", *output);
- output.Send();
+ output.CheckSent();
} else {
// The outputs are disabled, so pass nullptr in for the output.
- RunIteration(goal, &position, nullptr, status.get());
+ RunIteration(goal, &position, nullptr, &status);
ZeroOutputs();
}
- status->SetTimeToNow();
- AOS_LOG_STRUCT(DEBUG, "status", *status);
- status.Send();
+ status.CheckSent();
}
} // namespace controls
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index fe72022..6f1b487 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -4,10 +4,9 @@
#include <string.h>
#include <atomic>
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
-#include "aos/queue.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
#include "aos/type_traits/type_traits.h"
#include "aos/util/log_interval.h"
@@ -20,39 +19,24 @@
::std::chrono::milliseconds(5);
// Provides helper methods to assist in writing control loops.
-// This template expects to be constructed with a queue group as an argument
-// that has a goal, position, status, and output queue.
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
-template <class T>
+template <class GoalType, class PositionType, class StatusType,
+ class OutputType>
class ControlLoop {
public:
- // Create some convenient typedefs to reference the Goal, Position, Status,
- // and Output structures.
- typedef typename std::remove_reference<decltype(
- *(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type GoalType;
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type OutputType;
-
ControlLoop(EventLoop *event_loop, const ::std::string &name)
: event_loop_(event_loop), name_(name) {
- output_sender_ = event_loop_->MakeSender<OutputType>(name_ + ".output");
- status_sender_ = event_loop_->MakeSender<StatusType>(name_ + ".status");
- goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_ + ".goal");
- robot_state_fetcher_ =
- event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
+ output_sender_ = event_loop_->MakeSender<OutputType>(name_);
+ status_sender_ = event_loop_->MakeSender<StatusType>(name_);
+ goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_);
+ robot_state_fetcher_ = event_loop_->MakeFetcher<::aos::RobotState>("/aos");
joystick_state_fetcher_ =
- event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+ event_loop_->MakeFetcher<::aos::JoystickState>("/aos");
- event_loop_->MakeWatcher(name_ + ".position",
- [this](const PositionType &position) {
- this->IteratePosition(position);
- });
+ event_loop_->MakeWatcher(name_, [this](const PositionType &position) {
+ this->IteratePosition(position);
+ });
}
const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
@@ -80,7 +64,10 @@
// Sets the output to zero.
// Override this if a value of zero (or false) is not "off" for this
// subsystem.
- virtual void Zero(OutputType *output) { output->Zero(); }
+ virtual flatbuffers::Offset<OutputType> Zero(
+ typename ::aos::Sender<OutputType>::Builder *builder) {
+ return builder->template MakeBuilder<OutputType>().Finish();
+ }
protected:
// Runs one cycle of the loop.
@@ -88,6 +75,10 @@
EventLoop *event_loop() { return event_loop_; }
+ // Returns the position context. This is only valid inside the RunIteration
+ // method.
+ const aos::Context &position_context() { return event_loop_->context(); }
+
// Runs an iteration of the control loop.
// goal is the last goal that was sent. It might be any number of cycles old
// or nullptr if we haven't ever received a goal.
@@ -97,8 +88,10 @@
// output is going to be ignored and set to 0.
// status is the status of the control loop.
// Both output and status should be filled in by the implementation.
- virtual void RunIteration(const GoalType *goal, const PositionType *position,
- OutputType *output, StatusType *status) = 0;
+ virtual void RunIteration(
+ const GoalType *goal, const PositionType *position,
+ typename ::aos::Sender<OutputType>::Builder *output,
+ typename ::aos::Sender<StatusType>::Builder *status) = 0;
private:
static constexpr ::std::chrono::milliseconds kStaleLogInterval =
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index ff11987..b8ebc67 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -5,10 +5,12 @@
#include "gtest/gtest.h"
-#include "aos/events/simulated-event-loop.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
-#include "aos/testing/test_shm.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
namespace aos {
@@ -22,14 +24,28 @@
template <typename TestBaseClass>
class ControlLoopTestTemplated : public TestBaseClass {
public:
- ControlLoopTestTemplated(::std::chrono::nanoseconds dt = kTimeTick)
- : dt_(dt), robot_status_event_loop_(MakeEventLoop()) {
+ // Builds a control loop test with the provided configuration. Note: this
+ // merges and sorts the config to reduce user errors.
+ ControlLoopTestTemplated(absl::string_view configuration,
+ ::std::chrono::nanoseconds dt = kTimeTick)
+ : ControlLoopTestTemplated(
+ configuration::MergeConfiguration(
+ aos::FlatbufferDetachedBuffer<Configuration>(JsonToFlatbuffer(
+ configuration, Configuration::MiniReflectTypeTable()))),
+ dt) {}
+
+ ControlLoopTestTemplated(
+ FlatbufferDetachedBuffer<Configuration> configuration,
+ ::std::chrono::nanoseconds dt = kTimeTick)
+ : configuration_(std::move(configuration)),
+ event_loop_factory_(&configuration_.message()),
+ dt_(dt),
+ robot_status_event_loop_(MakeEventLoop()) {
+ testing::EnableTestLogging();
robot_state_sender_ =
- robot_status_event_loop_->MakeSender<::aos::RobotState>(
- ".aos.robot_state");
+ robot_status_event_loop_->MakeSender<::aos::RobotState>("/aos");
joystick_state_sender_ =
- robot_status_event_loop_->MakeSender<::aos::JoystickState>(
- ".aos.joystick_state");
+ robot_status_event_loop_->MakeSender<::aos::JoystickState>("/aos");
// Schedule the robot status send 1 nanosecond before the loop runs.
send_robot_state_phased_loop_ = robot_status_event_loop_->AddPhasedLoop(
@@ -91,15 +107,17 @@
void SendJoystickState() {
if (monotonic_now() >= kDSPacketTime + last_ds_time_ ||
last_enabled_ != enabled_) {
- auto new_state = joystick_state_sender_.MakeMessage();
- new_state->fake = true;
+ auto new_state = joystick_state_sender_.MakeBuilder();
+ ::aos::JoystickState::Builder builder =
+ new_state.template MakeBuilder<::aos::JoystickState>();
- new_state->enabled = enabled_;
- new_state->autonomous = false;
- new_state->team_id = team_id_;
+ builder.add_fake(true);
- AOS_LOG_STRUCT(INFO, "joystick_state", *new_state);
- new_state.Send();
+ builder.add_enabled(enabled_);
+ builder.add_autonomous(false);
+ builder.add_team_id(team_id_);
+
+ new_state.Send(builder.Finish());
last_ds_time_ = monotonic_now();
last_enabled_ = enabled_;
@@ -109,31 +127,35 @@
bool last_enabled_ = false;
void SendRobotState() {
- auto new_state = robot_state_sender_.MakeMessage();
+ auto new_state = robot_state_sender_.MakeBuilder();
- new_state->reader_pid = reader_pid_;
- new_state->outputs_enabled = enabled_;
- new_state->browned_out = false;
+ ::aos::RobotState::Builder builder =
+ new_state.template MakeBuilder<::aos::RobotState>();
- new_state->is_3v3_active = true;
- new_state->is_5v_active = true;
- new_state->voltage_3v3 = 3.3;
- new_state->voltage_5v = 5.0;
+ builder.add_reader_pid(reader_pid_);
+ builder.add_outputs_enabled(enabled_);
+ builder.add_browned_out(false);
- new_state->voltage_roborio_in = battery_voltage_;
- new_state->voltage_battery = battery_voltage_;
+ builder.add_is_3v3_active(true);
+ builder.add_is_5v_active(true);
+ builder.add_voltage_3v3(3.3);
+ builder.add_voltage_5v(5.0);
- AOS_LOG_STRUCT(INFO, "robot_state", *new_state);
- new_state.Send();
+ builder.add_voltage_roborio_in(battery_voltage_);
+ builder.add_voltage_battery(battery_voltage_);
+
+ new_state.Send(builder.Finish());
}
static constexpr ::std::chrono::microseconds kTimeTick{5000};
static constexpr ::std::chrono::milliseconds kDSPacketTime{20};
- const ::std::chrono::nanoseconds dt_;
+ FlatbufferDetachedBuffer<Configuration> configuration_;
SimulatedEventLoopFactory event_loop_factory_;
+ const ::std::chrono::nanoseconds dt_;
+
uint16_t team_id_ = 971;
int32_t reader_pid_ = 1;
double battery_voltage_ = 12.4;
diff --git a/aos/controls/control_loops.fbs b/aos/controls/control_loops.fbs
new file mode 100644
index 0000000..8456bfa
--- /dev/null
+++ b/aos/controls/control_loops.fbs
@@ -0,0 +1,24 @@
+namespace aos.control_loops;
+// This file defines basic messages for a Single Input Single Output (SISO)
+// control loop.
+
+table Goal {
+ goal:double;
+}
+
+table Position {
+ position:double;
+}
+
+table Output {
+ voltage:double;
+}
+
+table Status {
+ done:bool;
+}
+
+root_type Goal;
+root_type Position;
+root_type Output;
+root_type Status;
diff --git a/aos/controls/control_loops.q b/aos/controls/control_loops.q
deleted file mode 100644
index 2b0bfb1..0000000
--- a/aos/controls/control_loops.q
+++ /dev/null
@@ -1,34 +0,0 @@
-package aos.control_loops;
-
-interface ControlLoop {
- queue goal;
- queue position;
- queue output;
- queue status;
-};
-
-message Goal {
- double goal;
-};
-
-message Position {
- double position;
-};
-
-message Output {
- double voltage;
-};
-
-message Status {
- bool done;
-};
-
-// Single Input Single Output control loop.
-queue_group SISO {
- implements ControlLoop;
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
index 5248ea3..c63166f 100644
--- a/aos/controls/polytope.h
+++ b/aos/controls/polytope.h
@@ -7,8 +7,7 @@
#include "third_party/cddlib/lib-src/setoper.h"
#include "third_party/cddlib/lib-src/cdd.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/matrix_logging.h"
+#include "glog/logging.h"
#endif // __linux__
namespace aos {
@@ -191,9 +190,9 @@
if (error != dd_NoError || polyhedra == NULL) {
dd_WriteErrorMessages(stderr, error);
dd_FreeMatrix(matrix);
- AOS_LOG_MATRIX(ERROR, "bad H", H);
- AOS_LOG_MATRIX(ERROR, "bad k_", k);
- AOS_LOG(FATAL, "dd_DDMatrix2Poly failed\n");
+ LOG(ERROR) << "bad H" << H;
+ LOG(ERROR) << "bad k_" << k;
+ LOG(FATAL) << "dd_DDMatrix2Poly failed";
}
dd_MatrixPtr vertex_matrix = dd_CopyGenerators(polyhedra);
@@ -209,7 +208,7 @@
}
// Rays are unsupported right now. This may change in the future.
- AOS_CHECK_EQ(0, num_rays);
+ CHECK_EQ(0, num_rays);
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices(
number_of_dimensions, num_vertices);
diff --git a/aos/controls/replay_control_loop.h b/aos/controls/replay_control_loop.h
deleted file mode 100644
index 23593ab..0000000
--- a/aos/controls/replay_control_loop.h
+++ /dev/null
@@ -1,195 +0,0 @@
-#ifndef AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_
-#define AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_
-
-#include <fcntl.h>
-
-#include "aos/queue.h"
-#include "aos/controls/control_loop.h"
-#include "aos/logging/replay.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/time/time.h"
-#include "aos/macros.h"
-
-namespace aos {
-namespace controls {
-
-// Handles reading logged messages and running them through a control loop
-// again.
-// T should be a queue group suitable for use with ControlLoop.
-template <class T>
-class ControlLoopReplayer {
- public:
- typedef typename ControlLoop<T>::GoalType GoalType;
- typedef typename ControlLoop<T>::PositionType PositionType;
- typedef typename ControlLoop<T>::StatusType StatusType;
- typedef typename ControlLoop<T>::OutputType OutputType;
-
- // loop_group is where to send the messages out.
- // process_name is the name of the process which wrote the log messages in the
- // file(s).
- ControlLoopReplayer(T *loop_group, const ::std::string &process_name)
- : loop_group_(loop_group) {
- // Clear out any old messages which will confuse the code.
- loop_group_->status.FetchLatest();
- loop_group_->output.FetchLatest();
-
- AddDirectQueueSender<::aos::JoystickState>(
- "wpilib_interface.DSReader", "joystick_state", ".aos.joystick_state");
- AddDirectQueueSender<::aos::RobotState>("wpilib_interface.SensorReader",
- "robot_state", ".aos.robot_state");
-
- replayer_.AddHandler(
- process_name, "position",
- ::std::function<void(const PositionType &)>(::std::ref(position_)));
- replayer_.AddHandler(
- process_name, "output",
- ::std::function<void(const OutputType &)>(::std::ref(output_)));
- replayer_.AddHandler(
- process_name, "status",
- ::std::function<void(const StatusType &)>(::std::ref(status_)));
- // The timing of goal messages doesn't matter, and we don't need to look
- // back at them after running the loop.
- AddDirectQueueSender<GoalType>(
- process_name, "goal", ::std::string(loop_group_->name()) + ".goal");
- }
-
- template <class QT>
- void AddDirectQueueSender(const ::std::string &process_name,
- const ::std::string &log_message,
- const ::std::string &name) {
- replayer_.AddDirectQueueSender<QT>(process_name, log_message, name);
- }
-
- // Replays messages from a file.
- // filename can be straight from the command line; all sanity checking etc is
- // handled by this function.
- void ProcessFile(const char *filename);
-
- private:
- // A message handler which saves off messages and records whether it currently
- // has a new one or not.
- template <class S>
- class CaptureMessage {
- public:
- CaptureMessage() {}
-
- void operator()(const S &message) {
- AOS_CHECK(!have_new_message_);
- saved_message_ = message;
- have_new_message_ = true;
- }
-
- const S &saved_message() const { return saved_message_; }
- bool have_new_message() const { return have_new_message_; }
- void clear_new_message() { have_new_message_ = false; }
-
- private:
- S saved_message_;
- bool have_new_message_ = false;
-
- DISALLOW_COPY_AND_ASSIGN(CaptureMessage);
- };
-
- // Runs through the file currently loaded in replayer_.
- // Returns after going through the entire file.
- void DoProcessFile();
-
- ::aos::ShmEventLoop event_loop_;
-
- T *const loop_group_;
-
- CaptureMessage<PositionType> position_;
- CaptureMessage<OutputType> output_;
- CaptureMessage<StatusType> status_;
-
- // The output that the loop sends for ZeroOutputs(). It might not actually be
- // all fields zeroed, so we pick the first one and remember it to compare.
- CaptureMessage<OutputType> zero_output_;
-
- ::aos::logging::linux_code::LogReplayer replayer_;
-};
-
-template <class T>
-void ControlLoopReplayer<T>::ProcessFile(const char *filename) {
- int fd;
- if (strcmp(filename, "-") == 0) {
- fd = STDIN_FILENO;
- } else {
- fd = open(filename, O_RDONLY);
- }
- if (fd == -1) {
- AOS_PLOG(FATAL, "couldn't open file '%s' for reading", filename);
- }
-
- replayer_.OpenFile(fd);
- DoProcessFile();
- replayer_.CloseCurrentFile();
-
- AOS_PCHECK(close(fd));
-}
-
-template <class T>
-void ControlLoopReplayer<T>::DoProcessFile() {
- while (true) {
- // Dig through messages until we get a status, which indicates the end of
- // the control loop cycle.
- while (!status_.have_new_message()) {
- if (replayer_.ProcessMessage()) return;
- }
-
- // Send out the position message (after adjusting the time offset) so the
- // loop will run right now.
- if (!position_.have_new_message()) {
- AOS_LOG(WARNING, "don't have a new position this cycle -> skipping\n");
- status_.clear_new_message();
- position_.clear_new_message();
- output_.clear_new_message();
- continue;
- }
- ::aos::time::OffsetToNow(position_.saved_message().sent_time);
- {
- auto position_message = loop_group_->position.MakeMessage();
- *position_message = position_.saved_message();
- AOS_CHECK(position_message.Send());
- }
- position_.clear_new_message();
-
- // Wait for the loop to finish running.
- loop_group_->status.FetchNextBlocking();
-
- // Point out if the status is different.
- if (!loop_group_->status->EqualsNoTime(status_.saved_message())) {
- AOS_LOG_STRUCT(WARNING, "expected status", status_.saved_message());
- AOS_LOG_STRUCT(WARNING, "got status", *loop_group_->status);
- }
- status_.clear_new_message();
-
- // Point out if the output is different. This is a lot more complicated than
- // for the status because there isn't always an output logged.
- bool loop_new_output = loop_group_->output.FetchLatest();
- if (output_.have_new_message()) {
- if (!loop_new_output) {
- AOS_LOG_STRUCT(WARNING, "no output, expected", output_.saved_message());
- } else if (!loop_group_->output->EqualsNoTime(output_.saved_message())) {
- AOS_LOG_STRUCT(WARNING, "expected output", output_.saved_message());
- AOS_LOG_STRUCT(WARNING, "got output", *loop_group_->output);
- }
- } else if (loop_new_output) {
- if (zero_output_.have_new_message()) {
- if (!loop_group_->output->EqualsNoTime(zero_output_.saved_message())) {
- AOS_LOG_STRUCT(WARNING, "expected null output",
- zero_output_.saved_message());
- AOS_LOG_STRUCT(WARNING, "got output", *loop_group_->output);
- }
- } else {
- zero_output_(*loop_group_->output);
- }
- }
- output_.clear_new_message();
- }
-}
-
-} // namespace controls
-} // namespace aos
-
-#endif // AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_
diff --git a/aos/downloader/downloader.bzl b/aos/downloader/downloader.bzl
deleted file mode 100644
index 139401a..0000000
--- a/aos/downloader/downloader.bzl
+++ /dev/null
@@ -1,116 +0,0 @@
-def _aos_downloader_impl(ctx):
- all_files = ctx.files.srcs + ctx.files.start_srcs + [ctx.outputs._startlist]
- ctx.file_action(
- output = ctx.outputs.executable,
- executable = True,
- content = '\n'.join([
- '#!/bin/bash',
- 'set -e',
- 'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
- ] + ['%s %s --dirs %s -- %s "$@"' % (
- ctx.executable._downloader.short_path,
- ' '.join([src.short_path for src in d.downloader_srcs]),
- d.downloader_dir,
- ctx.attr.default_target) for d in ctx.attr.dirs] + [
- 'exec %s %s -- %s "$@"' % (ctx.executable._downloader.short_path,
- ' '.join([src.short_path for src in all_files]),
- ctx.attr.default_target),
- ]),
- )
-
- ctx.file_action(
- output = ctx.outputs._startlist,
- content = '\n'.join([f.basename for f in ctx.files.start_srcs]) + '\n',
- )
-
- to_download = [ctx.outputs._startlist]
- to_download += all_files
- for d in ctx.attr.dirs:
- to_download += d.downloader_srcs
-
- return struct(
- runfiles = ctx.runfiles(
- files = to_download + ctx.files._downloader,
- transitive_files = ctx.attr._downloader.default_runfiles.files,
- collect_data = True,
- collect_default = True,
- ),
- files = depset([ctx.outputs.executable]),
- )
-
-def _aos_downloader_dir_impl(ctx):
- return struct(
- downloader_dir = ctx.attr.dir,
- downloader_srcs = ctx.files.srcs
- )
-
-"""Creates a binary which downloads code to a robot.
-
-Running this with `bazel run` will actually download everything.
-
-This also generates a start_list.txt file with the names of binaries to start.
-
-Attrs:
- srcs: The files to download. They currently all get shoved into one folder.
- dirs: A list of aos_downloader_dirs to download too.
- start_srcs: Like srcs, except they also get put into start_list.txt.
- default_target: The default host to download to. If not specified, defaults to
- roboRIO-971.local.
-"""
-
-aos_downloader = rule(
- attrs = {
- "_downloader": attr.label(
- executable = True,
- cfg = "host",
- default = Label("//aos/downloader"),
- ),
- "start_srcs": attr.label_list(
- mandatory = True,
- allow_files = True,
- ),
- "srcs": attr.label_list(
- mandatory = True,
- allow_files = True,
- ),
- "dirs": attr.label_list(
- mandatory = False,
- providers = [
- "downloader_dir",
- "downloader_srcs",
- ],
- ),
- "default_target": attr.string(
- default = "roboRIO-971-frc.local",
- ),
- },
- executable = True,
- outputs = {
- "_startlist": "%{name}.start_list.dir/start_list.txt",
- },
- implementation = _aos_downloader_impl,
-)
-
-"""Downloads files to a specific directory.
-
-This rule does nothing by itself. Use it by adding to the dirs attribute of an
-aos_downloader rule.
-
-Attrs:
- srcs: The files to download. They all go in the same directory.
- dir: The directory (relative to the standard download directory) to put all
- the files in.
-"""
-
-aos_downloader_dir = rule(
- attrs = {
- "srcs": attr.label_list(
- mandatory = True,
- allow_files = True,
- ),
- "dir": attr.string(
- mandatory = True,
- ),
- },
- implementation = _aos_downloader_dir_impl,
-)
diff --git a/aos/event.cc b/aos/event.cc
index a0115dc..1b60362 100644
--- a/aos/event.cc
+++ b/aos/event.cc
@@ -3,7 +3,7 @@
#include <chrono>
#include "aos/type_traits/type_traits.h"
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
@@ -16,8 +16,8 @@
while (__atomic_load_n(&impl_, __ATOMIC_SEQ_CST) == 0) {
const int ret = futex_wait(&impl_);
if (ret != 0) {
- AOS_CHECK_EQ(-1, ret);
- AOS_PLOG(FATAL, "futex_wait(%p) failed", &impl_);
+ CHECK_EQ(-1, ret);
+ PLOG(FATAL) << "futex_wait(" << &impl_ << ") failed";
}
}
}
@@ -37,8 +37,8 @@
const int ret = futex_wait_timeout(&impl_, &timeout_timespec);
if (ret != 0) {
if (ret == 2) return false;
- AOS_CHECK_EQ(-1, ret);
- AOS_PLOG(FATAL, "futex_wait(%p) failed", &impl_);
+ CHECK_EQ(-1, ret);
+ PLOG(FATAL) << "futex_wait(" << &impl_ << ") failed";
}
}
}
@@ -47,7 +47,7 @@
// to condition variable-based implementations.
void Event::Set() {
if (futex_set(&impl_) == -1) {
- AOS_PLOG(FATAL, "futex_set(%p) failed", &impl_);
+ PLOG(FATAL) << "futex_set(" << &impl_ << ") failed";
}
}
diff --git a/aos/events/BUILD b/aos/events/BUILD
index b69fc66..17ed688 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -1,3 +1,19 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+package(default_visibility = ["//visibility:public"])
+
+flatbuffer_cc_library(
+ name = "test_message_fbs",
+ srcs = ["test_message.fbs"],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "pingpong_fbs",
+ srcs = ["pingpong.fbs"],
+ gen_reflections = 1,
+)
+
cc_library(
name = "epoll",
srcs = ["epoll.cc"],
@@ -10,72 +26,97 @@
)
cc_library(
- name = "raw-event-loop",
- hdrs = ["raw-event-loop.h"],
- deps = [
- "//aos:queues",
- "//aos/time",
- ],
-)
-
-cc_library(
- name = "event-loop",
- srcs = ["event-loop-tmpl.h"],
+ name = "event_loop",
+ srcs = ["event_loop_tmpl.h"],
hdrs = [
- "event-loop.h",
- "raw-event-loop.h",
+ "event_loop.h",
],
visibility = ["//visibility:public"],
deps = [
- ":raw-event-loop",
- "//aos:queues",
+ "//aos:configuration",
+ "//aos:configuration_fbs",
+ "//aos:flatbuffers",
"//aos/time",
+ "@com_github_google_flatbuffers//:flatbuffers",
+ ],
+)
+
+cc_binary(
+ name = "ping",
+ srcs = [
+ "ping.cc",
+ ],
+ data = ["config.fb.json"],
+ deps = [
+ ":pingpong_fbs",
+ ":shm_event_loop",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_binary(
+ name = "pong",
+ srcs = [
+ "pong.cc",
+ ],
+ data = ["config.fb.json"],
+ deps = [
+ ":pingpong_fbs",
+ ":shm_event_loop",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "@com_github_google_glog//:glog",
],
)
cc_library(
- name = "shm-event-loop",
- srcs = ["shm-event-loop.cc"],
- hdrs = ["shm-event-loop.h"],
+ name = "shm_event_loop",
+ srcs = ["shm_event_loop.cc"],
+ hdrs = ["shm_event_loop.h"],
visibility = ["//visibility:public"],
deps = [
":epoll",
- ":event-loop",
- "//aos:init",
- "//aos:queues",
- "//aos/logging",
+ ":event_loop",
+ ":test_message_fbs",
+ "//aos:realtime",
+ "//aos/ipc_lib:lockless_queue",
+ "//aos/ipc_lib:signalfd",
"//aos/util:phased_loop",
],
)
cc_test(
- name = "shm-event-loop_test",
- srcs = ["shm-event-loop_test.cc"],
+ name = "shm_event_loop_test",
+ srcs = ["shm_event_loop_test.cc"],
shard_count = 5,
deps = [
- ":event-loop_param_test",
- ":shm-event-loop",
- "//aos/testing:test_shm",
+ ":event_loop_param_test",
+ ":shm_event_loop",
+ ":test_message_fbs",
],
)
cc_library(
- name = "event-loop_param_test",
+ name = "event_loop_param_test",
testonly = True,
- srcs = ["event-loop_param_test.cc"],
- hdrs = ["event-loop_param_test.h"],
+ srcs = ["event_loop_param_test.cc"],
+ hdrs = ["event_loop_param_test.h"],
deps = [
- ":event-loop",
- "//aos/logging:queue_logging",
+ ":event_loop",
+ ":test_message_fbs",
"//aos/testing:googletest",
],
)
cc_test(
name = "simulated_event_loop_test",
- srcs = ["simulated-event-loop_test.cc"],
+ srcs = ["simulated_event_loop_test.cc"],
deps = [
- ":event-loop_param_test",
+ ":event_loop_param_test",
":simulated_event_loop",
"//aos/testing:googletest",
],
@@ -84,14 +125,19 @@
cc_library(
name = "simulated_event_loop",
testonly = True,
- srcs = ["simulated-event-loop.cc"],
- hdrs = ["simulated-event-loop.h"],
+ srcs = [
+ "event_scheduler.cc",
+ "simulated_event_loop.cc",
+ ],
+ hdrs = [
+ "event_scheduler.h",
+ "simulated_event_loop.h",
+ ],
visibility = ["//visibility:public"],
deps = [
- ":event-loop",
- "//aos:queues",
- "//aos/logging",
- "//aos/testing:test_logging",
+ ":event_loop",
+ "//aos/ipc_lib:index",
"//aos/util:phased_loop",
+ "@com_google_absl//absl/container:btree",
],
)
diff --git a/aos/events/config.fb.json b/aos/events/config.fb.json
new file mode 100644
index 0000000..4068243
--- /dev/null
+++ b/aos/events/config.fb.json
@@ -0,0 +1,12 @@
+{
+ "channels": [
+ {
+ "name": "/test",
+ "type": "aos.examples.Ping"
+ },
+ {
+ "name": "/test",
+ "type": "aos.examples.Pong"
+ }
+ ]
+}
diff --git a/aos/events/epoll.h b/aos/events/epoll.h
index f6ebe95..0f335e5 100644
--- a/aos/events/epoll.h
+++ b/aos/events/epoll.h
@@ -6,6 +6,7 @@
#include <sys/timerfd.h>
#include <unistd.h>
#include <atomic>
+#include <functional>
#include <vector>
#include "aos/time/time.h"
diff --git a/aos/events/event-loop-tmpl.h b/aos/events/event-loop-tmpl.h
deleted file mode 100644
index 4c53be3..0000000
--- a/aos/events/event-loop-tmpl.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef _AOS_EVENTS_EVENT_LOOP_TMPL_H_
-#define _AOS_EVENTS_EVENT_LOOP_TMPL_H_
-
-#include <type_traits>
-#include "aos/events/event-loop.h"
-
-namespace aos {
-
-// From a watch functor, this will extract the message type of the argument.
-// This is the template forward declaration, and it extracts the call operator
-// as a PTMF to be used by the following specialization.
-template <class T>
-struct watch_message_type_trait
- : watch_message_type_trait<decltype(&T::operator())> {};
-
-// From a watch functor, this will extract the message type of the argument.
-// This is the template specialization.
-template <class ClassType, class ReturnType, class A1>
-struct watch_message_type_trait<ReturnType (ClassType::*)(A1) const> {
- using message_type = typename std::decay<A1>::type;
-};
-
-template <typename T>
-typename Sender<T>::Message Sender<T>::MakeMessage() {
- return Message(sender_.get());
-}
-
-template <typename Watch>
-void EventLoop::MakeWatcher(const std::string &path, Watch &&w) {
- using T = typename watch_message_type_trait<Watch>::message_type;
-
- return MakeRawWatcher(path, QueueTypeInfo::Get<T>(),
- [w](const Message *message) {
- w(*reinterpret_cast<const T *>(message));
- });
-}
-
-} // namespace aos
-
-#endif // _AOS_EVENTS_EVENT_LOOP_TMPL_H
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
deleted file mode 100644
index 4d6fda5..0000000
--- a/aos/events/event-loop.h
+++ /dev/null
@@ -1,143 +0,0 @@
-#ifndef _AOS_EVENTS_EVENT_LOOP_H_
-#define _AOS_EVENTS_EVENT_LOOP_H_
-
-#include <string>
-#include "aos/queue.h"
-#include "aos/time/time.h"
-#include "aos/events/raw-event-loop.h"
-
-namespace aos {
-
-// Fetches the newest message from a queue.
-template <typename T>
-class Fetcher {
- public:
- Fetcher() {}
- // Fetches the next message. Returns true if it fetched a new message. This
- // method will only return messages sent after the Fetcher was created.
- bool FetchNext() { return fetcher_->FetchNext(); }
- // Fetches the most recent message. Returns true if it fetched a new message.
- // This will return the latest message regardless of if it was sent before or
- // after the fetcher was created.
- bool Fetch() { return fetcher_->Fetch(); }
-
- const T *get() const {
- return reinterpret_cast<const T *>(fetcher_->most_recent());
- }
- const T &operator*() const { return *get(); }
- const T *operator->() const { return get(); }
-
- private:
- friend class EventLoop;
- Fetcher(::std::unique_ptr<RawFetcher> fetcher) : fetcher_(::std::move(fetcher)) {}
- ::std::unique_ptr<RawFetcher> fetcher_;
-};
-
-// Sends messages to a queue.
-template <typename T>
-class Sender {
- public:
- typedef T Type;
-
- Sender() {}
-
- // Represents a single message about to be sent to the queue.
- // The lifecycle goes:
- //
- // Message msg = sender.MakeMessage();
- // Populate(msg.get());
- // msg.Send();
- //
- // Or:
- //
- // Message msg = sender.MakeMessage();
- // PopulateOrNot(msg.get());
- class Message {
- public:
- Message(RawSender *sender)
- : msg_(reinterpret_cast<T *>(sender->GetMessage()), *sender) {
- msg_->Zero();
- }
-
- T *get() { return msg_.get(); }
- const T *get() const { return msg_.get(); }
- T &operator*() { return *get(); }
- T *operator->() { return get(); }
- const T &operator*() const { return *get(); }
- const T *operator->() const { return get(); }
-
- // Sends the message to the queue. Should only be called once. Returns true
- // if the message was successfully sent, and false otherwise.
- bool Send() {
- RawSender *sender = &msg_.get_deleter();
- return sender->Send(msg_.release());
- }
-
- private:
- std::unique_ptr<T, RawSender &> msg_;
- };
-
- // Constructs an above message.
- Message MakeMessage();
-
- // Returns the name of the underlying queue.
- const char *name() const { return sender_->name(); }
-
- private:
- friend class EventLoop;
- Sender(::std::unique_ptr<RawSender> sender) : sender_(::std::move(sender)) {}
- ::std::unique_ptr<RawSender> sender_;
-};
-
-// TODO(parker): Consider making EventLoop wrap a RawEventLoop rather than
-// inheriting.
-class EventLoop : public RawEventLoop {
- public:
- virtual ~EventLoop() {}
-
- // Current time.
- virtual monotonic_clock::time_point monotonic_now() = 0;
-
- // Note, it is supported to create:
- // multiple fetchers, and (one sender or one watcher) per <path, type>
- // tuple.
-
- // Makes a class that will always fetch the most recent value
- // sent to path.
- template <typename T>
- Fetcher<T> MakeFetcher(const ::std::string &path) {
- return Fetcher<T>(MakeRawFetcher(path, QueueTypeInfo::Get<T>()));
- }
-
- // Makes class that allows constructing and sending messages to
- // address path.
- template <typename T>
- Sender<T> MakeSender(const ::std::string &path) {
- return Sender<T>(MakeRawSender(path, QueueTypeInfo::Get<T>()));
- }
-
- // Watch is a functor that have a call signature like so:
- // void Event(const MessageType& type);
- //
- // This will watch messages sent to path.
- // Note that T needs to match both send and recv side.
- // TODO(parker): Need to support ::std::bind. For now, use lambdas.
- template <typename Watch>
- void MakeWatcher(const ::std::string &path, Watch &&w);
-
- // The passed in function will be called when the event loop starts.
- // Use this to run code once the thread goes into "real-time-mode",
- virtual void OnRun(::std::function<void()>) = 0;
-
- // TODO(austin): OnExit
-
- // Sets the scheduler priority to run the event loop at. This may not be
- // called after we go into "real-time-mode".
- virtual void SetRuntimeRealtimePriority(int priority) = 0;
-};
-
-} // namespace aos
-
-#include "aos/events/event-loop-tmpl.h"
-
-#endif // _AOS_EVENTS_EVENT_LOOP_H
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
new file mode 100644
index 0000000..f614517
--- /dev/null
+++ b/aos/events/event_loop.h
@@ -0,0 +1,328 @@
+#ifndef AOS_EVENTS_EVENT_LOOP_H_
+#define AOS_EVENTS_EVENT_LOOP_H_
+
+#include <atomic>
+#include <string>
+
+#include "absl/strings/string_view.h"
+#include "aos/configuration.h"
+#include "aos/configuration_generated.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
+#include "flatbuffers/flatbuffers.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+// Struct available on Watchers and Fetchers with context about the current
+// message.
+struct Context {
+ // Time that the message was sent.
+ monotonic_clock::time_point monotonic_sent_time;
+ realtime_clock::time_point realtime_sent_time;
+ // Index in the queue.
+ uint32_t queue_index;
+ // Size of the data sent.
+ size_t size;
+ // Pointer to the data.
+ void *data;
+};
+
+// Raw version of fetcher. Contains a local variable that the fetcher will
+// update. This is used for reflection and as an interface to implement typed
+// fetchers.
+class RawFetcher {
+ public:
+ RawFetcher() {}
+ virtual ~RawFetcher() {}
+
+ // Non-blocking fetch of the next message in the queue. Returns true if there
+ // was a new message and we got it.
+ virtual bool FetchNext() = 0;
+
+ // Non-blocking fetch of the latest message:
+ virtual bool Fetch() = 0;
+
+ // Returns a pointer to data in the most recent message, or nullptr if there
+ // is no message.
+ const void *most_recent_data() const { return data_; }
+
+ const Context &context() const { return context_; }
+
+ protected:
+ RawFetcher(const RawFetcher &) = delete;
+ RawFetcher &operator=(const RawFetcher &) = delete;
+
+ void *data_ = nullptr;
+ Context context_;
+};
+
+// Raw version of sender. Sends a block of data. This is used for reflection
+// and as a building block to implement typed senders.
+class RawSender {
+ public:
+ RawSender() {}
+ virtual ~RawSender() {}
+
+ // Sends a message without copying it. The users starts by copying up to
+ // size() bytes into the data backed by data(). They then call Send to send.
+ // Returns true on a successful send.
+ virtual void *data() = 0;
+ virtual size_t size() = 0;
+ virtual bool Send(size_t size) = 0;
+
+ // Sends a single block of data by copying it.
+ virtual bool Send(void *data, size_t size) = 0;
+
+ // Returns the name of this sender.
+ virtual const absl::string_view name() const = 0;
+
+ protected:
+ RawSender(const RawSender &) = delete;
+ RawSender &operator=(const RawSender &) = delete;
+};
+
+
+// Fetches the newest message from a channel.
+// This provides a polling based interface for channels.
+template <typename T>
+class Fetcher {
+ public:
+ Fetcher() {}
+
+ // Fetches the next message. Returns true if it fetched a new message. This
+ // method will only return messages sent after the Fetcher was created.
+ bool FetchNext() { return fetcher_->FetchNext(); }
+
+ // Fetches the most recent message. Returns true if it fetched a new message.
+ // This will return the latest message regardless of if it was sent before or
+ // after the fetcher was created.
+ bool Fetch() { return fetcher_->Fetch(); }
+
+ // Returns a pointer to the contained flatbuffer, or nullptr if there is no
+ // available message.
+ const T *get() const {
+ return fetcher_->most_recent_data() != nullptr
+ ? flatbuffers::GetRoot<T>(reinterpret_cast<const char *>(
+ fetcher_->most_recent_data()))
+ : nullptr;
+ }
+
+ // Returns the context holding timestamps and other metadata about the
+ // message.
+ const Context &context() const { return fetcher_->context(); }
+
+ const T &operator*() const { return *get(); }
+ const T *operator->() const { return get(); }
+
+ private:
+ friend class EventLoop;
+ Fetcher(::std::unique_ptr<RawFetcher> fetcher)
+ : fetcher_(::std::move(fetcher)) {}
+ ::std::unique_ptr<RawFetcher> fetcher_;
+};
+
+// Sends messages to a channel.
+template <typename T>
+class Sender {
+ public:
+ Sender() {}
+
+ // Represents a single message about to be sent to the queue.
+ // The lifecycle goes:
+ //
+ // Builder builder = sender.MakeBuilder();
+ // T::Builder t_builder = builder.MakeBuilder<T>();
+ // Populate(&t_builder);
+ // builder.Send(t_builder.Finish());
+ class Builder {
+ public:
+ Builder(RawSender *sender, void *data, size_t size)
+ : alloc_(data, size), fbb_(size, &alloc_), sender_(sender) {
+ fbb_.ForceDefaults(1);
+ }
+
+ flatbuffers::FlatBufferBuilder *fbb() { return &fbb_; }
+
+ template <typename T2>
+ typename T2::Builder MakeBuilder() {
+ return typename T2::Builder(fbb_);
+ }
+
+ bool Send(flatbuffers::Offset<T> offset) {
+ fbb_.Finish(offset);
+ return sender_->Send(fbb_.GetSize());
+ }
+
+ // CHECKs that this message was sent.
+ void CheckSent() { fbb_.Finished(); }
+
+ private:
+ PreallocatedAllocator alloc_;
+ flatbuffers::FlatBufferBuilder fbb_;
+ RawSender *sender_;
+ };
+
+ // Constructs an above builder.
+ Builder MakeBuilder();
+
+ // Returns the name of the underlying queue.
+ const absl::string_view name() const { return sender_->name(); }
+
+ private:
+ friend class EventLoop;
+ Sender(std::unique_ptr<RawSender> sender) : sender_(std::move(sender)) {}
+ std::unique_ptr<RawSender> sender_;
+};
+
+// Interface for timers
+class TimerHandler {
+ public:
+ virtual ~TimerHandler() {}
+
+ // Timer should sleep until base, base + offset, base + offset * 2, ...
+ // If repeat_offset isn't set, the timer only expires once.
+ virtual void Setup(monotonic_clock::time_point base,
+ monotonic_clock::duration repeat_offset =
+ ::aos::monotonic_clock::zero()) = 0;
+
+ // Stop future calls to callback().
+ virtual void Disable() = 0;
+};
+
+// Interface for phased loops. They are built on timers.
+class PhasedLoopHandler {
+ public:
+ virtual ~PhasedLoopHandler() {}
+
+ // Sets the interval and offset. Any changes to interval and offset only take
+ // effect when the handler finishes running.
+ virtual void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset) = 0;
+};
+
+// TODO(austin): Ping pong example apps, and then start doing introspection.
+// TODO(austin): Timing reporter. Publish statistics on latencies of
+// handlers.
+class EventLoop {
+ public:
+ EventLoop(const Configuration *configuration)
+ : configuration_(configuration) {}
+
+ virtual ~EventLoop() {}
+
+ // Current time.
+ virtual monotonic_clock::time_point monotonic_now() = 0;
+ virtual realtime_clock::time_point realtime_now() = 0;
+
+ // Note, it is supported to create:
+ // multiple fetchers, and (one sender or one watcher) per <name, type>
+ // tuple.
+
+ // Makes a class that will always fetch the most recent value
+ // sent to the provided channel.
+ template <typename T>
+ Fetcher<T> MakeFetcher(const absl::string_view channel_name) {
+ const Channel *channel = configuration::GetChannel(
+ configuration_, channel_name, T::GetFullyQualifiedName(), name());
+ CHECK(channel != nullptr)
+ << ": Channel { \"name\": \"" << channel_name << "\", \"type\": \""
+ << T::GetFullyQualifiedName() << "\" } not found in config.";
+
+ return Fetcher<T>(MakeRawFetcher(channel));
+ }
+
+ // Makes class that allows constructing and sending messages to
+ // the provided channel.
+ template <typename T>
+ Sender<T> MakeSender(const absl::string_view channel_name) {
+ const Channel *channel = configuration::GetChannel(
+ configuration_, channel_name, T::GetFullyQualifiedName(), name());
+ CHECK(channel != nullptr)
+ << ": Channel { \"name\": \"" << channel_name << "\", \"type\": \""
+ << T::GetFullyQualifiedName() << "\" } not found in config.";
+
+ return Sender<T>(MakeRawSender(channel));
+ }
+
+ // This will watch messages sent to the provided channel.
+ //
+ // Watch is a functor that have a call signature like so:
+ // void Event(const MessageType& type);
+ //
+ // TODO(parker): Need to support ::std::bind. For now, use lambdas.
+ // TODO(austin): Do we need a functor? Or is a std::function good enough?
+ template <typename Watch>
+ void MakeWatcher(const absl::string_view name, Watch &&w);
+
+ // The passed in function will be called when the event loop starts.
+ // Use this to run code once the thread goes into "real-time-mode",
+ virtual void OnRun(::std::function<void()> on_run) = 0;
+
+ // Sets the name of the event loop. This is the application name.
+ virtual void set_name(const absl::string_view name) = 0;
+ // Gets the name of the event loop.
+ virtual const absl::string_view name() const = 0;
+
+ // Creates a timer that executes callback when the timer expires
+ // Returns a TimerHandle for configuration of the timer
+ virtual TimerHandler *AddTimer(::std::function<void()> callback) = 0;
+
+ // Creates a timer that executes callback periodically at the specified
+ // interval and offset. Returns a PhasedLoopHandler for interacting with the
+ // timer.
+ virtual PhasedLoopHandler *AddPhasedLoop(
+ ::std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset = ::std::chrono::seconds(0)) = 0;
+
+ // TODO(austin): OnExit
+
+ // Threadsafe.
+ bool is_running() const { return is_running_.load(); }
+
+ // Sets the scheduler priority to run the event loop at. This may not be
+ // called after we go into "real-time-mode".
+ virtual void SetRuntimeRealtimePriority(int priority) = 0;
+
+ // Fetches new messages from the provided channel (path, type). Note: this
+ // channel must be a member of the exact configuration object this was built
+ // with.
+ virtual std::unique_ptr<RawFetcher> MakeRawFetcher(
+ const Channel *channel) = 0;
+
+ // Will watch channel (name, type) for new messages
+ virtual void MakeRawWatcher(
+ const Channel *channel,
+ std::function<void(const Context &context, const void *message)>
+ watcher) = 0;
+
+ // Returns the context for the current message.
+ // TODO(austin): Fill out whatever is useful for timers.
+ const Context &context() const { return context_; }
+
+ // Returns the configuration that this event loop was built with.
+ const Configuration *configuration() const { return configuration_; }
+
+ protected:
+ void set_is_running(bool value) { is_running_.store(value); }
+
+ // Will send new messages from channel (path, type).
+ virtual std::unique_ptr<RawSender> MakeRawSender(const Channel *channel) = 0;
+
+ private:
+ ::std::atomic<bool> is_running_{false};
+
+ // Context available for watchers.
+ Context context_;
+
+ const Configuration *configuration_;
+};
+
+} // namespace aos
+
+#include "aos/events/event_loop_tmpl.h"
+
+#endif // AOS_EVENTS_EVENT_LOOP_H
diff --git a/aos/events/event-loop_param_test.cc b/aos/events/event_loop_param_test.cc
similarity index 64%
rename from aos/events/event-loop_param_test.cc
rename to aos/events/event_loop_param_test.cc
index 04343cc..8a0f9b6 100644
--- a/aos/events/event-loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -1,11 +1,13 @@
-#include "aos/events/event-loop_param_test.h"
+#include "aos/events/event_loop_param_test.h"
#include <chrono>
+#include "glog/logging.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "glog/logging.h"
+#include "aos/events/test_message_generated.h"
namespace aos {
namespace testing {
@@ -13,39 +15,27 @@
namespace chrono = ::std::chrono;
} // namespace
-struct TestMessage : public ::aos::Message {
- enum { kQueueLength = 100, kHash = 0x696c0cdc };
- int msg_value;
-
- void Zero() {
- ::aos::Message::Zero();
- msg_value = 0;
- }
- static size_t Size() { return 1 + ::aos::Message::Size(); }
- size_t Print(char *buffer, size_t length) const;
- TestMessage() { Zero(); }
-};
-
// Tests that watcher can receive messages from a sender.
// Also tests that OnRun() works.
TEST_P(AbstractEventLoopTest, Basic) {
auto loop1 = Make();
auto loop2 = MakePrimary();
- auto sender = loop1->MakeSender<TestMessage>("/test");
+ aos::Sender<TestMessage> sender = loop1->MakeSender<TestMessage>("/test");
bool happened = false;
loop2->OnRun([&]() {
happened = true;
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
});
loop2->MakeWatcher("/test", [&](const TestMessage &message) {
- EXPECT_EQ(message.msg_value, 200);
+ EXPECT_EQ(message.value(), 200);
this->Exit();
});
@@ -67,13 +57,14 @@
EXPECT_FALSE(fetcher.Fetch());
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
EXPECT_TRUE(fetcher.Fetch());
ASSERT_FALSE(fetcher.get() == nullptr);
- EXPECT_EQ(fetcher->msg_value, 200);
+ EXPECT_EQ(fetcher.get()->value(), 200);
}
// Tests that watcher will receive all messages sent if they are sent after
@@ -87,7 +78,7 @@
::std::vector<int> values;
loop2->MakeWatcher("/test", [&](const TestMessage &message) {
- values.push_back(message.msg_value);
+ values.push_back(message.value());
if (values.size() == 2) {
this->Exit();
}
@@ -95,21 +86,24 @@
// Before Run, should be ignored.
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 199;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(199);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
loop2->OnRun([&]() {
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
});
@@ -129,18 +123,20 @@
::std::vector<int> values;
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
loop2->MakeWatcher("/test", [&](const TestMessage &message) {
- values.push_back(message.msg_value);
+ values.push_back(message.value());
});
// Add a timer to actually quit.
@@ -164,20 +160,22 @@
::std::vector<int> values;
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
// Add a timer to actually quit.
auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
while (fetcher.FetchNext()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
this->Exit();
});
@@ -200,14 +198,16 @@
::std::vector<int> values;
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
@@ -215,7 +215,7 @@
// Add a timer to actually quit.
auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
while (fetcher.FetchNext()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
this->Exit();
});
@@ -239,14 +239,16 @@
::std::vector<int> values;
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
@@ -254,11 +256,11 @@
// Add a timer to actually quit.
auto test_timer = loop2->AddTimer([&fetcher, &values, this]() {
if (fetcher.Fetch()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
// Do it again to make sure we don't double fetch.
if (fetcher.Fetch()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
this->Exit();
});
@@ -281,14 +283,16 @@
::std::vector<int> values;
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 201;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(201);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
@@ -296,31 +300,34 @@
// Add a timer to actually quit.
auto test_timer = loop2->AddTimer([&fetcher, &values, &sender, this]() {
if (fetcher.Fetch()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 202;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(202);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 203;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(203);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 204;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(204);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
if (fetcher.FetchNext()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
if (fetcher.Fetch()) {
- values.push_back(fetcher->msg_value);
+ values.push_back(fetcher.get()->value());
}
this->Exit();
@@ -345,29 +352,31 @@
Fetcher<TestMessage> fetcher = fetch_loop->MakeFetcher<TestMessage>("/test");
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 100;
- ASSERT_TRUE(msg.Send());
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(100);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
{
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- ASSERT_TRUE(msg.Send());
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
}
ASSERT_TRUE(fetcher.FetchNext());
ASSERT_NE(nullptr, fetcher.get());
- EXPECT_EQ(100, fetcher->msg_value);
+ EXPECT_EQ(100, fetcher.get()->value());
ASSERT_TRUE(fetcher.FetchNext());
ASSERT_NE(nullptr, fetcher.get());
- EXPECT_EQ(200, fetcher->msg_value);
+ EXPECT_EQ(200, fetcher.get()->value());
// When we run off the end of the queue, expect to still have the old message:
ASSERT_FALSE(fetcher.FetchNext());
ASSERT_NE(nullptr, fetcher.get());
- EXPECT_EQ(200, fetcher->msg_value);
+ EXPECT_EQ(200, fetcher.get()->value());
}
// Verify that making a fetcher and watcher for "/test" succeeds.
@@ -384,6 +393,14 @@
auto fetcher2 = loop->MakeFetcher<TestMessage>("/test");
}
+// Verify that registering a watcher for an invalid channel name dies.
+TEST_P(AbstractEventLoopDeathTest, InvalidChannelName) {
+ auto loop = Make();
+ EXPECT_DEATH(
+ { loop->MakeWatcher("/test/invalid", [&](const TestMessage &) {}); },
+ "/test/invalid");
+}
+
// Verify that registering a watcher twice for "/test" fails.
TEST_P(AbstractEventLoopDeathTest, TwoWatcher) {
auto loop = Make();
@@ -438,16 +455,17 @@
loop2->MakeWatcher("/test1", [&](const TestMessage &) {});
loop2->MakeWatcher("/test2", [&](const TestMessage &message) {
- EXPECT_EQ(message.msg_value, 200);
+ EXPECT_EQ(message.value(), 200);
this->Exit();
});
auto sender = loop1->MakeSender<TestMessage>("/test2");
loop2->OnRun([&]() {
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
});
Run();
@@ -525,9 +543,18 @@
auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
auto test_timer = loop1->AddTimer([&sender]() {
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ ASSERT_TRUE(msg.Send(builder.Finish()));
+ });
+
+ loop2->MakeWatcher("/test", [&loop2](const TestMessage &msg) {
+ // Confirm that the data pointer makes sense from a watcher.
+ EXPECT_GT(&msg, loop2->context().data);
+ EXPECT_LT(&msg, reinterpret_cast<void *>(
+ reinterpret_cast<char *>(loop2->context().data) +
+ loop2->context().size));
});
test_timer->Setup(loop1->monotonic_now() + ::std::chrono::seconds(1));
@@ -537,15 +564,36 @@
EXPECT_TRUE(fetcher.Fetch());
- monotonic_clock::duration time_offset =
- fetcher->sent_time - (loop1->monotonic_now() - ::std::chrono::seconds(1));
+ monotonic_clock::duration monotonic_time_offset =
+ fetcher.context().monotonic_sent_time -
+ (loop1->monotonic_now() - ::std::chrono::seconds(1));
+ realtime_clock::duration realtime_time_offset =
+ fetcher.context().realtime_sent_time -
+ (loop1->realtime_now() - ::std::chrono::seconds(1));
- EXPECT_TRUE(time_offset > ::std::chrono::milliseconds(-500))
- << ": Got " << fetcher->sent_time.time_since_epoch().count()
+ EXPECT_TRUE(monotonic_time_offset > ::std::chrono::milliseconds(-500))
+ << ": Got "
+ << fetcher.context().monotonic_sent_time.time_since_epoch().count()
<< " expected " << loop1->monotonic_now().time_since_epoch().count();
- EXPECT_TRUE(time_offset < ::std::chrono::milliseconds(500))
- << ": Got " << fetcher->sent_time.time_since_epoch().count()
+ // Confirm that the data pointer makes sense.
+ EXPECT_GT(fetcher.get(), fetcher.context().data);
+ EXPECT_LT(fetcher.get(),
+ reinterpret_cast<void *>(
+ reinterpret_cast<char *>(fetcher.context().data) +
+ fetcher.context().size));
+ EXPECT_TRUE(monotonic_time_offset < ::std::chrono::milliseconds(500))
+ << ": Got "
+ << fetcher.context().monotonic_sent_time.time_since_epoch().count()
<< " expected " << loop1->monotonic_now().time_since_epoch().count();
+
+ EXPECT_TRUE(realtime_time_offset > ::std::chrono::milliseconds(-500))
+ << ": Got "
+ << fetcher.context().realtime_sent_time.time_since_epoch().count()
+ << " expected " << loop1->realtime_now().time_since_epoch().count();
+ EXPECT_TRUE(realtime_time_offset < ::std::chrono::milliseconds(500))
+ << ": Got "
+ << fetcher.context().realtime_sent_time.time_since_epoch().count()
+ << " expected " << loop1->realtime_now().time_since_epoch().count();
}
// Tests that a couple phased loops run in a row result in the correct offset
@@ -612,41 +660,5 @@
1.0, 0.1);
}
-// Verify that sending lots and lots of messages and using FetchNext gets a
-// contiguous block of messages and doesn't crash.
-// TODO(austin): We should store the same number of messages in simulation and
-// reality.
-TEST_P(AbstractEventLoopTest, LotsOfSends) {
- auto loop1 = MakePrimary();
- auto loop2 = Make();
- auto sender = loop1->MakeSender<TestMessage>("/test");
- auto fetcher = loop2->MakeFetcher<TestMessage>("/test");
-
- auto test_timer = loop1->AddTimer([&sender, &fetcher, this]() {
- for (int i = 0; i < 100000; ++i) {
- auto msg = sender.MakeMessage();
- msg->msg_value = i;
- msg.Send();
- }
-
- int last = 0;
- if (fetcher.FetchNext()) {
- last = fetcher->msg_value;
- }
- while (fetcher.FetchNext()) {
- EXPECT_EQ(last + 1, fetcher->msg_value);
- ++last;
- }
-
- this->Exit();
- });
-
- loop1->OnRun([&test_timer, &loop1]() {
- test_timer->Setup(loop1->monotonic_now() + ::std::chrono::milliseconds(10));
- });
-
- Run();
-}
-
} // namespace testing
} // namespace aos
diff --git a/aos/events/event-loop_param_test.h b/aos/events/event_loop_param_test.h
similarity index 61%
rename from aos/events/event-loop_param_test.h
rename to aos/events/event_loop_param_test.h
index 83f0b37..fb08ac3 100644
--- a/aos/events/event-loop_param_test.h
+++ b/aos/events/event_loop_param_test.h
@@ -3,7 +3,9 @@
#include <vector>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
#include "gtest/gtest.h"
namespace aos {
@@ -11,6 +13,25 @@
class EventLoopTestFactory {
public:
+ EventLoopTestFactory()
+ : flatbuffer_(JsonToFlatbuffer("{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/test\",\n"
+ " \"type\": \"aos.TestMessage\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/test1\",\n"
+ " \"type\": \"aos.TestMessage\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/test2\",\n"
+ " \"type\": \"aos.TestMessage\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ Configuration::MiniReflectTypeTable())) {}
+
virtual ~EventLoopTestFactory() {}
// Makes a connected event loop.
@@ -27,6 +48,11 @@
// Advances time by sleeping. Can't be called from inside a loop.
virtual void SleepFor(::std::chrono::nanoseconds duration) = 0;
+
+ const Configuration *configuration() { return &flatbuffer_.message(); }
+
+ private:
+ FlatbufferDetachedBuffer<Configuration> flatbuffer_;
};
class AbstractEventLoopTestBase
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
new file mode 100644
index 0000000..9910d4d
--- /dev/null
+++ b/aos/events/event_loop_tmpl.h
@@ -0,0 +1,48 @@
+#ifndef AOS_EVENTS_EVENT_LOOP_TMPL_H_
+#define AOS_EVENTS_EVENT_LOOP_TMPL_H_
+
+#include <type_traits>
+#include "aos/events/event_loop.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+// From a watch functor, this will extract the message type of the argument.
+// This is the template forward declaration, and it extracts the call operator
+// as a PTMF to be used by the following specialization.
+template <class T>
+struct watch_message_type_trait
+ : watch_message_type_trait<decltype(&T::operator())> {};
+
+// From a watch functor, this will extract the message type of the argument.
+// This is the template specialization.
+template <class ClassType, class ReturnType, class A1>
+struct watch_message_type_trait<ReturnType (ClassType::*)(A1) const> {
+ using message_type = typename std::decay<A1>::type;
+};
+
+template <typename T>
+typename Sender<T>::Builder Sender<T>::MakeBuilder() {
+ return Builder(sender_.get(), sender_->data(), sender_->size());
+}
+
+template <typename Watch>
+void EventLoop::MakeWatcher(const absl::string_view channel_name, Watch &&w) {
+ using T = typename watch_message_type_trait<Watch>::message_type;
+ const Channel *channel = configuration::GetChannel(
+ configuration_, channel_name, T::GetFullyQualifiedName(), name());
+
+ CHECK(channel != nullptr)
+ << ": Channel { \"name\": \"" << channel_name << "\", \"type\": \""
+ << T::GetFullyQualifiedName() << "\" } not found in config.";
+
+ return MakeRawWatcher(
+ channel, [this, w](const Context &context, const void *message) {
+ context_ = context;
+ w(*flatbuffers::GetRoot<T>(reinterpret_cast<const char *>(message)));
+ });
+}
+
+} // namespace aos
+
+#endif // AOS_EVENTS_EVENT_LOOP_TMPL_H
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
new file mode 100644
index 0000000..a4cfa72
--- /dev/null
+++ b/aos/events/event_scheduler.cc
@@ -0,0 +1,48 @@
+#include "aos/events/event_scheduler.h"
+
+#include <algorithm>
+#include <deque>
+
+#include "aos/events/event_loop.h"
+
+namespace aos {
+
+EventScheduler::Token EventScheduler::Schedule(
+ ::aos::monotonic_clock::time_point time, ::std::function<void()> callback) {
+ return events_list_.emplace(time, callback);
+}
+
+void EventScheduler::Deschedule(EventScheduler::Token token) {
+ events_list_.erase(token);
+}
+
+void EventScheduler::RunFor(monotonic_clock::duration duration) {
+ const ::aos::monotonic_clock::time_point end_time =
+ monotonic_now() + duration;
+ is_running_ = true;
+ while (!events_list_.empty() && is_running_) {
+ auto iter = events_list_.begin();
+ ::aos::monotonic_clock::time_point next_time = iter->first;
+ if (next_time > end_time) {
+ break;
+ }
+ now_ = iter->first;
+ ::std::function<void()> callback = ::std::move(iter->second);
+ events_list_.erase(iter);
+ callback();
+ }
+ now_ = end_time;
+}
+
+void EventScheduler::Run() {
+ is_running_ = true;
+ while (!events_list_.empty() && is_running_) {
+ auto iter = events_list_.begin();
+ now_ = iter->first;
+ ::std::function<void()> callback = ::std::move(iter->second);
+ events_list_.erase(iter);
+ callback();
+ }
+}
+
+} // namespace aos
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
new file mode 100644
index 0000000..66d34b3
--- /dev/null
+++ b/aos/events/event_scheduler.h
@@ -0,0 +1,61 @@
+#ifndef AOS_EVENTS_EVENT_SCHEDULER_H_
+#define AOS_EVENTS_EVENT_SCHEDULER_H_
+
+#include <algorithm>
+#include <map>
+#include <memory>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+class EventScheduler {
+ public:
+ using ChannelType =
+ std::multimap<monotonic_clock::time_point, std::function<void()>>;
+ using Token = ChannelType::iterator;
+
+ // Schedule an event with a callback function
+ // Returns an iterator to the event
+ Token Schedule(monotonic_clock::time_point time,
+ std::function<void()> callback);
+
+ Token InvalidToken() { return events_list_.end(); }
+
+ // Deschedule an event by its iterator
+ void Deschedule(Token token);
+
+ // Runs until exited.
+ void Run();
+ // Runs for a duration.
+ void RunFor(monotonic_clock::duration duration);
+
+ void Exit() { is_running_ = false; }
+
+ bool is_running() const { return is_running_; }
+
+ monotonic_clock::time_point monotonic_now() const { return now_; }
+ realtime_clock::time_point realtime_now() const {
+ // TODO(austin): Make this all configurable...
+ return realtime_clock::epoch() + now_.time_since_epoch() +
+ std::chrono::seconds(1000000);
+ }
+
+ private:
+ // Current execution time.
+ monotonic_clock::time_point now_ = monotonic_clock::epoch();
+
+ // Multimap holding times to run functions. These are stored in order, and
+ // the order is the callback tree.
+ ChannelType events_list_;
+ bool is_running_ = false;
+};
+
+} // namespace aos
+
+#endif // AOS_EVENTS_EVENT_SCHEDULER_H_
diff --git a/aos/events/ping.cc b/aos/events/ping.cc
new file mode 100644
index 0000000..b904cb2
--- /dev/null
+++ b/aos/events/ping.cc
@@ -0,0 +1,88 @@
+#include <chrono>
+
+#include "aos/configuration.h"
+#include "aos/events/pingpong_generated.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_int32(sleep_ms, 10, "Time to sleep between pings");
+
+namespace aos {
+
+namespace chrono = std::chrono;
+
+class Ping {
+ public:
+ Ping(EventLoop *event_loop)
+ : event_loop_(event_loop),
+ sender_(event_loop_->MakeSender<examples::Ping>("/test")) {
+ timer_handle_ = event_loop_->AddTimer([this]() { SendPing(); });
+
+ event_loop_->MakeWatcher(
+ "/test", [this](const examples::Pong &pong) { HandlePong(pong); });
+
+ event_loop_->OnRun([this]() {
+ timer_handle_->Setup(event_loop_->monotonic_now(),
+ chrono::milliseconds(FLAGS_sleep_ms));
+ });
+
+ event_loop_->SetRuntimeRealtimePriority(5);
+ }
+
+ void SendPing() {
+ ++count_;
+ aos::Sender<examples::Ping>::Builder msg = sender_.MakeBuilder();
+ examples::Ping::Builder builder = msg.MakeBuilder<examples::Ping>();
+ builder.add_value(count_);
+ builder.add_send_time(
+ event_loop_->monotonic_now().time_since_epoch().count());
+ CHECK(msg.Send(builder.Finish()));
+ VLOG(2) << "Sending ping";
+ }
+
+ void HandlePong(const examples::Pong &pong) {
+ const aos::monotonic_clock::time_point monotonic_send_time(
+ chrono::nanoseconds(pong.initial_send_time()));
+ const aos::monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+
+ const chrono::nanoseconds round_trip_time =
+ monotonic_now - monotonic_send_time;
+
+ if (pong.value() == count_) {
+ VLOG(1) << "Elapsed time " << round_trip_time.count() << " ns "
+ << FlatbufferToJson(&pong);
+ } else {
+ VLOG(1) << "Missmatched pong message";
+ }
+ }
+
+ private:
+ EventLoop *event_loop_;
+ aos::Sender<examples::Ping> sender_;
+ TimerHandler *timer_handle_;
+ int count_ = 0;
+};
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ FLAGS_logtostderr = true;
+ google::InitGoogleLogging(argv[0]);
+ ::gflags::ParseCommandLineFlags(&argc, &argv, true);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos/events/config.fb.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ aos::Ping ping(&event_loop);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/aos/events/pingpong.fbs b/aos/events/pingpong.fbs
new file mode 100644
index 0000000..67e5015
--- /dev/null
+++ b/aos/events/pingpong.fbs
@@ -0,0 +1,14 @@
+namespace aos.examples;
+
+table Ping {
+ value:int;
+ send_time:long;
+}
+
+table Pong {
+ value:int;
+ initial_send_time:long;
+}
+
+root_type Ping;
+root_type Pong;
diff --git a/aos/events/pong.cc b/aos/events/pong.cc
new file mode 100644
index 0000000..1504bfb
--- /dev/null
+++ b/aos/events/pong.cc
@@ -0,0 +1,54 @@
+#include <chrono>
+
+#include "aos/configuration.h"
+#include "aos/events/pingpong_generated.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+namespace chrono = std::chrono;
+
+class Pong {
+ public:
+ Pong(EventLoop *event_loop)
+ : event_loop_(event_loop),
+ sender_(event_loop_->MakeSender<examples::Pong>("/test")) {
+ event_loop_->MakeWatcher("/test", [this](const examples::Ping &ping) {
+ aos::Sender<examples::Pong>::Builder msg = sender_.MakeBuilder();
+ examples::Pong::Builder builder = msg.MakeBuilder<examples::Pong>();
+ builder.add_value(ping.value());
+ builder.add_initial_send_time(ping.send_time());
+ CHECK(msg.Send(builder.Finish()));
+ });
+
+ event_loop_->SetRuntimeRealtimePriority(5);
+ }
+
+ private:
+ EventLoop *event_loop_;
+ aos::Sender<examples::Pong> sender_;
+};
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ FLAGS_logtostderr = true;
+ google::InitGoogleLogging(argv[0]);
+ ::gflags::ParseCommandLineFlags(&argc, &argv, true);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos/events/config.fb.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ aos::Pong ping(&event_loop);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/aos/events/raw-event-loop.h b/aos/events/raw-event-loop.h
deleted file mode 100644
index 887c7ab..0000000
--- a/aos/events/raw-event-loop.h
+++ /dev/null
@@ -1,179 +0,0 @@
-#ifndef _AOS_EVENTS_RAW_EVENT_LOOP_H_
-#define _AOS_EVENTS_RAW_EVENT_LOOP_H_
-
-#include <atomic>
-#include <memory>
-#include <string>
-#include "aos/queue.h"
-#include "aos/time/time.h"
-
-// This file contains raw versions of the classes in event-loop.h.
-//
-// Users should look exclusively at event-loop.h. Only people who wish to
-// implement a new IPC layer (like a fake layer for example) may wish to use
-// these classes.
-namespace aos {
-
-// Raw version of fetcher. Contains a local variable that the fetcher will
-// update.
-// It is the job of the typed version to cast this to the appropriate type.
-class RawFetcher {
- public:
- RawFetcher() {}
- virtual ~RawFetcher() {}
-
- // Non-blocking fetch of the next message in the queue. Returns true if there
- // was a new message and we got it.
- virtual bool FetchNext() = 0;
- // Non-blocking fetch of the latest message:
- virtual bool Fetch() = 0;
-
- const void *most_recent() { return most_recent_; }
-
- protected:
- RawFetcher(const RawFetcher &) = delete;
- RawFetcher &operator=(const RawFetcher &) = delete;
- void set_most_recent(const void *most_recent) { most_recent_ = most_recent; }
-
- private:
- const void *most_recent_ = nullptr;
-};
-
-// Raw version of sender. Sending a message is a 3 part process. Fetch an opaque
-// token, cast that token to the message type, populate and then calling one of
-// Send() or Free().
-class RawSender {
- public:
- RawSender() {}
- virtual ~RawSender() {}
-
- virtual aos::Message *GetMessage() = 0;
-
- virtual void Free(aos::Message *msg) = 0;
-
- virtual bool Send(aos::Message *msg) = 0;
-
- // Call operator that calls Free().
- template <typename T>
- void operator()(T *t) {
- Free(t);
- }
-
- virtual const char *name() const = 0;
-
- protected:
- RawSender(const RawSender &) = delete;
- RawSender &operator=(const RawSender &) = delete;
-};
-
-// Opaque Information extracted from a particular type passed to the underlying
-// system so that it knows how much memory to allocate etc.
-struct QueueTypeInfo {
- // Message size:
- size_t size;
- // This should be a globally unique identifier for the type.
- int hash;
- // Config parameter for how long the queue should be.
- int queue_length;
-
- template <typename T>
- static QueueTypeInfo Get() {
- QueueTypeInfo info;
- info.size = sizeof(T);
- info.hash = T::kHash;
- info.queue_length = T::kQueueLength;
- return info;
- }
-
- // Necessary for the comparison of QueueTypeInfo objects in the
- // SimulatedEventLoop.
- bool operator<(const QueueTypeInfo &other) const {
- if (size != other.size) return size < other.size;
- if (hash != other.hash) return hash < other.hash;
- return queue_length < other.queue_length;
- }
-};
-
-// Interface for timers
-class TimerHandler {
- public:
- virtual ~TimerHandler() {}
-
- // Timer should sleep until base, base + offset, base + offset * 2, ...
- // If repeat_offset isn't set, the timer only expires once.
- virtual void Setup(monotonic_clock::time_point base,
- monotonic_clock::duration repeat_offset =
- ::aos::monotonic_clock::zero()) = 0;
-
- // Stop future calls to callback().
- virtual void Disable() = 0;
-};
-
-// Interface for phased loops. They are built on timers.
-class PhasedLoopHandler {
- public:
- virtual ~PhasedLoopHandler() {}
-
- // Sets the interval and offset. Any changes to interval and offset only take
- // effect when the handler finishes running.
- virtual void set_interval_and_offset(
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) = 0;
-};
-
-class EventScheduler;
-
-// Virtual base class for all event queue-types.
-class RawEventLoop {
- public:
- virtual ~RawEventLoop() {}
-
- // Current time.
- virtual monotonic_clock::time_point monotonic_now() = 0;
-
- // The passed in function will be called when the event loop starts.
- // Use this to run code once the thread goes into "real-time-mode",
- virtual void OnRun(::std::function<void()> on_run) = 0;
-
- // Sets the name of the event loop.
- virtual void set_name(const char *name) = 0;
-
- // Threadsafe.
- bool is_running() const { return is_running_.load(); }
-
- // Creates a timer that executes callback when the timer expires
- // Returns a TimerHandle for configuration of the timer
- virtual TimerHandler *AddTimer(::std::function<void()> callback) = 0;
-
- // Creates a timer that executes callback periodically at the specified
- // interval and offset. Returns a PhasedLoopHandler for interacting with the
- // timer.
- virtual PhasedLoopHandler *AddPhasedLoop(
- ::std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset = ::std::chrono::seconds(0)) = 0;
-
- protected:
- friend class EventScheduler;
- void set_is_running(bool value) { is_running_.store(value); }
-
- // Will send new messages from (path, type).
- virtual std::unique_ptr<RawSender> MakeRawSender(
- const std::string &path, const QueueTypeInfo &type) = 0;
-
- // Will fetch new messages from (path, type).
- virtual std::unique_ptr<RawFetcher> MakeRawFetcher(
- const std::string &path, const QueueTypeInfo &type) = 0;
-
- // Will watch (path, type) for new messages
- virtual void MakeRawWatcher(
- const std::string &path, const QueueTypeInfo &type,
- std::function<void(const Message *message)> watcher) = 0;
-
- private:
- ::std::atomic<bool> is_running_{false};
-};
-
-} // namespace aos
-
-#endif // _AOS_EVENTS_RAW_EVENT_LOOP_H_
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
deleted file mode 100644
index 5034f0b..0000000
--- a/aos/events/shm-event-loop.cc
+++ /dev/null
@@ -1,507 +0,0 @@
-#include "aos/events/shm-event-loop.h"
-
-#include <sys/timerfd.h>
-#include <algorithm>
-#include <atomic>
-#include <chrono>
-#include <stdexcept>
-
-#include "aos/events/epoll.h"
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/queue.h"
-#include "aos/util/phased_loop.h"
-
-namespace aos {
-
-ShmEventLoop::ShmEventLoop() {}
-
-namespace {
-
-namespace chrono = ::std::chrono;
-
-class ShmFetcher : public RawFetcher {
- public:
- explicit ShmFetcher(RawQueue *queue) : queue_(queue) {
- // Move index_ to point to the end of the queue as it is at construction
- // time. Also grab the oldest message but don't expose it to the user yet.
- static constexpr Options<RawQueue> kOptions =
- RawQueue::kFromEnd | RawQueue::kNonBlock;
- msg_ = queue_->ReadMessageIndex(kOptions, &index_);
- }
- ~ShmFetcher() {
- if (msg_) {
- queue_->FreeMessage(msg_);
- }
- }
-
- bool FetchNext() override {
- const void *msg = queue_->ReadMessageIndex(RawQueue::kNonBlock, &index_);
- // Only update the internal pointer if we got a new message.
- if (msg != nullptr) {
- queue_->FreeMessage(msg_);
- msg_ = msg;
- set_most_recent(msg_);
- }
- return msg != nullptr;
- }
-
- bool Fetch() override {
- static constexpr Options<RawQueue> kOptions =
- RawQueue::kFromEnd | RawQueue::kNonBlock;
- const void *msg = queue_->ReadMessageIndex(kOptions, &index_);
- // Only update the internal pointer if we got a new message.
- if (msg != nullptr && msg != msg_) {
- queue_->FreeMessage(msg_);
- msg_ = msg;
- set_most_recent(msg_);
- return true;
- } else {
- // The message has to get freed if we didn't use it (and
- // RawQueue::FreeMessage is ok to call on nullptr).
- queue_->FreeMessage(msg);
-
- // We have a message from construction time. Give it to the user now.
- if (msg_ != nullptr && most_recent() != msg_) {
- set_most_recent(msg_);
- return true;
- } else {
- return false;
- }
- }
- }
-
- private:
- int index_ = 0;
- RawQueue *queue_;
- const void *msg_ = nullptr;
-};
-
-class ShmSender : public RawSender {
- public:
- explicit ShmSender(RawQueue *queue) : queue_(queue) {}
-
- ::aos::Message *GetMessage() override {
- return reinterpret_cast<::aos::Message *>(queue_->GetMessage());
- }
-
- void Free(::aos::Message *msg) override { queue_->FreeMessage(msg); }
-
- bool Send(::aos::Message *msg) override {
- assert(queue_ != nullptr);
- {
- // TODO(austin): This lets multiple senders reorder messages since time
- // isn't acquired with a lock held.
- if (msg->sent_time == monotonic_clock::min_time) {
- msg->sent_time = monotonic_clock::now();
- }
- }
- return queue_->WriteMessage(msg, RawQueue::kOverride);
- }
-
- const char *name() const override { return queue_->name(); }
-
- private:
- RawQueue *queue_;
-};
-
-} // namespace
-
-namespace internal {
-
-// Class to manage the state for a Watcher.
-class WatcherThreadState {
- public:
- WatcherThreadState(
- ShmEventLoop::ThreadState *thread_state, RawQueue *queue,
- ::std::function<void(const ::aos::Message *message)> watcher)
- : thread_state_(thread_state),
- queue_(queue),
- index_(0),
- watcher_(::std::move(watcher)) {}
-
- ~WatcherThreadState() {
- // Only kill the thread if it is running.
- if (running_) {
- // TODO(austin): CHECK that we aren't RT here.
-
- // Try joining. If we fail, we weren't asleep on the condition in the
- // queue. So hit it again and again until that's true.
- struct timespec end_time;
- AOS_PCHECK(clock_gettime(CLOCK_REALTIME, &end_time) == 0);
- while (true) {
- void *retval = nullptr;
- end_time.tv_nsec += 100000000;
- if (end_time.tv_nsec > 1000000000L) {
- end_time.tv_nsec -= 1000000000L;
- ++end_time.tv_sec;
- }
- int ret = pthread_timedjoin_np(pthread_, &retval, &end_time);
- if (ret == ETIMEDOUT) continue;
- AOS_PCHECK(ret == 0);
- break;
- }
- }
- }
-
- // Starts the thread and waits until it is running.
- void Start() {
- AOS_PCHECK(pthread_create(&pthread_, nullptr, &StaticRun, this) == 0);
- IPCRecursiveMutexLocker locker(&thread_started_mutex_);
- if (locker.owner_died()) ::aos::Die("Owner died");
- while (!running_) {
- AOS_CHECK(!thread_started_condition_.Wait());
- }
- }
-
- void GrabQueueIndex() {
- // Right after we are signaled to start, point index to the current index
- // so we don't read any messages received before now. Otherwise we will
- // get a significantly delayed read.
- static constexpr Options<RawQueue> kOptions =
- RawQueue::kFromEnd | RawQueue::kNonBlock;
- const void *msg = queue_->ReadMessageIndex(kOptions, &index_);
- if (msg) {
- queue_->FreeMessage(msg);
- }
- }
-
- private:
- // Runs Run given a WatcherThreadState as the argument. This is an adapter
- // between pthreads and Run.
- static void *StaticRun(void *arg) {
- WatcherThreadState *watcher_thread_state =
- reinterpret_cast<WatcherThreadState *>(arg);
- watcher_thread_state->Run();
- return nullptr;
- }
-
- // Runs the watcher callback on new messages.
- void Run() {
- ::aos::SetCurrentThreadName(thread_state_->name() + ".watcher");
-
- // Signal the main thread that we are now ready.
- thread_state_->MaybeSetCurrentThreadRealtimePriority();
- {
- IPCRecursiveMutexLocker locker(&thread_started_mutex_);
- if (locker.owner_died()) ::aos::Die("Owner died");
- running_ = true;
- thread_started_condition_.Broadcast();
- }
-
- // Wait for the global start before handling events.
- thread_state_->WaitForStart();
-
- // Bail immediately if we are supposed to stop.
- if (!thread_state_->is_running()) {
- ::aos::UnsetCurrentThreadRealtimePriority();
- return;
- }
-
- const void *msg = nullptr;
- while (true) {
- msg = queue_->ReadMessageIndex(RawQueue::kBlock, &index_,
- chrono::seconds(1));
- // We hit a timeout. Confirm that we should be running and retry. Note,
- // is_running is threadsafe (it's an atomic underneath). Worst case, we
- // check again in a second.
- if (msg == nullptr) {
- if (!thread_state_->is_running()) break;
- continue;
- }
-
- {
- // Grab the lock so that only one callback can be called at a time.
- MutexLocker locker(&thread_state_->mutex_);
- if (!thread_state_->is_running()) break;
-
- watcher_(reinterpret_cast<const Message *>(msg));
- // watcher_ may have exited the event loop.
- if (!thread_state_->is_running()) break;
- }
- // Drop the reference.
- queue_->FreeMessage(msg);
- }
-
- // And drop the last reference.
- queue_->FreeMessage(msg);
- // Now that everything is cleaned up, drop RT priority before destroying the
- // thread.
- ::aos::UnsetCurrentThreadRealtimePriority();
- }
- pthread_t pthread_;
- ShmEventLoop::ThreadState *thread_state_;
- RawQueue *queue_;
- int32_t index_;
- bool running_ = false;
-
- ::std::function<void(const Message *message)> watcher_;
-
- // Mutex and condition variable used to wait until the thread is started
- // before going RT.
- ::aos::Mutex thread_started_mutex_;
- ::aos::Condition thread_started_condition_{&thread_started_mutex_};
-};
-
-// Adapter class to adapt a timerfd to a TimerHandler.
-// The part of the API which is accessed by the TimerHandler interface needs to
-// be threadsafe. This means Setup and Disable.
-class TimerHandlerState : public TimerHandler {
- public:
- TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
- : shm_event_loop_(shm_event_loop), fn_(::std::move(fn)) {
- shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
- MutexLocker locker(&shm_event_loop_->thread_state_.mutex_);
- timerfd_.Read();
- fn_();
- });
- }
-
- ~TimerHandlerState() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }
-
- void Setup(monotonic_clock::time_point base,
- monotonic_clock::duration repeat_offset) override {
- // SetTime is threadsafe already.
- timerfd_.SetTime(base, repeat_offset);
- }
-
- void Disable() override {
- // Disable is also threadsafe already.
- timerfd_.Disable();
- }
-
- private:
- ShmEventLoop *shm_event_loop_;
-
- TimerFd timerfd_;
-
- // Function to be run on the thread
- ::std::function<void()> fn_;
-};
-
-// Adapter class to the timerfd and PhasedLoop.
-// The part of the API which is accessed by the PhasedLoopHandler interface
-// needs to be threadsafe. This means set_interval_and_offset
-class PhasedLoopHandler : public ::aos::PhasedLoopHandler {
- public:
- PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset)
- : shm_event_loop_(shm_event_loop),
- phased_loop_(interval, shm_event_loop_->monotonic_now(), offset),
- fn_(::std::move(fn)) {
- shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
- MutexLocker locker(&shm_event_loop_->thread_state_.mutex_);
- {
- MutexLocker locker(&mutex_);
- timerfd_.Read();
- }
- // Call the function. To avoid needing a recursive mutex, drop the lock
- // before running the function.
- fn_(cycles_elapsed_);
- {
- MutexLocker locker(&mutex_);
- Reschedule();
- }
- });
- }
-
- ~PhasedLoopHandler() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }
-
- void set_interval_and_offset(
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) override {
- MutexLocker locker(&mutex_);
- phased_loop_.set_interval_and_offset(interval, offset);
- }
-
- void Startup() {
- MutexLocker locker(&mutex_);
- phased_loop_.Reset(shm_event_loop_->monotonic_now());
- Reschedule();
- }
-
- private:
- // Reschedules the timer. Must be called with the mutex held.
- void Reschedule() {
- cycles_elapsed_ = phased_loop_.Iterate(shm_event_loop_->monotonic_now());
- timerfd_.SetTime(phased_loop_.sleep_time(), ::aos::monotonic_clock::zero());
- }
-
- ShmEventLoop *shm_event_loop_;
-
- // Mutex to protect access to the timerfd_ (not strictly necessary), and the
- // phased_loop (necessary).
- ::aos::Mutex mutex_;
-
- TimerFd timerfd_;
- time::PhasedLoop phased_loop_;
-
- int cycles_elapsed_ = 1;
-
- // Function to be run
- const ::std::function<void(int)> fn_;
-};
-} // namespace internal
-
-::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
- const ::std::string &path, const QueueTypeInfo &type) {
- return ::std::unique_ptr<RawFetcher>(new ShmFetcher(
- RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
-}
-
-::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
- const ::std::string &path, const QueueTypeInfo &type) {
- Take(path);
- return ::std::unique_ptr<RawSender>(new ShmSender(
- RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
-}
-
-void ShmEventLoop::MakeRawWatcher(
- const ::std::string &path, const QueueTypeInfo &type,
- ::std::function<void(const Message *message)> watcher) {
- Take(path);
- ::std::unique_ptr<internal::WatcherThreadState> state(
- new internal::WatcherThreadState(
- &thread_state_, RawQueue::Fetch(path.c_str(), type.size, type.hash,
- type.queue_length),
- std::move(watcher)));
- watchers_.push_back(::std::move(state));
-}
-
-TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
- ::std::unique_ptr<internal::TimerHandlerState> timer(
- new internal::TimerHandlerState(this, ::std::move(callback)));
-
- timers_.push_back(::std::move(timer));
-
- return timers_.back().get();
-}
-
-PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
- ::std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) {
- ::std::unique_ptr<internal::PhasedLoopHandler> phased_loop(
- new internal::PhasedLoopHandler(this, ::std::move(callback), interval,
- offset));
-
- phased_loops_.push_back(::std::move(phased_loop));
-
- return phased_loops_.back().get();
-}
-
-void ShmEventLoop::OnRun(::std::function<void()> on_run) {
- on_run_.push_back(::std::move(on_run));
-}
-
-void ShmEventLoop::set_name(const char *name) { thread_state_.name_ = name; }
-
-void ShmEventLoop::Run() {
- // Start all the watcher threads.
- for (::std::unique_ptr<internal::WatcherThreadState> &watcher : watchers_) {
- watcher->Start();
- }
-
- ::aos::SetCurrentThreadName(thread_state_.name());
-
- // Now, all the threads are up. Lock everything into memory and go RT.
- if (thread_state_.priority_ != -1) {
- ::aos::InitRT();
- }
- thread_state_.MaybeSetCurrentThreadRealtimePriority();
- set_is_running(true);
-
- // Now that we are realtime (but before the OnRun handlers run), snap the
- // queue index.
- for (::std::unique_ptr<internal::WatcherThreadState> &watcher : watchers_) {
- watcher->GrabQueueIndex();
- }
-
- // Now that we are RT, run all the OnRun handlers.
- for (const auto &run : on_run_) {
- run();
- }
-
- // Start up all the phased loops.
- for (::std::unique_ptr<internal::PhasedLoopHandler> &phased_loop :
- phased_loops_) {
- phased_loop->Startup();
- }
- // TODO(austin): We don't need a separate watcher thread if there are only
- // watchers and fetchers. Could lazily create the epoll loop and pick a
- // victim watcher to run in this thread.
- // Trigger all the threads to start now.
- thread_state_.Start();
-
- // And start our main event loop which runs all the timers and handles Quit.
- epoll_.Run();
-
- // Once epoll exits, there is no useful nonrt work left to do.
- set_is_running(false);
-
- // Signal all the watcher threads to exit. After this point, no more
- // callbacks will be handled.
- thread_state_.Exit();
-
- // Nothing time or synchronization critical needs to happen after this point.
- // Drop RT priority.
- ::aos::UnsetCurrentThreadRealtimePriority();
-
- // The watcher threads get cleaned up in the destructor.
-}
-
-void ShmEventLoop::ThreadState::Start() {
- MutexLocker locker(&mutex_);
- loop_running_ = true;
- if (loop_finished_) ::aos::Die("Cannot restart an ShmEventLoop()");
- loop_running_cond_.Broadcast();
-}
-
-void ShmEventLoop::ThreadState::WaitForStart() {
- MutexLocker locker(&mutex_);
- while (!(loop_running_ || loop_finished_)) {
- Condition::WaitResult wait_result =
- loop_running_cond_.WaitTimed(chrono::milliseconds(1000));
- if (wait_result == Condition::WaitResult::kOwnerDied) {
- ::aos::Die("ShmEventLoop mutex lock problem.\n");
- }
- }
-}
-
-void ShmEventLoop::ThreadState::MaybeSetCurrentThreadRealtimePriority() {
- if (priority_ != -1) {
- ::aos::SetCurrentThreadRealtimePriority(priority_);
- }
-}
-
-void ShmEventLoop::Exit() { epoll_.Quit(); }
-
-void ShmEventLoop::ThreadState::Exit() {
- IPCRecursiveMutexLocker locker(&mutex_);
- if (locker.owner_died()) ::aos::Die("Owner died");
- loop_running_ = false;
- loop_finished_ = true;
- loop_running_cond_.Broadcast();
-}
-
-ShmEventLoop::~ShmEventLoop() {
- if (is_running()) {
- ::aos::Die("ShmEventLoop destroyed while running\n");
- }
-}
-
-void ShmEventLoop::Take(const ::std::string &path) {
- if (is_running()) {
- ::aos::Die("Cannot add new objects while running.\n");
- }
-
- const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
- if (prior != taken_.end()) {
- ::aos::Die("%s is already being used.", path.c_str());
- } else {
- taken_.emplace_back(path);
- }
-}
-
-} // namespace aos
diff --git a/aos/events/shm-event-loop.h b/aos/events/shm-event-loop.h
deleted file mode 100644
index 5db8319..0000000
--- a/aos/events/shm-event-loop.h
+++ /dev/null
@@ -1,124 +0,0 @@
-#ifndef AOS_EVENTS_SHM_EVENT_LOOP_H_
-#define AOS_EVENTS_SHM_EVENT_LOOP_H_
-
-#include <unordered_set>
-#include <vector>
-
-#include "aos/condition.h"
-#include "aos/events/epoll.h"
-#include "aos/events/event-loop.h"
-#include "aos/mutex/mutex.h"
-
-namespace aos {
-namespace internal {
-
-class WatcherThreadState;
-class TimerHandlerState;
-class PhasedLoopHandler;
-
-} // namespace internal
-
-// Specialization of EventLoop that is built from queues running out of shared
-// memory. See more details at aos/queue.h
-//
-// This object must be interacted with from one thread, but the Senders and
-// Fetchers may be used from multiple threads afterwords (as long as their
-// destructors are called back in one thread again)
-class ShmEventLoop : public EventLoop {
- public:
- ShmEventLoop();
- ~ShmEventLoop() override;
-
- ::aos::monotonic_clock::time_point monotonic_now() override {
- return ::aos::monotonic_clock::now();
- }
-
- ::std::unique_ptr<RawSender> MakeRawSender(
- const ::std::string &path, const QueueTypeInfo &type) override;
- ::std::unique_ptr<RawFetcher> MakeRawFetcher(
- const ::std::string &path, const QueueTypeInfo &type) override;
-
- void MakeRawWatcher(
- const ::std::string &path, const QueueTypeInfo &type,
- ::std::function<void(const aos::Message *message)> watcher) override;
-
- TimerHandler *AddTimer(::std::function<void()> callback) override;
- ::aos::PhasedLoopHandler *AddPhasedLoop(
- ::std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset =
- ::std::chrono::seconds(0)) override;
-
- void OnRun(::std::function<void()> on_run) override;
- void Run();
- void Exit();
-
- // TODO(austin): Add a function to register control-C call.
-
- void SetRuntimeRealtimePriority(int priority) override {
- if (is_running()) {
- ::aos::Die("Cannot set realtime priority while running.");
- }
- thread_state_.priority_ = priority;
- }
-
- void set_name(const char *name) override;
-
- private:
- friend class internal::WatcherThreadState;
- friend class internal::TimerHandlerState;
- friend class internal::PhasedLoopHandler;
- // This ThreadState ensures that two watchers in the same loop cannot be
- // triggered concurrently. Because watchers block threads indefinitely, this
- // has to be shared_ptr in case the EventLoop is destroyed before the thread
- // receives any new events.
- class ThreadState {
- public:
- void WaitForStart();
-
- bool is_running() { return loop_running_; }
-
- void Start();
-
- void Exit();
-
- void MaybeSetCurrentThreadRealtimePriority();
-
- const ::std::string &name() const { return name_; }
-
- private:
- friend class internal::WatcherThreadState;
- friend class internal::TimerHandlerState;
- friend class internal::PhasedLoopHandler;
- friend class ShmEventLoop;
-
- // This mutex ensures that only one watch event happens at a time.
- ::aos::Mutex mutex_;
- // Block on this until the loop starts.
- ::aos::Condition loop_running_cond_{&mutex_};
- // Used to notify watchers that the loop is done.
- ::std::atomic<bool> loop_running_{false};
- bool loop_finished_ = false;
- int priority_ = -1;
-
- // Immutable after Start is called.
- ::std::string name_;
- };
-
- // Tracks that we can't have multiple watchers or a sender and a watcher (or
- // multiple senders) on a single queue (path).
- void Take(const ::std::string &path);
-
- ::std::vector<::std::function<void()>> on_run_;
- ThreadState thread_state_;
- ::std::vector<::std::string> taken_;
- internal::EPoll epoll_;
-
- ::std::vector<::std::unique_ptr<internal::TimerHandlerState>> timers_;
- ::std::vector<::std::unique_ptr<internal::PhasedLoopHandler>> phased_loops_;
- ::std::vector<::std::unique_ptr<internal::WatcherThreadState>> watchers_;
-};
-
-} // namespace aos
-
-#endif // AOS_EVENTS_SHM_EVENT_LOOP_H_
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
new file mode 100644
index 0000000..5a88717
--- /dev/null
+++ b/aos/events/shm_event_loop.cc
@@ -0,0 +1,611 @@
+#include "glog/logging.h"
+
+#include "aos/events/shm_event_loop.h"
+
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/timerfd.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <stdexcept>
+
+#include "aos/events/epoll.h"
+#include "aos/ipc_lib/lockless_queue.h"
+#include "aos/realtime.h"
+#include "aos/util/phased_loop.h"
+
+DEFINE_string(shm_base, "/dev/shm/aos",
+ "Directory to place queue backing mmaped files in.");
+DEFINE_uint32(permissions, 0770,
+ "Permissions to make shared memory files and folders.");
+
+namespace aos {
+
+std::string ShmFolder(const Channel *channel) {
+ CHECK(channel->has_name());
+ CHECK_EQ(channel->name()->string_view()[0], '/');
+ return FLAGS_shm_base + channel->name()->str() + "/";
+}
+std::string ShmPath(const Channel *channel) {
+ CHECK(channel->has_type());
+ return ShmFolder(channel) + channel->type()->str() + ".v0";
+}
+
+class MMapedQueue {
+ public:
+ MMapedQueue(const Channel *channel) {
+ std::string path = ShmPath(channel);
+
+ // TODO(austin): Pull these out into the config if there is a need.
+ config_.num_watchers = 10;
+ config_.num_senders = 10;
+ config_.queue_size = 2 * channel->frequency();
+ config_.message_data_size = channel->max_size();
+
+ size_ = ipc_lib::LocklessQueueMemorySize(config_);
+
+ MkdirP(path);
+
+ // There are 2 cases. Either the file already exists, or it does not
+ // already exist and we need to create it. Start by trying to create it. If
+ // that fails, the file has already been created and we can open it
+ // normally.. Once the file has been created it wil never be deleted.
+ fd_ = open(path.c_str(), O_RDWR | O_CREAT | O_EXCL,
+ O_CLOEXEC | FLAGS_permissions);
+ if (fd_ == -1 && errno == EEXIST) {
+ VLOG(1) << path << " already created.";
+ // File already exists.
+ fd_ = open(path.c_str(), O_RDWR, O_CLOEXEC);
+ PCHECK(fd_ != -1) << ": Failed to open " << path;
+ while (true) {
+ struct stat st;
+ PCHECK(fstat(fd_, &st) == 0);
+ if (st.st_size != 0) {
+ CHECK_EQ(static_cast<size_t>(st.st_size), size_)
+ << ": Size of " << path
+ << " doesn't match expected size of backing queue file. Did the "
+ "queue definition change?";
+ break;
+ } else {
+ // The creating process didn't get around to it yet. Give it a bit.
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ VLOG(1) << path << " is zero size, waiting";
+ }
+ }
+ } else {
+ VLOG(1) << "Created " << path;
+ PCHECK(fd_ != -1) << ": Failed to open " << path;
+ PCHECK(ftruncate(fd_, size_) == 0);
+ }
+
+ data_ = mmap(NULL, size_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0);
+ PCHECK(data_ != MAP_FAILED);
+
+ ipc_lib::InitializeLocklessQueueMemory(memory(), config_);
+ }
+
+ ~MMapedQueue() {
+ PCHECK(munmap(data_, size_) == 0);
+ PCHECK(close(fd_) == 0);
+ }
+
+ ipc_lib::LocklessQueueMemory *memory() const {
+ return reinterpret_cast<ipc_lib::LocklessQueueMemory *>(data_);
+ }
+
+ const ipc_lib::LocklessQueueConfiguration &config() const {
+ return config_;
+ }
+
+ private:
+ void MkdirP(absl::string_view path) {
+ struct stat st;
+ auto last_slash_pos = path.find_last_of("/");
+
+ std::string folder(last_slash_pos == absl::string_view::npos
+ ? absl::string_view("")
+ : path.substr(0, last_slash_pos));
+ if (stat(folder.c_str(), &st) == -1) {
+ PCHECK(errno == ENOENT);
+ CHECK_NE(folder, "") << ": Base path doesn't exist";
+ MkdirP(folder);
+ VLOG(1) << "Creating " << folder;
+ PCHECK(mkdir(folder.c_str(), FLAGS_permissions) == 0);
+ }
+ }
+
+ ipc_lib::LocklessQueueConfiguration config_;
+
+ int fd_;
+
+ size_t size_;
+ void *data_;
+};
+
+// Returns the portion of the path after the last /.
+absl::string_view Filename(absl::string_view path) {
+ auto last_slash_pos = path.find_last_of("/");
+
+ return last_slash_pos == absl::string_view::npos
+ ? path
+ : path.substr(last_slash_pos + 1, path.size());
+}
+
+ShmEventLoop::ShmEventLoop(const Configuration *configuration)
+ : EventLoop(configuration), name_(Filename(program_invocation_name)) {}
+
+namespace {
+
+namespace chrono = ::std::chrono;
+
+class ShmFetcher : public RawFetcher {
+ public:
+ explicit ShmFetcher(const Channel *channel)
+ : lockless_queue_memory_(channel),
+ lockless_queue_(lockless_queue_memory_.memory(),
+ lockless_queue_memory_.config()),
+ data_storage_(static_cast<AlignedChar *>(aligned_alloc(
+ alignof(AlignedChar), channel->max_size())),
+ &free) {
+ context_.data = nullptr;
+ // Point the queue index at the next index to read starting now. This
+ // makes it such that FetchNext will read the next message sent after
+ // the fetcher is created.
+ PointAtNextQueueIndex();
+ }
+
+ ~ShmFetcher() { data_ = nullptr; }
+
+ // Points the next message to fetch at the queue index which will be
+ // populated next.
+ void PointAtNextQueueIndex() {
+ actual_queue_index_ = lockless_queue_.LatestQueueIndex();
+ if (!actual_queue_index_.valid()) {
+ // Nothing in the queue. The next element will show up at the 0th
+ // index in the queue.
+ actual_queue_index_ =
+ ipc_lib::QueueIndex::Zero(lockless_queue_.queue_size());
+ } else {
+ actual_queue_index_ = actual_queue_index_.Increment();
+ }
+ }
+
+ bool FetchNext() override {
+ // TODO(austin): Write a test which starts with nothing in the queue,
+ // and then calls FetchNext() after something is sent.
+ // TODO(austin): Get behind and make sure it dies both here and with
+ // Fetch.
+ ipc_lib::LocklessQueue::ReadResult read_result = lockless_queue_.Read(
+ actual_queue_index_.index(), &context_.monotonic_sent_time,
+ &context_.realtime_sent_time, &context_.size,
+ reinterpret_cast<char *>(data_storage_.get()));
+ if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
+ context_.queue_index = actual_queue_index_.index();
+ data_ = reinterpret_cast<char *>(data_storage_.get()) +
+ lockless_queue_.message_data_size() - context_.size;
+ context_.data = data_;
+ actual_queue_index_ = actual_queue_index_.Increment();
+ }
+
+ // Make sure the data wasn't modified while we were reading it. This
+ // can only happen if you are reading the last message *while* it is
+ // being written to, which means you are pretty far behind.
+ CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
+ << ": Got behind while reading and the last message was modified "
+ "out "
+ "from under us while we were reading it. Don't get so far "
+ "behind.";
+
+ CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
+ << ": The next message is no longer available.";
+ return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
+ }
+
+ bool Fetch() override {
+ const ipc_lib::QueueIndex queue_index = lockless_queue_.LatestQueueIndex();
+ // actual_queue_index_ is only meaningful if it was set by Fetch or
+ // FetchNext. This happens when valid_data_ has been set. So, only
+ // skip checking if valid_data_ is true.
+ //
+ // Also, if the latest queue index is invalid, we are empty. So there
+ // is nothing to fetch.
+ if ((data_ != nullptr &&
+ queue_index == actual_queue_index_.DecrementBy(1u)) ||
+ !queue_index.valid()) {
+ return false;
+ }
+
+ ipc_lib::LocklessQueue::ReadResult read_result =
+ lockless_queue_.Read(queue_index.index(), &context_.monotonic_sent_time,
+ &context_.realtime_sent_time, &context_.size,
+ reinterpret_cast<char *>(data_storage_.get()));
+ if (read_result == ipc_lib::LocklessQueue::ReadResult::GOOD) {
+ context_.queue_index = queue_index.index();
+ data_ = reinterpret_cast<char *>(data_storage_.get()) +
+ lockless_queue_.message_data_size() - context_.size;
+ context_.data = data_;
+ actual_queue_index_ = queue_index.Increment();
+ }
+
+ // Make sure the data wasn't modified while we were reading it. This
+ // can only happen if you are reading the last message *while* it is
+ // being written to, which means you are pretty far behind.
+ CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::OVERWROTE)
+ << ": Got behind while reading and the last message was modified "
+ "out "
+ "from under us while we were reading it. Don't get so far "
+ "behind.";
+
+ CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::NOTHING_NEW)
+ << ": Queue index went backwards. This should never happen.";
+
+ // We fell behind between when we read the index and read the value.
+ // This isn't worth recovering from since this means we went to sleep
+ // for a long time in the middle of this function.
+ CHECK(read_result != ipc_lib::LocklessQueue::ReadResult::TOO_OLD)
+ << ": The next message is no longer available.";
+ return read_result == ipc_lib::LocklessQueue::ReadResult::GOOD;
+ }
+
+ bool RegisterWakeup(int priority) {
+ return lockless_queue_.RegisterWakeup(priority);
+ }
+
+ void UnregisterWakeup() { lockless_queue_.UnregisterWakeup(); }
+
+ private:
+ MMapedQueue lockless_queue_memory_;
+ ipc_lib::LocklessQueue lockless_queue_;
+
+ ipc_lib::QueueIndex actual_queue_index_ =
+ ipc_lib::LocklessQueue::empty_queue_index();
+
+ struct AlignedChar {
+ alignas(32) char data;
+ };
+
+ std::unique_ptr<AlignedChar, decltype(&free)> data_storage_;
+};
+
+class ShmSender : public RawSender {
+ public:
+ explicit ShmSender(const Channel *channel, const ShmEventLoop *shm_event_loop)
+ : RawSender(),
+ shm_event_loop_(shm_event_loop),
+ name_(channel->name()->str()),
+ lockless_queue_memory_(channel),
+ lockless_queue_(lockless_queue_memory_.memory(),
+ lockless_queue_memory_.config()),
+ lockless_queue_sender_(lockless_queue_.MakeSender()) {}
+
+ void *data() override { return lockless_queue_sender_.Data(); }
+ size_t size() override { return lockless_queue_sender_.size(); }
+ bool Send(size_t size) override {
+ lockless_queue_sender_.Send(size);
+ lockless_queue_.Wakeup(shm_event_loop_->priority());
+ return true;
+ }
+
+ bool Send(void *msg, size_t length) override {
+ lockless_queue_sender_.Send(reinterpret_cast<char *>(msg), length);
+ lockless_queue_.Wakeup(shm_event_loop_->priority());
+ // TODO(austin): Return an error if we send too fast.
+ return true;
+ }
+
+ const absl::string_view name() const override { return name_; }
+
+ private:
+ const ShmEventLoop *shm_event_loop_;
+ std::string name_;
+ MMapedQueue lockless_queue_memory_;
+ ipc_lib::LocklessQueue lockless_queue_;
+ ipc_lib::LocklessQueue::Sender lockless_queue_sender_;
+};
+
+} // namespace
+
+namespace internal {
+
+// Class to manage the state for a Watcher.
+class WatcherState {
+ public:
+ WatcherState(
+ const Channel *channel,
+ std::function<void(const Context &context, const void *message)> watcher)
+ : shm_fetcher_(channel), watcher_(watcher) {}
+
+ ~WatcherState() {}
+
+ // Points the next message to fetch at the queue index which will be populated
+ // next.
+ void PointAtNextQueueIndex() { shm_fetcher_.PointAtNextQueueIndex(); }
+
+ // Returns true if there is new data available.
+ bool HasNewData() {
+ if (!has_new_data_) {
+ has_new_data_ = shm_fetcher_.FetchNext();
+ }
+
+ return has_new_data_;
+ }
+
+ // Returns the time of the current data sample.
+ aos::monotonic_clock::time_point event_time() const {
+ return shm_fetcher_.context().monotonic_sent_time;
+ }
+
+ // Consumes the data by calling the callback.
+ void CallCallback() {
+ CHECK(has_new_data_);
+ watcher_(shm_fetcher_.context(), shm_fetcher_.most_recent_data());
+ has_new_data_ = false;
+ }
+
+ // Starts the thread and waits until it is running.
+ bool RegisterWakeup(int priority) {
+ return shm_fetcher_.RegisterWakeup(priority);
+ }
+
+ void UnregisterWakeup() { return shm_fetcher_.UnregisterWakeup(); }
+
+ private:
+ bool has_new_data_ = false;
+
+ ShmFetcher shm_fetcher_;
+
+ std::function<void(const Context &context, const void *message)> watcher_;
+};
+
+// Adapter class to adapt a timerfd to a TimerHandler.
+// The part of the API which is accessed by the TimerHandler interface needs to
+// be threadsafe. This means Setup and Disable.
+class TimerHandlerState : public TimerHandler {
+ public:
+ TimerHandlerState(ShmEventLoop *shm_event_loop, ::std::function<void()> fn)
+ : shm_event_loop_(shm_event_loop), fn_(::std::move(fn)) {
+ shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
+ timerfd_.Read();
+ fn_();
+ });
+ }
+
+ ~TimerHandlerState() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }
+
+ void Setup(monotonic_clock::time_point base,
+ monotonic_clock::duration repeat_offset) override {
+ // SetTime is threadsafe already.
+ timerfd_.SetTime(base, repeat_offset);
+ }
+
+ void Disable() override {
+ // Disable is also threadsafe already.
+ timerfd_.Disable();
+ }
+
+ private:
+ ShmEventLoop *shm_event_loop_;
+
+ TimerFd timerfd_;
+
+ // Function to be run on the thread
+ ::std::function<void()> fn_;
+};
+
+// Adapter class to the timerfd and PhasedLoop.
+// The part of the API which is accessed by the PhasedLoopHandler interface
+// needs to be threadsafe. This means set_interval_and_offset
+class PhasedLoopHandler : public ::aos::PhasedLoopHandler {
+ public:
+ PhasedLoopHandler(ShmEventLoop *shm_event_loop, ::std::function<void(int)> fn,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset)
+ : shm_event_loop_(shm_event_loop),
+ phased_loop_(interval, shm_event_loop_->monotonic_now(), offset),
+ fn_(::std::move(fn)) {
+ shm_event_loop_->epoll_.OnReadable(timerfd_.fd(), [this]() {
+ timerfd_.Read();
+ // Call the function. To avoid needing a recursive mutex, drop the lock
+ // before running the function.
+ fn_(cycles_elapsed_);
+ Reschedule();
+ });
+ }
+
+ ~PhasedLoopHandler() { shm_event_loop_->epoll_.DeleteFd(timerfd_.fd()); }
+
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset) override {
+ phased_loop_.set_interval_and_offset(interval, offset);
+ }
+
+ void Startup() {
+ phased_loop_.Reset(shm_event_loop_->monotonic_now());
+ Reschedule();
+ }
+
+ private:
+ // Reschedules the timer. Must be called with the mutex held.
+ void Reschedule() {
+ cycles_elapsed_ = phased_loop_.Iterate(shm_event_loop_->monotonic_now());
+ timerfd_.SetTime(phased_loop_.sleep_time(), ::aos::monotonic_clock::zero());
+ }
+
+ ShmEventLoop *shm_event_loop_;
+
+ TimerFd timerfd_;
+ time::PhasedLoop phased_loop_;
+
+ int cycles_elapsed_ = 1;
+
+ // Function to be run
+ const ::std::function<void(int)> fn_;
+};
+} // namespace internal
+
+::std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
+ const Channel *channel) {
+ return ::std::unique_ptr<RawFetcher>(new ShmFetcher(channel));
+}
+
+::std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
+ const Channel *channel) {
+ Take(channel);
+ return ::std::unique_ptr<RawSender>(new ShmSender(channel, this));
+}
+
+void ShmEventLoop::MakeRawWatcher(
+ const Channel *channel,
+ std::function<void(const Context &context, const void *message)> watcher) {
+ Take(channel);
+
+ ::std::unique_ptr<internal::WatcherState> state(
+ new internal::WatcherState(
+ channel, std::move(watcher)));
+ watchers_.push_back(::std::move(state));
+}
+
+TimerHandler *ShmEventLoop::AddTimer(::std::function<void()> callback) {
+ ::std::unique_ptr<internal::TimerHandlerState> timer(
+ new internal::TimerHandlerState(this, ::std::move(callback)));
+
+ timers_.push_back(::std::move(timer));
+
+ return timers_.back().get();
+}
+
+PhasedLoopHandler *ShmEventLoop::AddPhasedLoop(
+ ::std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset) {
+ ::std::unique_ptr<internal::PhasedLoopHandler> phased_loop(
+ new internal::PhasedLoopHandler(this, ::std::move(callback), interval,
+ offset));
+
+ phased_loops_.push_back(::std::move(phased_loop));
+
+ return phased_loops_.back().get();
+}
+
+void ShmEventLoop::OnRun(::std::function<void()> on_run) {
+ on_run_.push_back(::std::move(on_run));
+}
+
+void ShmEventLoop::Run() {
+ std::unique_ptr<ipc_lib::SignalFd> signalfd;
+
+ if (watchers_.size() > 0) {
+ signalfd.reset(new ipc_lib::SignalFd({ipc_lib::kWakeupSignal}));
+
+ epoll_.OnReadable(signalfd->fd(), [signalfd_ptr = signalfd.get(), this]() {
+ signalfd_siginfo result = signalfd_ptr->Read();
+ CHECK_EQ(result.ssi_signo, ipc_lib::kWakeupSignal);
+
+ // TODO(austin): We should really be checking *everything*, not just
+ // watchers, and calling the oldest thing first. That will improve
+ // determinism a lot.
+
+ while (true) {
+ // Call the handlers in time order of their messages.
+ aos::monotonic_clock::time_point min_event_time =
+ aos::monotonic_clock::max_time;
+ size_t min_watcher_index = -1;
+ size_t watcher_index = 0;
+ for (::std::unique_ptr<internal::WatcherState> &watcher : watchers_) {
+ if (watcher->HasNewData()) {
+ if (watcher->event_time() < min_event_time) {
+ min_watcher_index = watcher_index;
+ min_event_time = watcher->event_time();
+ }
+ }
+ ++watcher_index;
+ }
+
+ if (min_event_time == aos::monotonic_clock::max_time) {
+ break;
+ }
+
+ watchers_[min_watcher_index]->CallCallback();
+ }
+ });
+ }
+
+ // Now, all the threads are up. Lock everything into memory and go RT.
+ if (priority_ != 0) {
+ ::aos::InitRT();
+
+ LOG(INFO) << "Setting priority to " << priority_;
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
+ }
+
+ set_is_running(true);
+
+ // Now that we are realtime (but before the OnRun handlers run), snap the
+ // queue index.
+ for (::std::unique_ptr<internal::WatcherState> &watcher : watchers_) {
+ watcher->PointAtNextQueueIndex();
+ CHECK(watcher->RegisterWakeup(priority_));
+ }
+
+ // Now that we are RT, run all the OnRun handlers.
+ for (const auto &run : on_run_) {
+ run();
+ }
+
+ // Start up all the phased loops.
+ for (::std::unique_ptr<internal::PhasedLoopHandler> &phased_loop :
+ phased_loops_) {
+ phased_loop->Startup();
+ }
+
+ // And start our main event loop which runs all the timers and handles Quit.
+ epoll_.Run();
+
+ // Once epoll exits, there is no useful nonrt work left to do.
+ set_is_running(false);
+
+ // Nothing time or synchronization critical needs to happen after this point.
+ // Drop RT priority.
+ ::aos::UnsetCurrentThreadRealtimePriority();
+
+ for (::std::unique_ptr<internal::WatcherState> &watcher : watchers_) {
+ watcher->UnregisterWakeup();
+ }
+
+ if (watchers_.size() > 0) {
+ epoll_.DeleteFd(signalfd->fd());
+ signalfd.reset();
+ }
+}
+
+void ShmEventLoop::Exit() { epoll_.Quit(); }
+
+ShmEventLoop::~ShmEventLoop() {
+ CHECK(!is_running()) << ": ShmEventLoop destroyed while running";
+}
+
+void ShmEventLoop::Take(const Channel *channel) {
+ CHECK(!is_running()) << ": Cannot add new objects while running.";
+
+ // Cheat aggresively. Use the shared memory path as a proxy for a unique
+ // identifier for the channel.
+ const std::string path = ShmPath(channel);
+
+ const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
+ CHECK(prior == taken_.end()) << ": " << path << " is already being used.";
+
+ taken_.emplace_back(path);
+}
+
+void ShmEventLoop::SetRuntimeRealtimePriority(int priority) {
+ if (is_running()) {
+ LOG(FATAL) << "Cannot set realtime priority while running.";
+ }
+ priority_ = priority;
+}
+
+} // namespace aos
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
new file mode 100644
index 0000000..e859201
--- /dev/null
+++ b/aos/events/shm_event_loop.h
@@ -0,0 +1,92 @@
+#ifndef AOS_EVENTS_SHM_EVENT_LOOP_H_
+#define AOS_EVENTS_SHM_EVENT_LOOP_H_
+
+#include <unordered_set>
+#include <vector>
+
+#include "aos/events/epoll.h"
+#include "aos/events/event_loop.h"
+#include "aos/ipc_lib/signalfd.h"
+#include "aos/mutex/mutex.h"
+
+namespace aos {
+namespace internal {
+
+class WatcherState;
+class TimerHandlerState;
+class PhasedLoopHandler;
+
+} // namespace internal
+
+// Specialization of EventLoop that is built from queues running out of shared
+// memory. See more details at aos/queue.h
+//
+// This object must be interacted with from one thread, but the Senders and
+// Fetchers may be used from multiple threads afterwords (as long as their
+// destructors are called back in one thread again)
+class ShmEventLoop : public EventLoop {
+ public:
+ ShmEventLoop(const Configuration *configuration);
+ ~ShmEventLoop() override;
+
+ aos::monotonic_clock::time_point monotonic_now() override {
+ return aos::monotonic_clock::now();
+ }
+ aos::realtime_clock::time_point realtime_now() override {
+ return aos::realtime_clock::now();
+ }
+
+ std::unique_ptr<RawSender> MakeRawSender(const Channel *channel) override;
+ std::unique_ptr<RawFetcher> MakeRawFetcher(const Channel *channel) override;
+
+ void MakeRawWatcher(
+ const Channel *channel,
+ std::function<void(const Context &context, const void *message)> watcher)
+ override;
+
+ TimerHandler *AddTimer(std::function<void()> callback) override;
+ aos::PhasedLoopHandler *AddPhasedLoop(
+ std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset =
+ std::chrono::seconds(0)) override;
+
+ void OnRun(std::function<void()> on_run) override;
+ void Run();
+ void Exit();
+
+ // TODO(austin): Add a function to register control-C call.
+
+ void SetRuntimeRealtimePriority(int priority) override;
+
+ void set_name(const absl::string_view name) override {
+ name_ = std::string(name);
+ }
+ const absl::string_view name() const override { return name_; }
+
+ int priority() const { return priority_; }
+
+ private:
+ friend class internal::WatcherState;
+ friend class internal::TimerHandlerState;
+ friend class internal::PhasedLoopHandler;
+
+ // Tracks that we can't have multiple watchers or a sender and a watcher (or
+ // multiple senders) on a single queue (path).
+ void Take(const Channel *channel);
+
+ std::vector<std::function<void()>> on_run_;
+ int priority_ = 0;
+ std::string name_;
+ std::vector<std::string> taken_;
+
+ internal::EPoll epoll_;
+
+ std::vector<std::unique_ptr<internal::TimerHandlerState>> timers_;
+ std::vector<std::unique_ptr<internal::PhasedLoopHandler>> phased_loops_;
+ std::vector<std::unique_ptr<internal::WatcherState>> watchers_;
+};
+
+} // namespace aos
+
+#endif // AOS_EVENTS_SHM_EVENT_LOOP_H_
diff --git a/aos/events/shm-event-loop_test.cc b/aos/events/shm_event_loop_test.cc
similarity index 72%
rename from aos/events/shm-event-loop_test.cc
rename to aos/events/shm_event_loop_test.cc
index e0d0b49..4406e01 100644
--- a/aos/events/shm-event-loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -1,9 +1,13 @@
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
-#include "aos/events/event-loop_param_test.h"
-#include "aos/testing/test_shm.h"
+#include "aos/events/event_loop_param_test.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
+#include "aos/events/test_message_generated.h"
+
+DECLARE_string(shm_base);
+
namespace aos {
namespace testing {
namespace {
@@ -11,28 +15,40 @@
class ShmEventLoopTestFactory : public EventLoopTestFactory {
public:
+ ShmEventLoopTestFactory() {
+ // Put all the queue files in ${TEST_TMPDIR} if it is set, otherwise
+ // everything will be reusing /dev/shm when sharded.
+ char *test_tmpdir = getenv("TEST_TMPDIR");
+ if (test_tmpdir != nullptr) {
+ FLAGS_shm_base = std::string(test_tmpdir) + "/aos";
+ }
+
+ // Clean up anything left there before.
+ unlink((FLAGS_shm_base + "test/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "test1/aos.TestMessage.v0").c_str());
+ unlink((FLAGS_shm_base + "test2/aos.TestMessage.v0").c_str());
+ }
+
::std::unique_ptr<EventLoop> Make() override {
- return ::std::unique_ptr<EventLoop>(new ShmEventLoop());
+ return ::std::unique_ptr<EventLoop>(new ShmEventLoop(configuration()));
}
::std::unique_ptr<EventLoop> MakePrimary() override {
::std::unique_ptr<ShmEventLoop> loop =
- ::std::unique_ptr<ShmEventLoop>(new ShmEventLoop());
+ ::std::unique_ptr<ShmEventLoop>(new ShmEventLoop(configuration()));
primary_event_loop_ = loop.get();
return ::std::move(loop);
}
- void Run() override { AOS_CHECK_NOTNULL(primary_event_loop_)->Run(); }
+ void Run() override { CHECK_NOTNULL(primary_event_loop_)->Run(); }
- void Exit() override { AOS_CHECK_NOTNULL(primary_event_loop_)->Exit(); }
+ void Exit() override { CHECK_NOTNULL(primary_event_loop_)->Exit(); }
void SleepFor(::std::chrono::nanoseconds duration) override {
::std::this_thread::sleep_for(duration);
}
private:
- ::aos::testing::TestSharedMemory my_shm_;
-
::aos::ShmEventLoop *primary_event_loop_;
};
@@ -46,24 +62,13 @@
return new ShmEventLoopTestFactory();
}));
-struct TestMessage : public ::aos::Message {
- enum { kQueueLength = 100, kHash = 0x696c0cdc };
- int msg_value;
-
- void Zero() { msg_value = 0; }
- static size_t Size() { return 1 + ::aos::Message::Size(); }
- size_t Print(char *buffer, size_t length) const;
- TestMessage() { Zero(); }
-};
-
} // namespace
bool IsRealtime() {
int scheduler;
- if ((scheduler = sched_getscheduler(0)) == -1) {
- AOS_PLOG(FATAL, "sched_getscheduler(0) failed\n");
- }
- AOS_LOG(INFO, "scheduler is %d\n", scheduler);
+ PCHECK((scheduler = sched_getscheduler(0)) != -1);
+
+ LOG(INFO) << "scheduler is " << scheduler;
return scheduler == SCHED_FIFO || scheduler == SCHED_RR;
}
@@ -82,7 +87,7 @@
bool did_timer = false;
bool did_watcher = false;
- auto timer = loop->AddTimer([&did_timer, &loop, &factory]() {
+ auto timer = loop->AddTimer([&did_timer, &factory]() {
EXPECT_TRUE(IsRealtime());
did_timer = true;
factory.Exit();
@@ -97,9 +102,11 @@
EXPECT_TRUE(IsRealtime());
did_onrun = true;
timer->Setup(loop->monotonic_now() + chrono::milliseconds(100));
- auto msg = sender.MakeMessage();
- msg->msg_value = 200;
- msg.Send();
+
+ aos::Sender<TestMessage>::Builder msg = sender.MakeBuilder();
+ TestMessage::Builder builder = msg.MakeBuilder<TestMessage>();
+ builder.add_value(200);
+ msg.Send(builder.Finish());
});
factory.Run();
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
deleted file mode 100644
index 85ea21b..0000000
--- a/aos/events/simulated-event-loop.cc
+++ /dev/null
@@ -1,402 +0,0 @@
-#include "aos/events/simulated-event-loop.h"
-
-#include <algorithm>
-#include <deque>
-
-#include "aos/logging/logging.h"
-#include "aos/queue.h"
-#include "aos/testing/test_logging.h"
-#include "aos/util/phased_loop.h"
-
-namespace aos {
-namespace {
-
-class SimulatedSender : public RawSender {
- public:
- SimulatedSender(SimulatedQueue *queue, EventLoop *event_loop)
- : queue_(queue), event_loop_(event_loop) {
- testing::EnableTestLogging();
- }
- ~SimulatedSender() {}
-
- aos::Message *GetMessage() override {
- return RefCountedBuffer(queue_->size()).release();
- }
-
- void Free(aos::Message *msg) override { RefCountedBuffer tmp(msg); }
-
- bool Send(aos::Message *msg) override {
- {
- if (msg->sent_time == monotonic_clock::min_time) {
- msg->sent_time = event_loop_->monotonic_now();
- }
- }
- queue_->Send(RefCountedBuffer(msg));
- return true;
- }
-
- const char *name() const override { return queue_->name(); }
-
- private:
- SimulatedQueue *queue_;
- EventLoop *event_loop_;
-};
-} // namespace
-
-class SimulatedFetcher : public RawFetcher {
- public:
- explicit SimulatedFetcher(SimulatedQueue *queue) : queue_(queue) {}
- ~SimulatedFetcher() { queue_->UnregisterFetcher(this); }
-
- bool FetchNext() override {
- if (msgs_.size() == 0) return false;
-
- msg_ = msgs_.front();
- msgs_.pop_front();
- set_most_recent(msg_.get());
- return true;
- }
-
- bool Fetch() override {
- if (msgs_.size() == 0) {
- if (!msg_ && queue_->latest_message()) {
- msg_ = queue_->latest_message();
- set_most_recent(msg_.get());
- return true;
- } else {
- return false;
- }
- }
-
- // We've had a message enqueued, so we don't need to go looking for the
- // latest message from before we started.
- msg_ = msgs_.back();
- msgs_.clear();
- set_most_recent(msg_.get());
- return true;
- }
-
- private:
- friend class SimulatedQueue;
-
- // Internal method for Simulation to add a message to the buffer.
- void Enqueue(RefCountedBuffer buffer) {
- msgs_.emplace_back(buffer);
- }
-
- SimulatedQueue *queue_;
- RefCountedBuffer msg_;
-
- // Messages queued up but not in use.
- ::std::deque<RefCountedBuffer> msgs_;
-};
-
-class SimulatedTimerHandler : public TimerHandler {
- public:
- explicit SimulatedTimerHandler(EventScheduler *scheduler,
- ::std::function<void()> fn)
- : scheduler_(scheduler), fn_(fn) {}
- ~SimulatedTimerHandler() {}
-
- void Setup(monotonic_clock::time_point base,
- monotonic_clock::duration repeat_offset) override {
- Disable();
- const ::aos::monotonic_clock::time_point monotonic_now =
- scheduler_->monotonic_now();
- base_ = base;
- repeat_offset_ = repeat_offset;
- if (base < monotonic_now) {
- token_ = scheduler_->Schedule(monotonic_now, [this]() { HandleEvent(); });
- } else {
- token_ = scheduler_->Schedule(base, [this]() { HandleEvent(); });
- }
- }
-
- void HandleEvent() {
- const ::aos::monotonic_clock::time_point monotonic_now =
- scheduler_->monotonic_now();
- if (repeat_offset_ != ::aos::monotonic_clock::zero()) {
- // Reschedule.
- while (base_ <= monotonic_now) base_ += repeat_offset_;
- token_ = scheduler_->Schedule(base_, [this]() { HandleEvent(); });
- } else {
- token_ = EventScheduler::Token();
- }
- fn_();
- }
-
- void Disable() override {
- if (token_ != EventScheduler::Token()) {
- scheduler_->Deschedule(token_);
- token_ = EventScheduler::Token();
- }
- }
-
- ::aos::monotonic_clock::time_point monotonic_now() const {
- return scheduler_->monotonic_now();
- }
-
- private:
- EventScheduler *scheduler_;
- EventScheduler::Token token_;
- // Function to be run on the thread
- ::std::function<void()> fn_;
- monotonic_clock::time_point base_;
- monotonic_clock::duration repeat_offset_;
-};
-
-class SimulatedPhasedLoopHandler : public PhasedLoopHandler {
- public:
- SimulatedPhasedLoopHandler(EventScheduler *scheduler,
- ::std::function<void(int)> fn,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset)
- : simulated_timer_handler_(scheduler, [this]() { HandleTimerWakeup(); }),
- phased_loop_(interval, simulated_timer_handler_.monotonic_now(),
- offset),
- fn_(fn) {
- // TODO(austin): This assumes time doesn't change between when the
- // constructor is called and when we start running. It's probably a safe
- // assumption.
- Reschedule();
- }
-
- void HandleTimerWakeup() {
- fn_(cycles_elapsed_);
- Reschedule();
- }
-
- void set_interval_and_offset(
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) override {
- phased_loop_.set_interval_and_offset(interval, offset);
- }
-
- void Reschedule() {
- cycles_elapsed_ =
- phased_loop_.Iterate(simulated_timer_handler_.monotonic_now());
- simulated_timer_handler_.Setup(phased_loop_.sleep_time(),
- ::aos::monotonic_clock::zero());
- }
-
- private:
- SimulatedTimerHandler simulated_timer_handler_;
-
- time::PhasedLoop phased_loop_;
-
- int cycles_elapsed_ = 1;
-
- ::std::function<void(int)> fn_;
-};
-
-class SimulatedEventLoop : public EventLoop {
- public:
- explicit SimulatedEventLoop(
- EventScheduler *scheduler,
- ::std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue>
- *queues)
- : scheduler_(scheduler), queues_(queues) {
- scheduler_->AddRawEventLoop(this);
- }
- ~SimulatedEventLoop() override { scheduler_->RemoveRawEventLoop(this); };
-
- ::aos::monotonic_clock::time_point monotonic_now() override {
- return scheduler_->monotonic_now();
- }
-
- ::std::unique_ptr<RawSender> MakeRawSender(
- const ::std::string &path, const QueueTypeInfo &type) override;
-
- ::std::unique_ptr<RawFetcher> MakeRawFetcher(
- const ::std::string &path, const QueueTypeInfo &type) override;
-
- void MakeRawWatcher(
- const ::std::string &path, const QueueTypeInfo &type,
- ::std::function<void(const ::aos::Message *message)> watcher) override;
-
- TimerHandler *AddTimer(::std::function<void()> callback) override {
- timers_.emplace_back(new SimulatedTimerHandler(scheduler_, callback));
- return timers_.back().get();
- }
-
- PhasedLoopHandler *AddPhasedLoop(::std::function<void(int)> callback,
- const monotonic_clock::duration interval,
- const monotonic_clock::duration offset =
- ::std::chrono::seconds(0)) override {
- phased_loops_.emplace_back(
- new SimulatedPhasedLoopHandler(scheduler_, callback, interval, offset));
- return phased_loops_.back().get();
- }
-
- void OnRun(::std::function<void()> on_run) override {
- scheduler_->Schedule(scheduler_->monotonic_now(), on_run);
- }
-
- void set_name(const char *name) override { name_ = name; }
-
- SimulatedQueue *GetSimulatedQueue(
- const ::std::pair<::std::string, QueueTypeInfo> &);
-
- void Take(const ::std::string &path);
-
- void SetRuntimeRealtimePriority(int /*priority*/) override {
- if (is_running()) {
- ::aos::Die("Cannot set realtime priority while running.");
- }
- }
-
- private:
- EventScheduler *scheduler_;
- ::std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue>
- *queues_;
- ::std::vector<std::string> taken_;
- ::std::vector<std::unique_ptr<TimerHandler>> timers_;
- ::std::vector<std::unique_ptr<PhasedLoopHandler>> phased_loops_;
-
- ::std::string name_;
-};
-
-EventScheduler::Token EventScheduler::Schedule(
- ::aos::monotonic_clock::time_point time, ::std::function<void()> callback) {
- return events_list_.emplace(time, callback);
-}
-
-void EventScheduler::Deschedule(EventScheduler::Token token) {
- events_list_.erase(token);
-}
-
-void EventScheduler::RunFor(monotonic_clock::duration duration) {
- const ::aos::monotonic_clock::time_point end_time =
- monotonic_now() + duration;
- testing::MockTime(monotonic_now());
- for (RawEventLoop *event_loop : raw_event_loops_) {
- event_loop->set_is_running(true);
- }
- is_running_ = true;
- while (!events_list_.empty() && is_running_) {
- auto iter = events_list_.begin();
- ::aos::monotonic_clock::time_point next_time = iter->first;
- if (next_time > end_time) {
- break;
- }
- now_ = iter->first;
- testing::MockTime(now_);
- ::std::function<void()> callback = ::std::move(iter->second);
- events_list_.erase(iter);
- callback();
- }
- now_ = end_time;
- if (!is_running_) {
- for (RawEventLoop *event_loop : raw_event_loops_) {
- event_loop->set_is_running(false);
- }
- }
- testing::UnMockTime();
-}
-
-void EventScheduler::Run() {
- testing::MockTime(monotonic_now());
- for (RawEventLoop *event_loop : raw_event_loops_) {
- event_loop->set_is_running(true);
- }
- is_running_ = true;
- while (!events_list_.empty() && is_running_) {
- auto iter = events_list_.begin();
- now_ = iter->first;
- testing::MockTime(now_);
- ::std::function<void()> callback = ::std::move(iter->second);
- events_list_.erase(iter);
- callback();
- }
- if (!is_running_) {
- for (RawEventLoop *event_loop : raw_event_loops_) {
- event_loop->set_is_running(false);
- }
- }
- testing::UnMockTime();
-}
-
-void SimulatedEventLoop::MakeRawWatcher(
- const std::string &path, const QueueTypeInfo &type,
- std::function<void(const aos::Message *message)> watcher) {
- Take(path);
- ::std::pair<::std::string, QueueTypeInfo> key(path, type);
- GetSimulatedQueue(key)->MakeRawWatcher(watcher);
-}
-
-std::unique_ptr<RawSender> SimulatedEventLoop::MakeRawSender(
- const std::string &path, const QueueTypeInfo &type) {
- Take(path);
- ::std::pair<::std::string, QueueTypeInfo> key(path, type);
- return GetSimulatedQueue(key)->MakeRawSender(this);
-}
-
-std::unique_ptr<RawFetcher> SimulatedEventLoop::MakeRawFetcher(
- const std::string &path, const QueueTypeInfo &type) {
- ::std::pair<::std::string, QueueTypeInfo> key(path, type);
- return GetSimulatedQueue(key)->MakeRawFetcher();
-}
-
-SimulatedQueue *SimulatedEventLoop::GetSimulatedQueue(
- const ::std::pair<::std::string, QueueTypeInfo> &type) {
- auto it = queues_->find(type);
- if (it == queues_->end()) {
- it =
- queues_
- ->emplace(type, SimulatedQueue(type.second, type.first, scheduler_))
- .first;
- }
- return &it->second;
-}
-
-void SimulatedQueue::MakeRawWatcher(
- ::std::function<void(const aos::Message *message)> watcher) {
- watchers_.push_back(watcher);
-}
-
-::std::unique_ptr<RawSender> SimulatedQueue::MakeRawSender(
- EventLoop *event_loop) {
- return ::std::unique_ptr<RawSender>(new SimulatedSender(this, event_loop));
-}
-
-::std::unique_ptr<RawFetcher> SimulatedQueue::MakeRawFetcher() {
- ::std::unique_ptr<SimulatedFetcher> fetcher(new SimulatedFetcher(this));
- fetchers_.push_back(fetcher.get());
- return ::std::move(fetcher);
-}
-
-void SimulatedQueue::Send(RefCountedBuffer message) {
- latest_message_ = message;
- if (scheduler_->is_running()) {
- for (auto &watcher : watchers_) {
- scheduler_->Schedule(scheduler_->monotonic_now(),
- [watcher, message]() { watcher(message.get()); });
- }
- }
- for (auto &fetcher : fetchers_) {
- fetcher->Enqueue(message);
- }
-}
-
-void SimulatedQueue::UnregisterFetcher(SimulatedFetcher *fetcher) {
- fetchers_.erase(::std::find(fetchers_.begin(), fetchers_.end(), fetcher));
-}
-
-void SimulatedEventLoop::Take(const ::std::string &path) {
- if (is_running()) {
- ::aos::Die("Cannot add new objects while running.\n");
- }
- const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
- if (prior != taken_.end()) {
- ::aos::Die("%s is already being used.", path.c_str());
- } else {
- taken_.emplace_back(path);
- }
-}
-
-::std::unique_ptr<EventLoop> SimulatedEventLoopFactory::MakeEventLoop() {
- return ::std::unique_ptr<EventLoop>(
- new SimulatedEventLoop(&scheduler_, &queues_));
-}
-
-} // namespace aos
diff --git a/aos/events/simulated-event-loop.h b/aos/events/simulated-event-loop.h
deleted file mode 100644
index 2fcc9d1..0000000
--- a/aos/events/simulated-event-loop.h
+++ /dev/null
@@ -1,214 +0,0 @@
-#ifndef _AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
-#define _AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
-
-#include <algorithm>
-#include <map>
-#include <memory>
-#include <unordered_set>
-#include <utility>
-#include <vector>
-
-#include "aos/events/event-loop.h"
-
-namespace aos {
-
-// This class manages allocation of queue messages for simulation.
-// Unfortunately, because the current interfaces all assume that we pass around
-// raw pointers to messages we can't use a std::shared_ptr or the such, and
-// because aos::Message's themselves to not have any sort of built-in support
-// for this, we need to manage memory for the Messages in some custom fashion.
-// In this case, we do so by allocating a ref-counter in the bytes immediately
-// preceding the aos::Message. We then provide a constructor that takes just a
-// pointer to an existing message and we assume that it was allocated using this
-// class, and can decrement the counter if the RefCountedBuffer we constructed
-// goes out of scope. There are currently no checks to ensure that pointers
-// passed into this class were actually allocated using this class.
-class RefCountedBuffer {
- public:
- RefCountedBuffer() {}
- ~RefCountedBuffer() { clear(); }
-
- // Create a RefCountedBuffer for some Message that was already allocated using
- // a RefCountedBuffer class. This, or some function like it, is required to
- // allow us to let users of the simulated event loops work with raw pointers
- // to messages.
- explicit RefCountedBuffer(aos::Message *data) : data_(data) {}
-
- // Allocates memory for a new message of a given size. Does not initialize the
- // memory or call any constructors.
- explicit RefCountedBuffer(size_t size) {
- data_ = reinterpret_cast<uint8_t *>(malloc(kRefCountSize + size)) +
- kRefCountSize;
- // Initialize the allocated memory with an integer
- *GetRefCount() = 1;
- }
-
- RefCountedBuffer(const RefCountedBuffer &other) {
- data_ = other.data_;
- if (data_ != nullptr) {
- ++*GetRefCount();
- }
- }
-
- RefCountedBuffer(RefCountedBuffer &&other) { std::swap(data_, other.data_); }
-
- RefCountedBuffer &operator=(const RefCountedBuffer &other) {
- if (this == &other) return *this;
- clear();
- data_ = other.data_;
- ++*GetRefCount();
- return *this;
- }
-
- RefCountedBuffer &operator=(RefCountedBuffer &&other) {
- if (this == &other) return *this;
- std::swap(data_, other.data_);
- return *this;
- }
-
- operator bool() const { return data_ != nullptr; }
-
- aos::Message *get() const { return static_cast<aos::Message *>(data_); }
-
- aos::Message *release() {
- auto tmp = get();
- data_ = nullptr;
- return tmp;
- }
-
- void clear() {
- if (data_ != nullptr) {
- if (--*GetRefCount() == 0) {
- // Free memory block from the start of the allocated block
- free(GetRefCount());
- }
- data_ = nullptr;
- }
- }
-
- private:
- void *data_ = nullptr;
- // Qty. memory to be allocated to the ref counter
- static constexpr size_t kRefCountSize = sizeof(int64_t);
-
- int64_t *GetRefCount() {
- // Need to cast the void* to an 8 bit long object (size of void* is
- // technically 0)
- return reinterpret_cast<int64_t *>(static_cast<void *>(
- reinterpret_cast<uint8_t *>(data_) - kRefCountSize));
- }
-};
-
-class EventScheduler {
- public:
- using QueueType = ::std::multimap<::aos::monotonic_clock::time_point,
- ::std::function<void()>>;
- using Token = QueueType::iterator;
-
- // Schedule an event with a callback function
- // Returns an iterator to the event
- Token Schedule(::aos::monotonic_clock::time_point time,
- ::std::function<void()> callback);
-
- // Deschedule an event by its iterator
- void Deschedule(Token token);
-
- void Run();
- void RunFor(::aos::monotonic_clock::duration duration);
-
- void Exit() {
- is_running_ = false;
- }
-
- bool is_running() const { return is_running_; }
-
- void AddRawEventLoop(RawEventLoop *event_loop) {
- raw_event_loops_.push_back(event_loop);
- }
- void RemoveRawEventLoop(RawEventLoop *event_loop) {
- raw_event_loops_.erase(::std::find(raw_event_loops_.begin(),
- raw_event_loops_.end(), event_loop));
- }
-
- ::aos::monotonic_clock::time_point monotonic_now() const { return now_; }
-
- private:
- ::aos::monotonic_clock::time_point now_ = ::aos::monotonic_clock::epoch();
- QueueType events_list_;
- bool is_running_ = false;
- ::std::vector<RawEventLoop *> raw_event_loops_;
-};
-
-// Class for simulated fetchers.
-class SimulatedFetcher;
-
-class SimulatedQueue {
- public:
- explicit SimulatedQueue(const QueueTypeInfo &type, const ::std::string &name,
- EventScheduler *scheduler)
- : type_(type), name_(name), scheduler_(scheduler){};
-
- ~SimulatedQueue() { AOS_CHECK_EQ(0u, fetchers_.size()); }
-
- // Makes a connected raw sender which calls Send below.
- ::std::unique_ptr<RawSender> MakeRawSender(EventLoop *event_loop);
-
- // Makes a connected raw fetcher.
- ::std::unique_ptr<RawFetcher> MakeRawFetcher();
-
- // Registers a watcher for the queue.
- void MakeRawWatcher(
- ::std::function<void(const ::aos::Message *message)> watcher);
-
- // Sends the message to all the connected receivers and fetchers.
- void Send(RefCountedBuffer message);
-
- // Unregisters a fetcher.
- void UnregisterFetcher(SimulatedFetcher *fetcher);
-
- const RefCountedBuffer &latest_message() { return latest_message_; }
-
- size_t size() const { return type_.size; }
-
- const char *name() const { return name_.c_str(); }
-
- private:
- const QueueTypeInfo type_;
- const ::std::string name_;
-
- // List of all watchers.
- ::std::vector<std::function<void(const aos::Message *message)>> watchers_;
-
- // List of all fetchers.
- ::std::vector<SimulatedFetcher *> fetchers_;
- RefCountedBuffer latest_message_;
- EventScheduler *scheduler_;
-};
-
-class SimulatedEventLoopFactory {
- public:
- ::std::unique_ptr<EventLoop> MakeEventLoop();
-
- // Starts executing the event loops unconditionally.
- void Run() { scheduler_.Run(); }
- // Executes the event loops for a duration.
- void RunFor(monotonic_clock::duration duration) {
- scheduler_.RunFor(duration);
- }
-
- // Stops executing all event loops. Meant to be called from within an event
- // loop handler.
- void Exit() { scheduler_.Exit(); }
-
- monotonic_clock::time_point monotonic_now() const {
- return scheduler_.monotonic_now();
- }
-
- private:
- EventScheduler scheduler_;
- ::std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue> queues_;
-};
-
-} // namespace aos
-
-#endif //_AOS_EVENTS_TEST_EVENT_LOOP_H_
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
new file mode 100644
index 0000000..9bc74d5
--- /dev/null
+++ b/aos/events/simulated_event_loop.cc
@@ -0,0 +1,514 @@
+#include "aos/events/simulated_event_loop.h"
+
+#include <algorithm>
+#include <deque>
+
+#include "absl/container/btree_map.h"
+#include "absl/container/btree_set.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/phased_loop.h"
+
+namespace aos {
+
+// Container for both a message, and the context for it for simulation. This
+// makes tracking the timestamps associated with the data easy.
+struct SimulatedMessage {
+ // Struct to let us force data to be well aligned.
+ struct OveralignedChar {
+ char data alignas(32);
+ };
+
+ // Context for the data.
+ Context context;
+
+ // The data.
+ char *data() { return reinterpret_cast<char *>(&actual_data[0]); }
+
+ // Then the data.
+ OveralignedChar actual_data[];
+};
+
+class SimulatedFetcher;
+
+class SimulatedChannel {
+ public:
+ explicit SimulatedChannel(const Channel *channel, EventScheduler *scheduler)
+ : channel_(CopyFlatBuffer(channel)),
+ scheduler_(scheduler),
+ next_queue_index_(ipc_lib::QueueIndex::Zero(channel->max_size())) {}
+
+ ~SimulatedChannel() { CHECK_EQ(0u, fetchers_.size()); }
+
+ // Makes a connected raw sender which calls Send below.
+ ::std::unique_ptr<RawSender> MakeRawSender(EventLoop *event_loop);
+
+ // Makes a connected raw fetcher.
+ ::std::unique_ptr<RawFetcher> MakeRawFetcher();
+
+ // Registers a watcher for the queue.
+ void MakeRawWatcher(
+ ::std::function<void(const Context &context, const void *message)>
+ watcher);
+
+ // Sends the message to all the connected receivers and fetchers.
+ void Send(std::shared_ptr<SimulatedMessage> message);
+
+ // Unregisters a fetcher.
+ void UnregisterFetcher(SimulatedFetcher *fetcher);
+
+ std::shared_ptr<SimulatedMessage> latest_message() { return latest_message_; }
+
+ size_t max_size() const { return channel_.message().max_size(); }
+
+ const absl::string_view name() const {
+ return channel_.message().name()->string_view();
+ }
+
+ const Channel *channel() const { return &channel_.message(); }
+
+ private:
+ const FlatbufferDetachedBuffer<Channel> channel_;
+
+ // List of all watchers.
+ ::std::vector<
+ std::function<void(const Context &context, const void *message)>>
+ watchers_;
+
+ // List of all fetchers.
+ ::std::vector<SimulatedFetcher *> fetchers_;
+ std::shared_ptr<SimulatedMessage> latest_message_;
+ EventScheduler *scheduler_;
+
+ ipc_lib::QueueIndex next_queue_index_;
+};
+
+namespace {
+
+// Creates a SimulatedMessage with size bytes of storage.
+// This is a shared_ptr so we don't have to implement refcounting or copying.
+std::shared_ptr<SimulatedMessage> MakeSimulatedMessage(size_t size) {
+ SimulatedMessage *message = reinterpret_cast<SimulatedMessage *>(
+ malloc(sizeof(SimulatedMessage) + size));
+ message->context.size = size;
+ message->context.data = message->data();
+
+ return std::shared_ptr<SimulatedMessage>(message, free);
+}
+
+class SimulatedSender : public RawSender {
+ public:
+ SimulatedSender(SimulatedChannel *simulated_channel, EventLoop *event_loop)
+ : simulated_channel_(simulated_channel), event_loop_(event_loop) {}
+ ~SimulatedSender() {}
+
+ void *data() override {
+ if (!message_) {
+ message_ = MakeSimulatedMessage(simulated_channel_->max_size());
+ }
+ return message_->data();
+ }
+
+ size_t size() override { return simulated_channel_->max_size(); }
+
+ bool Send(size_t length) override {
+ CHECK_LE(length, size()) << ": Attempting to send too big a message.";
+ message_->context.monotonic_sent_time = event_loop_->monotonic_now();
+ message_->context.realtime_sent_time = event_loop_->realtime_now();
+ CHECK_LE(length, message_->context.size);
+ message_->context.size = length;
+
+ // TODO(austin): Track sending too fast.
+ simulated_channel_->Send(message_);
+
+ // Drop the reference to the message so that we allocate a new message for
+ // next time. Otherwise we will continue to reuse the same memory for all
+ // messages and corrupt it.
+ message_.reset();
+ return true;
+ }
+
+ bool Send(void *msg, size_t size) override {
+ CHECK_LE(size, this->size()) << ": Attempting to send too big a message.";
+
+ // This is wasteful, but since flatbuffers fill from the back end of the
+ // queue, we need it to be full sized.
+ message_ = MakeSimulatedMessage(simulated_channel_->max_size());
+
+ // Now fill in the message. size is already populated above, and
+ // queue_index will be populated in queue_. Put this at the back of the
+ // data segment.
+ memcpy(message_->data() + simulated_channel_->max_size() - size, msg, size);
+
+ return Send(size);
+ }
+
+ const absl::string_view name() const override {
+ return simulated_channel_->name();
+ }
+
+ private:
+ SimulatedChannel *simulated_channel_;
+ EventLoop *event_loop_;
+
+ std::shared_ptr<SimulatedMessage> message_;
+};
+} // namespace
+
+class SimulatedFetcher : public RawFetcher {
+ public:
+ explicit SimulatedFetcher(SimulatedChannel *queue) : queue_(queue) {}
+ ~SimulatedFetcher() { queue_->UnregisterFetcher(this); }
+
+ bool FetchNext() override {
+ if (msgs_.size() == 0) return false;
+
+ SetMsg(msgs_.front());
+ msgs_.pop_front();
+ return true;
+ }
+
+ bool Fetch() override {
+ if (msgs_.size() == 0) {
+ if (!msg_ && queue_->latest_message()) {
+ SetMsg(queue_->latest_message());
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // We've had a message enqueued, so we don't need to go looking for the
+ // latest message from before we started.
+ SetMsg(msgs_.back());
+ msgs_.clear();
+ return true;
+ }
+
+ private:
+ friend class SimulatedChannel;
+
+ // Updates the state inside RawFetcher to point to the data in msg_.
+ void SetMsg(std::shared_ptr<SimulatedMessage> msg) {
+ msg_ = msg;
+ data_ = msg_->context.data;
+ context_ = msg_->context;
+ }
+
+ // Internal method for Simulation to add a message to the buffer.
+ void Enqueue(std::shared_ptr<SimulatedMessage> buffer) {
+ msgs_.emplace_back(buffer);
+ }
+
+ SimulatedChannel *queue_;
+ std::shared_ptr<SimulatedMessage> msg_;
+
+ // Messages queued up but not in use.
+ ::std::deque<std::shared_ptr<SimulatedMessage>> msgs_;
+};
+
+class SimulatedTimerHandler : public TimerHandler {
+ public:
+ explicit SimulatedTimerHandler(EventScheduler *scheduler,
+ ::std::function<void()> fn)
+ : scheduler_(scheduler), token_(scheduler_->InvalidToken()), fn_(fn) {}
+ ~SimulatedTimerHandler() {}
+
+ void Setup(monotonic_clock::time_point base,
+ monotonic_clock::duration repeat_offset) override {
+ Disable();
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ scheduler_->monotonic_now();
+ base_ = base;
+ repeat_offset_ = repeat_offset;
+ if (base < monotonic_now) {
+ token_ = scheduler_->Schedule(monotonic_now, [this]() { HandleEvent(); });
+ } else {
+ token_ = scheduler_->Schedule(base, [this]() { HandleEvent(); });
+ }
+ }
+
+ void HandleEvent() {
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ scheduler_->monotonic_now();
+ if (repeat_offset_ != ::aos::monotonic_clock::zero()) {
+ // Reschedule.
+ while (base_ <= monotonic_now) base_ += repeat_offset_;
+ token_ = scheduler_->Schedule(base_, [this]() { HandleEvent(); });
+ } else {
+ token_ = scheduler_->InvalidToken();
+ }
+ fn_();
+ }
+
+ void Disable() override {
+ if (token_ != scheduler_->InvalidToken()) {
+ scheduler_->Deschedule(token_);
+ token_ = scheduler_->InvalidToken();
+ }
+ }
+
+ ::aos::monotonic_clock::time_point monotonic_now() const {
+ return scheduler_->monotonic_now();
+ }
+
+ private:
+ EventScheduler *scheduler_;
+ EventScheduler::Token token_;
+ // Function to be run on the thread
+ ::std::function<void()> fn_;
+ monotonic_clock::time_point base_;
+ monotonic_clock::duration repeat_offset_;
+};
+
+class SimulatedPhasedLoopHandler : public PhasedLoopHandler {
+ public:
+ SimulatedPhasedLoopHandler(EventScheduler *scheduler,
+ ::std::function<void(int)> fn,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset)
+ : simulated_timer_handler_(scheduler, [this]() { HandleTimerWakeup(); }),
+ phased_loop_(interval, simulated_timer_handler_.monotonic_now(),
+ offset),
+ fn_(fn) {
+ // TODO(austin): This assumes time doesn't change between when the
+ // constructor is called and when we start running. It's probably a safe
+ // assumption.
+ Reschedule();
+ }
+
+ void HandleTimerWakeup() {
+ fn_(cycles_elapsed_);
+ Reschedule();
+ }
+
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset) override {
+ phased_loop_.set_interval_and_offset(interval, offset);
+ }
+
+ void Reschedule() {
+ cycles_elapsed_ =
+ phased_loop_.Iterate(simulated_timer_handler_.monotonic_now());
+ simulated_timer_handler_.Setup(phased_loop_.sleep_time(),
+ ::aos::monotonic_clock::zero());
+ }
+
+ private:
+ SimulatedTimerHandler simulated_timer_handler_;
+
+ time::PhasedLoop phased_loop_;
+
+ int cycles_elapsed_ = 1;
+
+ ::std::function<void(int)> fn_;
+};
+
+class SimulatedEventLoop : public EventLoop {
+ public:
+ explicit SimulatedEventLoop(
+ EventScheduler *scheduler,
+ absl::btree_map<SimpleChannel, std::unique_ptr<SimulatedChannel>>
+ *channels,
+ const Configuration *configuration,
+ std::vector<std::pair<EventLoop *, std::function<void(bool)>>>
+ *raw_event_loops)
+ : EventLoop(configuration),
+ scheduler_(scheduler),
+ channels_(channels),
+ raw_event_loops_(raw_event_loops) {
+ raw_event_loops_->push_back(
+ std::make_pair(this, [this](bool value) { set_is_running(value); }));
+ }
+ ~SimulatedEventLoop() override {
+ for (auto it = raw_event_loops_->begin(); it != raw_event_loops_->end();
+ ++it) {
+ if (it->first == this) {
+ raw_event_loops_->erase(it);
+ break;
+ }
+ }
+ }
+
+ ::aos::monotonic_clock::time_point monotonic_now() override {
+ return scheduler_->monotonic_now();
+ }
+
+ ::aos::realtime_clock::time_point realtime_now() override {
+ return scheduler_->realtime_now();
+ }
+
+ ::std::unique_ptr<RawSender> MakeRawSender(const Channel *channel) override;
+
+ ::std::unique_ptr<RawFetcher> MakeRawFetcher(const Channel *channel) override;
+
+ void MakeRawWatcher(
+ const Channel *channel,
+ ::std::function<void(const Context &context, const void *message)>
+ watcher) override;
+
+ TimerHandler *AddTimer(::std::function<void()> callback) override {
+ timers_.emplace_back(new SimulatedTimerHandler(scheduler_, callback));
+ return timers_.back().get();
+ }
+
+ PhasedLoopHandler *AddPhasedLoop(::std::function<void(int)> callback,
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset =
+ ::std::chrono::seconds(0)) override {
+ phased_loops_.emplace_back(
+ new SimulatedPhasedLoopHandler(scheduler_, callback, interval, offset));
+ return phased_loops_.back().get();
+ }
+
+ void OnRun(::std::function<void()> on_run) override {
+ scheduler_->Schedule(scheduler_->monotonic_now(), on_run);
+ }
+
+ void set_name(const absl::string_view name) override {
+ name_ = std::string(name);
+ }
+ const absl::string_view name() const override { return name_; }
+
+ SimulatedChannel *GetSimulatedChannel(const Channel *channel);
+
+ void Take(const Channel *channel);
+
+ void SetRuntimeRealtimePriority(int /*priority*/) override {
+ CHECK(!is_running()) << ": Cannot set realtime priority while running.";
+ }
+
+ private:
+ EventScheduler *scheduler_;
+ absl::btree_map<SimpleChannel, std::unique_ptr<SimulatedChannel>> *channels_;
+ std::vector<std::pair<EventLoop *, std::function<void(bool)>>>
+ *raw_event_loops_;
+ absl::btree_set<SimpleChannel> taken_;
+ ::std::vector<std::unique_ptr<TimerHandler>> timers_;
+ ::std::vector<std::unique_ptr<PhasedLoopHandler>> phased_loops_;
+
+ ::std::string name_;
+};
+
+void SimulatedEventLoop::MakeRawWatcher(
+ const Channel *channel,
+ std::function<void(const Context &channel, const void *message)> watcher) {
+ Take(channel);
+ GetSimulatedChannel(channel)->MakeRawWatcher(watcher);
+}
+
+std::unique_ptr<RawSender> SimulatedEventLoop::MakeRawSender(
+ const Channel *channel) {
+ Take(channel);
+ return GetSimulatedChannel(channel)->MakeRawSender(this);
+}
+
+std::unique_ptr<RawFetcher> SimulatedEventLoop::MakeRawFetcher(
+ const Channel *channel) {
+ return GetSimulatedChannel(channel)->MakeRawFetcher();
+}
+
+SimulatedChannel *SimulatedEventLoop::GetSimulatedChannel(
+ const Channel *channel) {
+ auto it = channels_->find(SimpleChannel(channel));
+ if (it == channels_->end()) {
+ it = channels_
+ ->emplace(SimpleChannel(channel),
+ std::unique_ptr<SimulatedChannel>(
+ new SimulatedChannel(channel, scheduler_)))
+ .first;
+ }
+ return it->second.get();
+}
+
+void SimulatedChannel::MakeRawWatcher(
+ ::std::function<void(const Context &context, const void *message)>
+ watcher) {
+ watchers_.push_back(watcher);
+}
+
+::std::unique_ptr<RawSender> SimulatedChannel::MakeRawSender(
+ EventLoop *event_loop) {
+ return ::std::unique_ptr<RawSender>(new SimulatedSender(this, event_loop));
+}
+
+::std::unique_ptr<RawFetcher> SimulatedChannel::MakeRawFetcher() {
+ ::std::unique_ptr<SimulatedFetcher> fetcher(new SimulatedFetcher(this));
+ fetchers_.push_back(fetcher.get());
+ return ::std::move(fetcher);
+}
+
+void SimulatedChannel::Send(std::shared_ptr<SimulatedMessage> message) {
+ message->context.queue_index = next_queue_index_.index();
+ message->context.data =
+ message->data() + channel()->max_size() - message->context.size;
+ next_queue_index_ = next_queue_index_.Increment();
+
+ latest_message_ = message;
+ if (scheduler_->is_running()) {
+ for (auto &watcher : watchers_) {
+ scheduler_->Schedule(scheduler_->monotonic_now(), [watcher, message]() {
+ watcher(message->context, message->context.data);
+ });
+ }
+ }
+ for (auto &fetcher : fetchers_) {
+ fetcher->Enqueue(message);
+ }
+}
+
+void SimulatedChannel::UnregisterFetcher(SimulatedFetcher *fetcher) {
+ fetchers_.erase(::std::find(fetchers_.begin(), fetchers_.end(), fetcher));
+}
+
+SimpleChannel::SimpleChannel(const Channel *channel)
+ : name(CHECK_NOTNULL(CHECK_NOTNULL(channel)->name())->str()),
+ type(CHECK_NOTNULL(CHECK_NOTNULL(channel)->type())->str()) {}
+
+void SimulatedEventLoop::Take(const Channel *channel) {
+ CHECK(!is_running()) << ": Cannot add new objects while running.";
+
+ auto result = taken_.insert(SimpleChannel(channel));
+ CHECK(result.second) << ": " << FlatbufferToJson(channel)
+ << " is already being used.";
+}
+
+SimulatedEventLoopFactory::SimulatedEventLoopFactory(
+ const Configuration *configuration)
+ : configuration_(configuration) {}
+SimulatedEventLoopFactory::~SimulatedEventLoopFactory() {}
+
+::std::unique_ptr<EventLoop> SimulatedEventLoopFactory::MakeEventLoop() {
+ return ::std::unique_ptr<EventLoop>(new SimulatedEventLoop(
+ &scheduler_, &channels_, configuration_, &raw_event_loops_));
+}
+
+void SimulatedEventLoopFactory::RunFor(monotonic_clock::duration duration) {
+ for (const std::pair<EventLoop *, std::function<void(bool)>> &event_loop :
+ raw_event_loops_) {
+ event_loop.second(true);
+ }
+ scheduler_.RunFor(duration);
+ if (!scheduler_.is_running()) {
+ for (const std::pair<EventLoop *, std::function<void(bool)>> &event_loop :
+ raw_event_loops_) {
+ event_loop.second(false);
+ }
+ }
+}
+
+void SimulatedEventLoopFactory::Run() {
+ for (const std::pair<EventLoop *, std::function<void(bool)>> &event_loop :
+ raw_event_loops_) {
+ event_loop.second(true);
+ }
+ scheduler_.Run();
+ if (!scheduler_.is_running()) {
+ for (const std::pair<EventLoop *, std::function<void(bool)>> &event_loop :
+ raw_event_loops_) {
+ event_loop.second(false);
+ }
+ }
+}
+
+} // namespace aos
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
new file mode 100644
index 0000000..485ab68
--- /dev/null
+++ b/aos/events/simulated_event_loop.h
@@ -0,0 +1,86 @@
+#ifndef AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
+#define AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
+
+#include <algorithm>
+#include <map>
+#include <memory>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include "absl/container/btree_map.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/event_scheduler.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/flatbuffers.h"
+#include "aos/ipc_lib/index.h"
+#include "glog/logging.h"
+
+namespace aos {
+
+// Class for simulated fetchers.
+class SimulatedChannel;
+
+struct SimpleChannel {
+ SimpleChannel(const Channel *channel);
+ std::string name;
+ std::string type;
+
+ std::string DebugString() const {
+ return std::string("{ ") + name + ", " + type + "}";
+ }
+
+ bool operator==(const SimpleChannel &other) const {
+ return name == other.name && type == other.type;
+ }
+ bool operator<(const SimpleChannel &other) const {
+ int name_compare = other.name.compare(name);
+ if (name_compare == 0) {
+ return other.type < type;
+ } else if (name_compare < 0) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+};
+
+class SimulatedEventLoopFactory {
+ public:
+ // Constructs a SimulatedEventLoopFactory with the provided configuration.
+ // This configuration must remain in scope for the lifetime of the factory and
+ // all sub-objects.
+ SimulatedEventLoopFactory(const Configuration *configuration);
+ ~SimulatedEventLoopFactory();
+
+ ::std::unique_ptr<EventLoop> MakeEventLoop();
+
+ // Starts executing the event loops unconditionally.
+ void Run();
+ // Executes the event loops for a duration.
+ void RunFor(monotonic_clock::duration duration);
+
+ // Stops executing all event loops. Meant to be called from within an event
+ // loop handler.
+ void Exit() { scheduler_.Exit(); }
+
+ monotonic_clock::time_point monotonic_now() const {
+ return scheduler_.monotonic_now();
+ }
+ realtime_clock::time_point realtime_now() const {
+ return scheduler_.realtime_now();
+ }
+
+ private:
+ const Configuration *configuration_;
+ EventScheduler scheduler_;
+ // Map from name, type to queue.
+ absl::btree_map<SimpleChannel, std::unique_ptr<SimulatedChannel>> channels_;
+ // List of event loops to manage running and not running for.
+ std::vector<std::pair<EventLoop *, std::function<void(bool)>>>
+ raw_event_loops_;
+};
+
+} // namespace aos
+
+#endif // AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
diff --git a/aos/events/simulated-event-loop_test.cc b/aos/events/simulated_event_loop_test.cc
similarity index 90%
rename from aos/events/simulated-event-loop_test.cc
rename to aos/events/simulated_event_loop_test.cc
index e987a11..356c25b 100644
--- a/aos/events/simulated-event-loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -1,5 +1,6 @@
-#include "aos/events/simulated-event-loop.h"
-#include "aos/events/event-loop_param_test.h"
+#include "aos/events/simulated_event_loop.h"
+
+#include "aos/events/event_loop_param_test.h"
#include "gtest/gtest.h"
namespace aos {
@@ -9,6 +10,8 @@
class SimulatedEventLoopTestFactory : public EventLoopTestFactory {
public:
+ SimulatedEventLoopTestFactory() : event_loop_factory_(configuration()) {}
+
::std::unique_ptr<EventLoop> Make() override {
return event_loop_factory_.MakeEventLoop();
}
@@ -68,7 +71,7 @@
// Test that running for a time period with no handlers causes time to progress
// correctly.
TEST(SimulatedEventLoopTest, RunForNoHandlers) {
- SimulatedEventLoopFactory simulated_event_loop_factory;
+ SimulatedEventLoopFactory simulated_event_loop_factory(nullptr);
::std::unique_ptr<EventLoop> event_loop =
simulated_event_loop_factory.MakeEventLoop();
@@ -83,12 +86,12 @@
// Test that running for a time with a periodic handler causes time to end
// correctly.
TEST(SimulatedEventLoopTest, RunForTimerHandler) {
- SimulatedEventLoopFactory simulated_event_loop_factory;
+ SimulatedEventLoopFactory simulated_event_loop_factory(nullptr);
::std::unique_ptr<EventLoop> event_loop =
simulated_event_loop_factory.MakeEventLoop();
int counter = 0;
- auto timer = event_loop->AddTimer([&counter, &event_loop]() { ++counter; });
+ auto timer = event_loop->AddTimer([&counter]() { ++counter; });
event_loop->OnRun([&event_loop, &timer] {
timer->Setup(event_loop->monotonic_now() + chrono::milliseconds(50),
chrono::milliseconds(100));
diff --git a/aos/events/test_message.fbs b/aos/events/test_message.fbs
new file mode 100644
index 0000000..1d26c2e
--- /dev/null
+++ b/aos/events/test_message.fbs
@@ -0,0 +1,7 @@
+namespace aos;
+
+table TestMessage {
+ value:int;
+}
+
+root_type TestMessage;
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index 448f34b..fc91cf9 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -1,6 +1,9 @@
#ifndef AOS_FLATBUFFERS_H_
#define AOS_FLATBUFFERS_H_
+#include <array>
+
+#include "absl/strings/string_view.h"
#include "flatbuffers/flatbuffers.h"
namespace aos {
@@ -20,6 +23,8 @@
virtual uint8_t *data() = 0;
virtual size_t size() const = 0;
+ void Reset() { is_allocated_ = false; }
+
private:
bool is_allocated_ = false;
};
@@ -37,11 +42,27 @@
std::array<uint8_t, S> buffer_;
};
+// This class adapts a preallocated memory region to an Allocator.
+class PreallocatedAllocator : public FixedAllocatorBase {
+ public:
+ PreallocatedAllocator(void *data, size_t size) : data_(data), size_(size) {}
+ uint8_t *data() override { return reinterpret_cast<uint8_t *>(data_); }
+ const uint8_t *data() const override {
+ return reinterpret_cast<const uint8_t *>(data_);
+ }
+ size_t size() const override { return size_; }
+
+ private:
+ void* data_;
+ size_t size_;
+};
+
// Base class representing an object which holds the memory representing a root
// flatbuffer.
template <typename T>
class Flatbuffer {
public:
+ virtual ~Flatbuffer() {}
// Returns the MiniReflectTypeTable for T.
static const flatbuffers::TypeTable *MiniReflectTypeTable() {
return T::MiniReflectTypeTable();
@@ -82,6 +103,8 @@
return *this;
}
+ virtual ~FlatbufferArray() override {}
+
// Creates a builder wrapping the underlying data.
flatbuffers::FlatBufferBuilder FlatBufferBuilder() {
data_.deallocate(data_.data(), data_.size());
@@ -99,6 +122,39 @@
size_t size_ = data_.size();
};
+// String backed flatbuffer.
+template <typename T>
+class FlatbufferString : public Flatbuffer<T> {
+ public:
+ // Builds a flatbuffer using the contents of the string.
+ FlatbufferString(const absl::string_view data) : data_(data) {}
+ // Builds a Flatbuffer by copying the data from the other flatbuffer.
+ FlatbufferString(const Flatbuffer<T> &other) {
+ data_ = std::string(other.data(), other.size());
+ }
+
+ // Coppies the data from the other flatbuffer.
+ FlatbufferString &operator=(const Flatbuffer<T> &other) {
+ data_ = std::string(other.data(), other.size());
+ return *this;
+ }
+
+ virtual ~FlatbufferString() override {}
+
+ const uint8_t *data() const override {
+ return reinterpret_cast<const uint8_t *>(data_.data());
+ }
+ uint8_t *data() override {
+ // TODO(james): when we get c++17, can we drop the second cast?
+ return const_cast<uint8_t *>(
+ reinterpret_cast<const uint8_t *>(data_.data()));
+ }
+ size_t size() const override { return data_.size(); }
+
+ private:
+ std::string data_;
+};
+
// This object associates the message type with the memory storing the
// flatbuffer. This only stores root tables.
//
@@ -120,6 +176,8 @@
return *this;
}
+ virtual ~FlatbufferDetachedBuffer() override {}
+
// Constructs an empty flatbuffer of type T.
static FlatbufferDetachedBuffer<T> Empty() {
flatbuffers::FlatBufferBuilder fbb;
diff --git a/aos/init.cc b/aos/init.cc
index fdb1800..ac18413 100644
--- a/aos/init.cc
+++ b/aos/init.cc
@@ -14,8 +14,9 @@
#include <malloc.h>
#include "aos/die.h"
-#include "aos/logging/implementations.h"
#include "aos/ipc_lib/shared_mem.h"
+#include "aos/logging/implementations.h"
+#include "aos/realtime.h"
namespace FLAG__namespace_do_not_use_directly_use_DECLARE_double_instead {
extern double FLAGS_tcmalloc_release_rate __attribute__((weak));
@@ -34,25 +35,6 @@
} // namespace logging
namespace {
-void SetSoftRLimit(int resource, rlim64_t soft, bool set_for_root) {
- bool am_root = getuid() == 0;
- if (set_for_root || !am_root) {
- struct rlimit64 rlim;
- if (getrlimit64(resource, &rlim) == -1) {
- PDie("%s-init: getrlimit64(%d) failed",
- program_invocation_short_name, resource);
- }
- rlim.rlim_cur = soft;
- rlim.rlim_max = ::std::max(rlim.rlim_max, soft);
-
- if (setrlimit64(resource, &rlim) == -1) {
- PDie("%s-init: setrlimit64(%d, {cur=%ju,max=%ju}) failed",
- program_invocation_short_name, resource, (uintmax_t)rlim.rlim_cur,
- (uintmax_t)rlim.rlim_max);
- }
- }
-}
-
// Common stuff that needs to happen at the beginning of both the realtime and
// non-realtime initialization sequences. May be called twice.
void InitStart() {
@@ -68,35 +50,11 @@
} // namespace
-void LockAllMemory() {
- // Allow locking as much as we want into RAM.
- SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY, false);
-
- InitStart();
- if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
- PDie("%s-init: mlockall failed", program_invocation_short_name);
- }
-
- // Don't give freed memory back to the OS.
- AOS_CHECK_EQ(1, mallopt(M_TRIM_THRESHOLD, -1));
- // Don't use mmap for large malloc chunks.
- AOS_CHECK_EQ(1, mallopt(M_MMAP_MAX, 0));
-
- if (&FLAGS_tcmalloc_release_rate) {
- // Tell tcmalloc not to return memory.
- FLAGS_tcmalloc_release_rate = 0.0;
- }
-
- // Forces the memory pages for all the stack space that we're ever going to
- // use to be loaded into memory (so it can be locked there).
- uint8_t data[4096 * 8];
- // Not 0 because linux might optimize that to a 0-filled page.
- memset(data, 1, sizeof(data));
-
- static const size_t kHeapPreallocSize = 512 * 1024;
- char *const heap_data = static_cast<char *>(malloc(kHeapPreallocSize));
- memset(heap_data, 1, kHeapPreallocSize);
- free(heap_data);
+void InitGoogle(int *argc, char ***argv) {
+ FLAGS_logtostderr = true;
+ google::InitGoogleLogging((*argv)[0]);
+ gflags::ParseCommandLineFlags(argc, argv, true);
+ google::InstallFailureSignalHandler();
}
void InitNRT(bool for_realtime) {
@@ -120,16 +78,6 @@
GoRT(relative_priority);
}
-void InitRT() {
- LockAllMemory();
-
- // Only let rt processes run for 3 seconds straight.
- SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
-
- // Allow rt processes up to priority 40.
- SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
-}
-
void GoRT(int relative_priority) {
if (ShouldBeRealtime()) {
InitRT();
@@ -155,30 +103,6 @@
aos_core_free_shared_mem();
}
-void WriteCoreDumps() {
- // Do create core files of unlimited size.
- SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, true);
-}
-
-void SetCurrentThreadRealtimePriority(int priority) {
- // Make sure we will only be allowed to run for 3 seconds straight.
- SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
-
- struct sched_param param;
- param.sched_priority = priority;
- if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
- AOS_PLOG(FATAL, "sched_setscheduler(0, SCHED_FIFO, %d) failed\n", priority);
- }
-}
-
-void UnsetCurrentThreadRealtimePriority() {
- struct sched_param param;
- param.sched_priority = 0;
- if (sched_setscheduler(0, SCHED_OTHER, ¶m) == -1) {
- AOS_PLOG(FATAL, "sched_setscheduler(0, SCHED_OTHER, 0) failed\n");
- }
-}
-
void PinCurrentThreadToCPU(int number) {
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
@@ -186,13 +110,4 @@
AOS_PRCHECK(pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset));
}
-void SetCurrentThreadName(const ::std::string &name) {
- if (name.size() > 16) {
- AOS_LOG(FATAL, "thread name '%s' too long\n", name.c_str());
- }
- AOS_LOG(INFO, "this thread is changing to '%s'\n", name.c_str());
- AOS_PCHECK(prctl(PR_SET_NAME, name.c_str()));
- logging::internal::ReloadThreadName();
-}
-
} // namespace aos
diff --git a/aos/init.h b/aos/init.h
index be99a24..85d5da7 100644
--- a/aos/init.h
+++ b/aos/init.h
@@ -5,6 +5,9 @@
namespace aos {
+// Initializes glog and gflags.
+void InitGoogle(int *argc, char ***argv);
+
// In order to use shared memory, one of the Init* functions must be called in
// exactly 1 thread per process. It is OK to keep going without calling one of
// them again after fork(2)ing.
@@ -23,35 +26,12 @@
// exit gracefully).
void Cleanup();
-// Locks everything into memory and sets the limits. This plus InitNRT are
-// everything you need to do before SetCurrentThreadRealtimePriority will make
-// your thread RT. Called as part of ShmEventLoop::Run()
-void InitRT();
-
// Performs the realtime parts of initialization after InitNRT(true) has been called.
void GoRT(int relative_priority = 0);
-// Sets up this process to write core dump files.
-// This is called by Init*, but it's here for other files that want this
-// behavior without calling Init*.
-void WriteCoreDumps();
-
-// Sets the current thread's realtime priority.
-void SetCurrentThreadRealtimePriority(int priority);
-
-// Sets the current thread back down to non-realtime priority.
-void UnsetCurrentThreadRealtimePriority();
-
// Pins the current thread to CPU #number.
void PinCurrentThreadToCPU(int number);
-// Sets the name of the current thread.
-// This will displayed by `top -H`, dump_rtprio, and show up in logs.
-// name can have a maximum of 16 characters.
-void SetCurrentThreadName(const ::std::string &name);
-
-void LockAllMemory();
-
} // namespace aos
#endif // AOS_INIT_H_
diff --git a/aos/input/BUILD b/aos/input/BUILD
index 0da8d01..b4ceea6 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -9,11 +9,10 @@
"joystick_input.h",
],
deps = [
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/input:driver_station_data",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
],
)
@@ -29,10 +28,11 @@
"//aos:math",
"//aos/input:driver_station_data",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
],
)
@@ -45,7 +45,8 @@
"driver_station_data.h",
],
deps = [
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
+ "@com_github_google_glog//:glog",
],
)
@@ -54,12 +55,12 @@
srcs = ["action_joystick_input.cc"],
hdrs = ["action_joystick_input.h"],
deps = [
+ ":drivetrain_input",
"//aos:init",
"//aos/actions:action_lib",
- "//aos/input:drivetrain_input",
"//aos/input:joystick_input",
"//aos/logging",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
],
)
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 509f4c5..376009b 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -1,7 +1,7 @@
#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
-#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/auto_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
using ::aos::input::driver_station::ControlBit;
@@ -49,8 +49,10 @@
void ActionJoystickInput::StartAuto() {
AOS_LOG(INFO, "Starting auto mode\n");
- action_queue_.EnqueueAction(
- autonomous_action_factory_.Make(GetAutonomousMode()));
+ frc971::autonomous::AutonomousActionParamsT params;
+ params.mode = GetAutonomousMode();
+
+ action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
auto_action_running_ = true;
}
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 3057ace..781e224 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -4,7 +4,7 @@
#include "aos/input/driver_station_data.h"
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
-#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/auto_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
namespace aos {
@@ -99,7 +99,7 @@
AOS_LOG(WARNING, "no auto mode values\n");
return 0;
}
- return autonomous_mode_fetcher_->mode;
+ return autonomous_mode_fetcher_->mode();
}
// True if the internal state machine thinks auto is running right now.
diff --git a/aos/input/driver_station_data.cc b/aos/input/driver_station_data.cc
index 8898ca1..9e3bc52 100644
--- a/aos/input/driver_station_data.cc
+++ b/aos/input/driver_station_data.cc
@@ -1,20 +1,39 @@
#include "aos/input/driver_station_data.h"
+#include "glog/logging.h"
+
namespace aos {
namespace input {
namespace driver_station {
Data::Data() : current_values_(), old_values_() {}
-void Data::Update(const JoystickState &new_values) {
+void Data::Update(const JoystickState *new_values) {
old_values_ = current_values_;
- current_values_ = new_values;
+ CHECK(new_values->has_joysticks());
+ CHECK_EQ(new_values->joysticks()->size(), current_values_.joysticks.size());
+ for (size_t i = 0; i < current_values_.joysticks.size(); ++i) {
+ const Joystick *joystick = new_values->joysticks()->Get(i);
+ current_values_.joysticks[i].buttons =
+ joystick->buttons();
+ current_values_.joysticks[i].pov = joystick->pov();
+ for (size_t j = 0; j < joystick->axis()->size(); ++j) {
+ current_values_.joysticks[i].axis[j] = joystick->axis()->Get(j);
+ }
+
+ current_values_.joysticks[i].pov = joystick->pov();
+ }
+ current_values_.test_mode = new_values->test_mode();
+ current_values_.fms_attached = new_values->fms_attached();
+ current_values_.enabled = new_values->enabled();
+ current_values_.autonomous = new_values->autonomous();
+ current_values_.team_id = new_values->team_id();
+ current_values_.switch_left = new_values->switch_left();
+ current_values_.scale_left = new_values->scale_left();
}
-namespace {
-
-bool GetButton(const ButtonLocation location,
- const JoystickState &values) {
+bool Data::GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values) {
if (location.joystick() < 0 ||
location.joystick() > static_cast<int>(values.joysticks.size())) {
return false;
@@ -23,15 +42,16 @@
return false;
}
return values.joysticks[location.joystick() - 1].buttons &
- (1 << (location.number() - 1));
+ (1 << (location.number() - 1));
}
-bool DoGetPOV(const POVLocation location, const JoystickState &values) {
+bool Data::DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values) {
return values.joysticks[location.joystick() - 1].pov == location.number();
}
-bool GetControlBitValue(const ControlBit bit,
- const JoystickState &values) {
+bool Data::GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values) {
switch (bit) {
case ControlBit::kTestMode:
return values.test_mode;
@@ -46,8 +66,6 @@
}
}
-} // namespace
-
bool Data::IsPressed(const ButtonLocation location) const {
return GetButton(location, current_values_);
}
diff --git a/aos/input/driver_station_data.h b/aos/input/driver_station_data.h
index 6a52248..18eb579 100644
--- a/aos/input/driver_station_data.h
+++ b/aos/input/driver_station_data.h
@@ -4,9 +4,10 @@
// This file defines several types to support nicely looking at the data
// received from the driver's station.
+#include <array>
#include <memory>
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/joystick_state_generated.h"
namespace aos {
namespace input {
@@ -20,8 +21,7 @@
: joystick_(joystick), number_(number) {}
// How many joysticks there are.
- static const int kJoysticks = sizeof(JoystickState::joysticks) /
- sizeof(JoystickState::joysticks[0]);
+ static const int kJoysticks = 6;
// Which joystick number this is (1-based).
int joystick() const { return joystick_; }
@@ -68,8 +68,7 @@
: JoystickFeature(joystick, number) {}
// How many axes there are available on each joystick.
- static const int kAxes = sizeof(Joystick::axis) /
- sizeof(Joystick::axis[0]);
+ static const int kAxes = 6;
};
class Data {
@@ -79,7 +78,7 @@
Data();
// Updates the current information with a new set of values.
- void Update(const JoystickState &new_values);
+ void Update(const JoystickState *new_values);
bool IsPressed(POVLocation location) const;
bool PosEdge(POVLocation location) const;
@@ -101,7 +100,34 @@
float GetAxis(JoystickAxis axis) const;
private:
- JoystickState current_values_, old_values_;
+ struct SavedJoystickState {
+ struct SavedJoystick {
+ uint16_t buttons = 0;
+ std::array<double, JoystickAxis::kAxes> axis;
+ int pov = 0;
+ };
+
+ std::array<SavedJoystick, JoystickFeature::kJoysticks> joysticks;
+ bool test_mode = false;
+ bool fms_attached = false;
+ bool enabled = false;
+ bool autonomous = false;
+ uint16_t team_id = 0;
+
+ // 2018 scale and switch positions.
+ bool switch_left = false;
+ bool scale_left = false;
+ };
+
+ static bool GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values);
+ static bool DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values);
+
+ static bool GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values);
+
+ SavedJoystickState current_values_, old_values_;
};
} // namespace driver_station
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index a0bbf2c..687c8bb 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -8,13 +8,17 @@
#include "aos/commonmath.h"
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;
+namespace drivetrain = frc971::control_loops::drivetrain;
+
namespace aos {
namespace input {
@@ -33,7 +37,7 @@
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- robot_velocity_ = drivetrain_status_fetcher_->robot_speed;
+ robot_velocity_ = drivetrain_status_fetcher_->robot_speed();
}
// If we have a vision align function, and it is in control, don't run the
@@ -71,9 +75,9 @@
if (drivetrain_status_fetcher_.get()) {
if (is_control_loop_driving && !last_is_control_loop_driving_) {
- left_goal_ = drivetrain_status_fetcher_->estimated_left_position +
+ left_goal_ = drivetrain_status_fetcher_->estimated_left_position() +
wheel * wheel_multiplier_;
- right_goal_ = drivetrain_status_fetcher_->estimated_right_position -
+ right_goal_ = drivetrain_status_fetcher_->estimated_right_position() -
wheel * wheel_multiplier_;
}
}
@@ -82,24 +86,36 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- auto new_drivetrain_goal = drivetrain_goal_sender_.MakeMessage();
- new_drivetrain_goal->wheel = wheel;
- new_drivetrain_goal->wheel_velocity = wheel_velocity;
- new_drivetrain_goal->wheel_torque = wheel_torque;
- new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->throttle_velocity = throttle_velocity;
- new_drivetrain_goal->throttle_torque = throttle_torque;
- new_drivetrain_goal->highgear = high_gear;
- new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
- new_drivetrain_goal->controller_type =
- is_line_following ? 3 : (is_control_loop_driving ? 1 : 0);
- new_drivetrain_goal->left_goal = current_left_goal;
- new_drivetrain_goal->right_goal = current_right_goal;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- new_drivetrain_goal->linear.max_velocity = 3.0;
- new_drivetrain_goal->linear.max_acceleration = 20.0;
+ frc971::ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
- if (!new_drivetrain_goal.Send()) {
+ linear_builder.add_max_velocity(3.0);
+ linear_builder.add_max_acceleration(20.0);
+
+ flatbuffers::Offset<frc971::ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ auto goal_builder = builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_wheel(wheel);
+ goal_builder.add_wheel_velocity(wheel_velocity);
+ goal_builder.add_wheel_torque(wheel_torque);
+ goal_builder.add_throttle(throttle);
+ goal_builder.add_throttle_velocity(throttle_velocity);
+ goal_builder.add_throttle_torque(throttle_torque);
+ goal_builder.add_highgear(high_gear);
+ goal_builder.add_quickturn(data.IsPressed(quick_turn_));
+ goal_builder.add_controller_type(
+ is_line_following
+ ? drivetrain::ControllerType_LINE_FOLLOWER
+ : (is_control_loop_driving ? drivetrain::ControllerType_MOTION_PROFILE
+ : drivetrain::ControllerType_POLYDRIVE));
+ goal_builder.add_left_goal(current_left_goal);
+ goal_builder.add_right_goal(current_right_goal);
+ goal_builder.add_linear(linear_offset);
+
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending stick values failed\n");
}
@@ -304,8 +320,7 @@
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
::aos::EventLoop *event_loop, InputType type,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config) {
+ const drivetrain::DrivetrainConfig<double> &dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
using InputType = DrivetrainInputReader::InputType;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 26dfe95..0c43d99 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -9,8 +9,10 @@
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
namespace aos {
namespace input {
@@ -60,12 +62,11 @@
turn2_use_(turn2_use),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")) {}
virtual ~DrivetrainInputReader() = default;
@@ -130,9 +131,9 @@
virtual WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) = 0;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
double robot_velocity_ = 0.0;
diff --git a/aos/input/joystick_input.cc b/aos/input/joystick_input.cc
index d1bd302..743e138 100644
--- a/aos/input/joystick_input.cc
+++ b/aos/input/joystick_input.cc
@@ -3,18 +3,17 @@
#include <string.h>
#include <atomic>
-#include "aos/robot_state/robot_state.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/robot_state/robot_state_generated.h"
namespace aos {
namespace input {
-void JoystickInput::HandleData(const ::aos::JoystickState &joystick_state) {
+void JoystickInput::HandleData(const ::aos::JoystickState *joystick_state) {
data_.Update(joystick_state);
- mode_ = static_cast<int>(joystick_state.switch_left) |
- (static_cast<int>(joystick_state.scale_left) << 1);
+ mode_ = static_cast<int>(joystick_state->switch_left()) |
+ (static_cast<int>(joystick_state->scale_left()) << 1);
{
using driver_station::JoystickFeature;
diff --git a/aos/input/joystick_input.h b/aos/input/joystick_input.h
index ed72e78..5e8bf5a 100644
--- a/aos/input/joystick_input.h
+++ b/aos/input/joystick_input.h
@@ -3,7 +3,7 @@
#include <atomic>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/input/driver_station_data.h"
namespace aos {
@@ -21,7 +21,7 @@
event_loop_->MakeWatcher(
".aos.joystick_state",
[this](const ::aos::JoystickState &joystick_state) {
- this->HandleData(joystick_state);
+ this->HandleData(&joystick_state);
});
event_loop->SetRuntimeRealtimePriority(29);
}
@@ -30,7 +30,7 @@
int mode() const { return mode_; }
private:
- void HandleData(const ::aos::JoystickState &joystick_state);
+ void HandleData(const ::aos::JoystickState *joystick_state);
// Subclasses should do whatever they want with data here.
virtual void RunIteration(const driver_station::Data &data) = 0;
@@ -41,12 +41,6 @@
int mode_;
};
-// Class which will proxy joystick information from UDP packets to the queues.
-class JoystickProxy {
- public:
- void Run();
-};
-
} // namespace input
} // namespace aos
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index 684620a..908cb80 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -1,5 +1,3 @@
-package(default_visibility = ["//visibility:public"])
-
cc_library(
name = "aos_sync",
srcs = [
@@ -11,11 +9,12 @@
linkopts = [
"-lpthread",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:macros",
- "@com_google_absl//absl/base",
- "//aos/logging",
"//aos/util:compiler_memory_barrier",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/base",
],
)
@@ -27,6 +26,7 @@
hdrs = [
"core_lib.h",
],
+ visibility = ["//visibility:public"],
deps = [
":aos_sync",
":shared_mem_types",
@@ -36,7 +36,7 @@
cc_library(
name = "shared_mem",
srcs = [
- "shared_mem.c",
+ "shared_mem.cc",
],
hdrs = [
"shared_mem.h",
@@ -44,11 +44,12 @@
linkopts = [
"-lrt",
],
+ visibility = ["//visibility:public"],
deps = [
":aos_sync",
":core_lib",
":shared_mem_types",
- "//aos/logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -75,11 +76,11 @@
linkopts = [
"-lrt",
],
+ visibility = ["//visibility:public"],
deps = [
":core_lib",
":shared_mem",
"//aos:condition",
- "//aos/logging",
"//aos/mutex",
"//aos/util:options",
],
@@ -157,8 +158,9 @@
hdrs = [
"signalfd.h",
],
+ visibility = ["//visibility:public"],
deps = [
- "//aos/logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -166,6 +168,7 @@
name = "index",
srcs = ["index.cc"],
hdrs = ["index.h"],
+ visibility = ["//visibility:public"],
)
cc_test(
@@ -174,7 +177,7 @@
deps = [
":index",
"//aos/testing:googletest",
- "//aos/testing:test_logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -185,11 +188,11 @@
"lockless_queue_memory.h",
],
hdrs = ["lockless_queue.h"],
+ visibility = ["//visibility:public"],
deps = [
":aos_sync",
":index",
- "//aos:init",
- "//aos/logging",
+ "//aos:realtime",
"//aos/time",
"//aos/util:compiler_memory_barrier",
"@com_github_google_glog//:glog",
@@ -208,7 +211,7 @@
deps = [
":lockless_queue",
"//aos:event",
- "//third_party/googletest:gtest",
+ "//aos/testing:googletest",
],
)
@@ -225,7 +228,6 @@
"//aos/libc:aos_strsignal",
"//aos/testing:googletest",
"//aos/testing:prevent_exit",
- "//aos/testing:test_logging",
],
)
diff --git a/aos/ipc_lib/aos_sync.cc b/aos/ipc_lib/aos_sync.cc
index 3bdba21..efb1fb1 100644
--- a/aos/ipc_lib/aos_sync.cc
+++ b/aos/ipc_lib/aos_sync.cc
@@ -26,10 +26,10 @@
#include <algorithm>
#include <type_traits>
-#include "aos/logging/logging.h"
+#include "absl/base/call_once.h"
#include "aos/macros.h"
#include "aos/util/compiler_memory_barrier.h"
-#include "absl/base/call_once.h"
+#include "glog/logging.h"
using ::aos::linux_code::ipc_lib::FutexAccessorObserver;
@@ -354,16 +354,16 @@
return r;
}
-// This gets called by functions before AOS_LOG(FATAL)ing with error messages
+// This gets called by functions before LOG(FATAL)ing with error messages
// that would be incorrect if the error was caused by a process forking without
// initialize_in_new_thread getting called in the fork.
void check_cached_tid(pid_t tid) {
pid_t actual = do_get_tid();
if (tid != actual) {
- AOS_LOG(FATAL,
- "task %jd forked into %jd without letting aos_sync know"
- " so we're not really sure what's going on\n",
- static_cast<intmax_t>(tid), static_cast<intmax_t>(actual));
+ LOG(FATAL) << "task " << static_cast<intmax_t>(tid) << " forked into "
+ << static_cast<intmax_t>(actual)
+ << " without letting aos_sync know so we're not really sure "
+ "what's going on";
}
}
@@ -378,9 +378,9 @@
}
void InstallAtforkHook() {
- if (pthread_atfork(NULL, NULL, atfork_child) != 0) {
- AOS_PLOG(FATAL, "pthread_atfork(NULL, NULL, %p) failed", atfork_child);
- }
+ PCHECK(pthread_atfork(NULL, NULL, &atfork_child) == 0)
+ << ": pthread_atfork(NULL, NULL, "
+ << reinterpret_cast<void *>(&atfork_child) << ") failed";
}
// This gets called to set everything up in a new thread by get_tid().
@@ -472,11 +472,10 @@
robust_head.futex_offset = static_cast<ssize_t>(offsetof(aos_mutex, futex)) -
static_cast<ssize_t>(offsetof(aos_mutex, next));
robust_head.pending_next = 0;
- if (syscall(SYS_set_robust_list, robust_head_next_value(), sizeof(robust_head)) !=
- 0) {
- AOS_PLOG(FATAL, "set_robust_list(%p, %zd) failed",
- reinterpret_cast<void *>(robust_head.next), sizeof(robust_head));
- }
+ PCHECK(syscall(SYS_set_robust_list, robust_head_next_value(),
+ sizeof(robust_head)) == 0)
+ << ": set_robust_list(" << reinterpret_cast<void *>(robust_head.next)
+ << ", " << sizeof(robust_head) << ") failed";
if (kRobustListDebug) {
printf("%" PRId32 ": init done\n", get_tid());
}
@@ -675,12 +674,12 @@
}
}
my_robust_list::robust_head.pending_next = 0;
- if (ret == -EDEADLK) {
- AOS_LOG(FATAL, "multiple lock of %p by %" PRId32 "\n", m, tid);
- }
- AOS_PELOG(FATAL, -ret, "FUTEX_LOCK_PI(%p(=%" PRIu32 "), 1, %p) failed",
- &m->futex, __atomic_load_n(&m->futex, __ATOMIC_SEQ_CST),
- timeout);
+ CHECK_NE(ret, -EDEADLK) << ": multiple lock of " << m << " by " << tid;
+
+ errno = -ret;
+ PLOG(FATAL) << "FUTEX_LOCK_PI(" << &m->futex
+ << "(=" << __atomic_load_n(&m->futex, __ATOMIC_SEQ_CST)
+ << "), 1, " << timeout << ") failed";
} else {
if (kLockDebug) {
printf("%" PRId32 ": %p kernel lock done\n", tid, m);
@@ -746,8 +745,9 @@
continue;
}
my_robust_list::robust_head.pending_next = 0;
- AOS_PELOG(FATAL, -ret, "FUTEX_CMP_REQUEUE_PI(%p, 1, %d, %p, *%p) failed",
- c, number_requeue, &m->futex, c);
+ errno = -ret;
+ PLOG(FATAL) << "FUTEX_CMP_REQUEUE_PI(" << c << ", 1, " << number_requeue
+ << ", " << &m->futex << ", *" << c << ") failed";
} else {
return;
}
@@ -778,11 +778,10 @@
my_robust_list::robust_head.pending_next = 0;
check_cached_tid(tid);
if ((value & FUTEX_TID_MASK) == 0) {
- AOS_LOG(FATAL, "multiple unlock of aos_mutex %p by %" PRId32 "\n", m,
- tid);
+ LOG(FATAL) << "multiple unlock of aos_mutex " << m << " by " << tid;
} else {
- AOS_LOG(FATAL, "aos_mutex %p is locked by %" PRId32 ", not %" PRId32 "\n",
- m, value & FUTEX_TID_MASK, tid);
+ LOG(FATAL) << "aos_mutex " << m << " is locked by "
+ << (value & FUTEX_TID_MASK) << ", not " << tid;
}
}
@@ -795,7 +794,8 @@
const int ret = sys_futex_unlock_pi(&m->futex);
if (ret != 0) {
my_robust_list::robust_head.pending_next = 0;
- AOS_PELOG(FATAL, -ret, "FUTEX_UNLOCK_PI(%p) failed", &m->futex);
+ errno = -ret;
+ PLOG(FATAL) << "FUTEX_UNLOCK_PI(" << (&m->futex) << ") failed";
}
} else {
// There aren't any waiters, so no need to call into the kernel.
@@ -832,8 +832,9 @@
return 4;
}
my_robust_list::robust_head.pending_next = 0;
- AOS_PELOG(FATAL, -ret, "FUTEX_TRYLOCK_PI(%p, 0, NULL) failed",
- &m->futex);
+ errno = -ret;
+ PLOG(FATAL) << "FUTEX_TRYLOCK_PI(" << (&m->futex)
+ << ", 0, NULL) failed";
}
}
}
@@ -898,9 +899,9 @@
continue;
}
my_robust_list::robust_head.pending_next = 0;
- AOS_PELOG(FATAL, -ret,
- "FUTEX_WAIT_REQUEUE_PI(%p, %" PRIu32 ", %p) failed", c,
- wait_start, &m->futex);
+ errno = -ret;
+ PLOG(FATAL) << "FUTEX_WAIT_REQUEUE_PI(" << c << ", " << wait_start << ", "
+ << (&m->futex) << ") failed";
} else {
// Record that the kernel relocked it for us.
lock_pthread_mutex(m);
diff --git a/aos/ipc_lib/index_test.cc b/aos/ipc_lib/index_test.cc
index 689ed24..2e9a37b 100644
--- a/aos/ipc_lib/index_test.cc
+++ b/aos/ipc_lib/index_test.cc
@@ -1,7 +1,7 @@
#include "aos/ipc_lib/index.h"
-#include "aos/testing/test_logging.h"
#include "gtest/gtest.h"
+#include "glog/logging.h"
namespace aos {
namespace ipc_lib {
@@ -10,7 +10,8 @@
class QueueIndexTest : public ::testing::Test {
protected:
uint32_t GetIndex(const QueueIndex &index) {
- printf("Index, count: %x, %x\n", index.index_, index.count_);
+ LOG(INFO) << "Index, count: " << std::hex << index.index_ << ", "
+ << index.count_;
return index.index();
}
diff --git a/aos/ipc_lib/ipc_comparison.cc b/aos/ipc_lib/ipc_comparison.cc
index 5ea1c8b..1553159 100644
--- a/aos/ipc_lib/ipc_comparison.cc
+++ b/aos/ipc_lib/ipc_comparison.cc
@@ -28,6 +28,7 @@
#include "aos/logging/implementations.h"
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
#include "aos/time/time.h"
DEFINE_string(method, "", "Which IPC method to use");
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index cd0e2ae..c2b0254 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -9,9 +9,8 @@
#include <iostream>
#include <sstream>
-#include "aos/init.h"
#include "aos/ipc_lib/lockless_queue_memory.h"
-#include "aos/logging/logging.h"
+#include "aos/realtime.h"
#include "aos/util/compiler_memory_barrier.h"
#include "glog/logging.h"
@@ -20,8 +19,6 @@
namespace {
-constexpr bool kDebug = false;
-
void GrabQueueSetupLockOrDie(LocklessQueueMemory *memory) {
const int result = mutex_grab(&(memory->queue_setup_lock));
CHECK(result == 0 || result == 1);
@@ -59,9 +56,7 @@
const uint32_t tid =
__atomic_load_n(&(sender->tid.futex), __ATOMIC_RELAXED);
if (tid & FUTEX_OWNER_DIED) {
- if (kDebug) {
- printf("Found an easy death for sender %zu\n", i);
- }
+ VLOG(3) << "Found an easy death for sender " << i;
const Index to_replace = sender->to_replace.RelaxedLoad();
const Index scratch_index = sender->scratch_index.Load();
@@ -119,9 +114,7 @@
return;
}
- if (kDebug) {
- printf("Starting hard cleanup\n");
- }
+ VLOG(3) << "Starting hard cleanup";
size_t num_accounted_for = 0;
size_t num_missing = 0;
@@ -166,9 +159,8 @@
// Candidate.
CHECK_LE(to_replace.message_index(), accounted_for.size());
if (accounted_for[to_replace.message_index()]) {
- if (kDebug) {
- printf("Sender %zu died, to_replace is already accounted for\n", i);
- }
+ VLOG(3) << "Sender " << i
+ << " died, to_replace is already accounted for";
// If both are accounted for, we are corrupt...
CHECK(!accounted_for[scratch_index.message_index()]);
@@ -185,9 +177,8 @@
--num_missing;
++num_accounted_for;
} else if (accounted_for[scratch_index.message_index()]) {
- if (kDebug) {
- printf("Sender %zu died, scratch_index is already accounted for\n", i);
- }
+ VLOG(3) << "Sender " << i
+ << " died, scratch_index is already accounted for";
// scratch_index is accounted for. That means we did the insert,
// but didn't record it.
CHECK(to_replace.valid());
@@ -204,9 +195,7 @@
--num_missing;
++num_accounted_for;
} else {
- if (kDebug) {
- printf("Sender %zu died, neither is accounted for\n", i);
- }
+ VLOG(3) << "Sender " << i << " died, neither is accounted for";
// Ambiguous. There will be an unambiguous one somewhere that we
// can do first.
}
@@ -406,7 +395,8 @@
// creating a pidfd is likely not RT.
for (size_t i = 0; i < num_watchers; ++i) {
Watcher *w = memory_->GetWatcher(i);
- // Start by reading the tid. This needs to be atomic to force it to come first.
+ // Start by reading the tid. This needs to be atomic to force it to come
+ // first.
watcher_copy_[i].tid = __atomic_load_n(&(w->tid.futex), __ATOMIC_SEQ_CST);
watcher_copy_[i].pid = w->pid;
watcher_copy_[i].priority = w->priority;
@@ -521,18 +511,32 @@
return index;
}
+size_t LocklessQueue::Sender::size() { return memory_->message_data_size(); }
+
+void *LocklessQueue::Sender::Data() {
+ ::aos::ipc_lib::Sender *sender = memory_->GetSender(sender_index_);
+ Index scratch_index = sender->scratch_index.RelaxedLoad();
+ Message *message = memory_->GetMessage(scratch_index);
+ message->header.queue_index.Invalidate();
+
+ return &message->data[0];
+}
+
void LocklessQueue::Sender::Send(const char *data, size_t length) {
+ CHECK_LE(length, size());
+ memcpy(Data(), data, length);
+ Send(length);
+}
+
+void LocklessQueue::Sender::Send(size_t length) {
const size_t queue_size = memory_->queue_size();
- CHECK_LE(length, memory_->message_data_size());
+ CHECK_LE(length, size());
::aos::ipc_lib::Sender *sender = memory_->GetSender(sender_index_);
Index scratch_index = sender->scratch_index.RelaxedLoad();
Message *message = memory_->GetMessage(scratch_index);
- message->header.queue_index.Invalidate();
-
message->header.length = length;
- memcpy(&message->data[0], data, length);
while (true) {
const QueueIndex actual_next_queue_index =
@@ -564,10 +568,8 @@
memory_->next_queue_index.CompareAndExchangeStrong(
actual_next_queue_index, incremented_queue_index);
- if (kDebug) {
- printf("We were beat. Try again. Was %x, is %x\n", to_replace.get(),
- decremented_queue_index.index());
- }
+ VLOG(3) << "We were beat. Try again. Was " << std::hex
+ << to_replace.get() << ", is " << decremented_queue_index.index();
continue;
}
@@ -581,12 +583,10 @@
->header.queue_index.RelaxedLoad(queue_size);
if (previous_index != decremented_queue_index && previous_index.valid()) {
// Retry.
- if (kDebug) {
- printf(
- "Something fishy happened, queue index doesn't match. Retrying. "
- " Previous index was %x, should be %x\n",
- previous_index.index(), decremented_queue_index.index());
- }
+ VLOG(3) << "Something fishy happened, queue index doesn't match. "
+ "Retrying. Previous index was "
+ << std::hex << previous_index.index() << ", should be "
+ << decremented_queue_index.index();
continue;
}
}
@@ -597,8 +597,7 @@
// Before we are fully done filling out the message, update the Sender state
// with the new index to write. This re-uses the barrier for the
// queue_index store.
- const Index index_to_write(next_queue_index,
- scratch_index.message_index());
+ const Index index_to_write(next_queue_index, scratch_index.message_index());
sender->scratch_index.RelaxedStore(index_to_write);
@@ -616,9 +615,7 @@
->CompareAndExchangeStrong(to_replace, index_to_write)) {
// Aw, didn't succeed. Retry.
sender->to_replace.RelaxedInvalidate();
- if (kDebug) {
- printf("Failed to wrap into queue\n");
- }
+ VLOG(3) << "Failed to wrap into queue";
continue;
}
@@ -661,10 +658,8 @@
if (starting_queue_index != queue_index) {
// If we found a message that is exactly 1 loop old, we just wrapped.
if (starting_queue_index == queue_index.DecrementBy(queue_size)) {
- if (kDebug) {
- printf("Matches: %x, %x\n", starting_queue_index.index(),
- queue_index.DecrementBy(queue_size).index());
- }
+ VLOG(3) << "Matches: " << std::hex << starting_queue_index.index()
+ << ", " << queue_index.DecrementBy(queue_size).index();
return ReadResult::NOTHING_NEW;
} else {
// Someone has re-used this message between when we pulled it out of the
@@ -673,9 +668,7 @@
Message *new_m = memory_->GetMessage(queue_index);
if (m != new_m) {
m = new_m;
- if (kDebug) {
- printf("Retrying, m doesn't match\n");
- }
+ VLOG(3) << "Retrying, m doesn't match";
continue;
}
@@ -686,17 +679,14 @@
// Either we got too far behind (signaled by this being a valid
// message), or this is one of the initial messages which are invalid.
if (starting_queue_index.valid()) {
- if (kDebug) {
- printf("Too old. Tried for %x, got %x, behind by %d\n",
- queue_index.index(), starting_queue_index.index(),
- starting_queue_index.index() - queue_index.index());
- }
+ VLOG(3) << "Too old. Tried for " << std::hex << queue_index.index()
+ << ", got " << starting_queue_index.index() << ", behind by "
+ << std::dec
+ << (starting_queue_index.index() - queue_index.index());
return ReadResult::TOO_OLD;
}
- if (kDebug) {
- printf("Initial\n");
- }
+ VLOG(3) << "Initial";
// There isn't a valid message at this location.
//
@@ -705,28 +695,24 @@
// asking for something crazy, like something before the beginning of
// the queue. Tell them that they are behind.
if (uint32_queue_index < memory_->queue_size()) {
- if (kDebug) {
- printf("Near zero, %x\n", uint32_queue_index);
- }
+ VLOG(3) << "Near zero, " << std::hex << uint32_queue_index;
return ReadResult::NOTHING_NEW;
} else {
- if (kDebug) {
- printf("not near zero, %x\n", uint32_queue_index);
- }
+ VLOG(3) << "not near zero, " << std::hex << uint32_queue_index;
return ReadResult::TOO_OLD;
}
}
}
- if (kDebug) {
- printf("Eq: %x, %x\n", starting_queue_index.index(), queue_index.index());
- }
+ VLOG(3) << "Eq: " << std::hex << starting_queue_index.index() << ", "
+ << queue_index.index();
break;
}
- // Then read the data out.
+ // Then read the data out. Copy it all out to be deterministic and so we can
+ // make length be from either end.
*monotonic_sent_time = m->header.monotonic_sent_time;
*realtime_sent_time = m->header.realtime_sent_time;
- memcpy(data, &m->data[0], m->header.length);
+ memcpy(data, &m->data[0], message_data_size());
*length = m->header.length;
// And finally, confirm that the message *still* points to the queue index we
@@ -735,20 +721,22 @@
// it's lifetime.
const QueueIndex final_queue_index = m->header.queue_index.Load(queue_size);
if (final_queue_index != queue_index) {
- if (kDebug) {
- printf(
- "Changed out from under us. Reading %x, finished with %x, delta: "
- "%d\n",
- queue_index.index(), final_queue_index.index(),
- final_queue_index.index() - queue_index.index());
- }
- return ReadResult::TOO_OLD;
+ VLOG(3) << "Changed out from under us. Reading " << std::hex
+ << queue_index.index() << ", finished with "
+ << final_queue_index.index() << ", delta: " << std::dec
+ << (final_queue_index.index() - queue_index.index());
+ return ReadResult::OVERWROTE;
}
return ReadResult::GOOD;
}
-uint32_t LocklessQueue::LatestQueueIndex() {
+size_t LocklessQueue::queue_size() const { return memory_->queue_size(); }
+size_t LocklessQueue::message_data_size() const {
+ return memory_->message_data_size();
+}
+
+QueueIndex LocklessQueue::LatestQueueIndex() {
const size_t queue_size = memory_->queue_size();
// There is only one interesting case. We need to know if the queue is empty.
@@ -757,7 +745,7 @@
memory_->next_queue_index.Load(queue_size);
if (next_queue_index.valid()) {
const QueueIndex current_queue_index = next_queue_index.DecrementBy(1u);
- return current_queue_index.index();
+ return current_queue_index;
} else {
return empty_queue_index();
}
@@ -845,7 +833,8 @@
}
::std::cout << " }" << ::std::endl;
- ::std::cout << " Sender senders[" << memory->num_senders() << "] {" << ::std::endl;
+ ::std::cout << " Sender senders[" << memory->num_senders() << "] {"
+ << ::std::endl;
for (size_t i = 0; i < memory->num_senders(); ++i) {
Sender *s = memory->GetSender(i);
::std::cout << " [" << i << "] -> Sender {" << ::std::endl;
diff --git a/aos/ipc_lib/lockless_queue.h b/aos/ipc_lib/lockless_queue.h
index dafc157..fcc5d79 100644
--- a/aos/ipc_lib/lockless_queue.h
+++ b/aos/ipc_lib/lockless_queue.h
@@ -106,7 +106,7 @@
// Prints to stdout the data inside the queue for debugging.
void PrintLocklessQueueMemory(LocklessQueueMemory *memory);
-const static int kWakeupSignal = SIGRTMIN + 2;
+const static unsigned int kWakeupSignal = SIGRTMIN + 2;
// Class to manage sending and receiving data in the lockless queue. This is
// separate from the actual memory backing the queue so that memory can be
@@ -122,6 +122,8 @@
// Returns the number of messages in the queue.
size_t QueueSize() const;
+ size_t message_data_size() const;
+
// Registers this thread to receive the kWakeupSignal signal when Wakeup is
// called. Returns false if there was an error in registration.
bool RegisterWakeup(int priority);
@@ -137,8 +139,9 @@
// If you ask for a queue index 2 past the newest, you will still get
// NOTHING_NEW until that gets overwritten with new data. If you ask for an
// element newer than QueueSize() from the current message, we consider it
- // behind by a large amount and return TOO_OLD.
- enum class ReadResult { TOO_OLD, GOOD, NOTHING_NEW };
+ // behind by a large amount and return TOO_OLD. If the message is modified
+ // out from underneath us as we read it, return OVERWROTE.
+ enum class ReadResult { TOO_OLD, GOOD, NOTHING_NEW, OVERWROTE };
ReadResult Read(uint32_t queue_index,
::aos::monotonic_clock::time_point *monotonic_sent_time,
::aos::realtime_clock::time_point *realtime_sent_time,
@@ -147,8 +150,12 @@
// Returns the index to the latest queue message. Returns empty_queue_index()
// if there are no messages in the queue. Do note that this index wraps if
// more than 2^32 messages are sent.
- uint32_t LatestQueueIndex();
- static constexpr uint32_t empty_queue_index() { return 0xffffffff; }
+ QueueIndex LatestQueueIndex();
+ static QueueIndex empty_queue_index() { return QueueIndex::Invalid(); }
+
+ // Returns the size of the queue. This is mostly useful for manipulating
+ // QueueIndex.
+ size_t queue_size() const;
// TODO(austin): Return the oldest queue index. This lets us catch up nicely
// if we got behind.
@@ -181,6 +188,14 @@
~Sender();
+ // Sends a message without copying the data.
+ // Copy at most size() bytes of data into the memory pointed to by Data(),
+ // and then call Send().
+ // Note: calls to Data() are expensive enough that you should cache it.
+ size_t size();
+ void *Data();
+ void Send(size_t length);
+
// Sends up to length data. Does not wakeup the target.
void Send(const char *data, size_t length);
diff --git a/aos/ipc_lib/lockless_queue_death_test.cc b/aos/ipc_lib/lockless_queue_death_test.cc
index 7a68273..4f1d7e4 100644
--- a/aos/ipc_lib/lockless_queue_death_test.cc
+++ b/aos/ipc_lib/lockless_queue_death_test.cc
@@ -11,10 +11,10 @@
#include <memory>
#include <thread>
-#include "aos/init.h"
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/lockless_queue_memory.h"
#include "aos/libc/aos_strsignal.h"
+#include "aos/realtime.h"
#include "aos/testing/prevent_exit.h"
#include "aos/testing/test_logging.h"
#include "gflags/gflags.h"
@@ -507,14 +507,14 @@
TestShmRobustness(
config,
- [this, config, tid](void *memory) {
+ [config, tid](void *memory) {
// Initialize the queue and grab the tid.
LocklessQueue queue(
reinterpret_cast<aos::ipc_lib::LocklessQueueMemory *>(memory),
config);
*tid = gettid();
},
- [this, config](void *memory) {
+ [config](void *memory) {
// Now try to write 2 messages. We will get killed a bunch as this
// tries to happen.
LocklessQueue queue(
@@ -527,7 +527,7 @@
sender.Send(data, s + 1);
}
},
- [this, config, tid](void *raw_memory) {
+ [config, tid](void *raw_memory) {
// Confirm that we can create 2 senders (the number in the queue), and
// send a message. And that all the messages in the queue are valid.
::aos::ipc_lib::LocklessQueueMemory *memory =
diff --git a/aos/ipc_lib/lockless_queue_test.cc b/aos/ipc_lib/lockless_queue_test.cc
index f3b49b6..b9cb54d 100644
--- a/aos/ipc_lib/lockless_queue_test.cc
+++ b/aos/ipc_lib/lockless_queue_test.cc
@@ -11,11 +11,10 @@
#include "aos/event.h"
#include "aos/events/epoll.h"
-#include "aos/init.h"
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/queue_racer.h"
#include "aos/ipc_lib/signalfd.h"
-#include "aos/testing/test_logging.h"
+#include "aos/realtime.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
@@ -43,7 +42,6 @@
class LocklessQueueTest : public ::testing::Test {
public:
LocklessQueueTest() {
- ::aos::testing::EnableTestLogging();
config_.num_watchers = 10;
config_.num_senders = 100;
config_.queue_size = 10000;
@@ -247,8 +245,8 @@
// Send enough messages to wrap.
for (int i = 0; i < 20000; ++i) {
// Confirm that the queue index makes sense given the number of sends.
- EXPECT_EQ(queue.LatestQueueIndex(),
- i == 0 ? LocklessQueue::empty_queue_index() : i - 1);
+ EXPECT_EQ(queue.LatestQueueIndex().index(),
+ i == 0 ? LocklessQueue::empty_queue_index().index() : i - 1);
// Send a trivial piece of data.
char data[100];
@@ -257,7 +255,7 @@
// Confirm that the queue index still makes sense. This is easier since the
// empty case has been handled.
- EXPECT_EQ(queue.LatestQueueIndex(), i);
+ EXPECT_EQ(queue.LatestQueueIndex().index(), i);
// Read a result from 5 in the past.
::aos::monotonic_clock::time_point monotonic_sent_time;
diff --git a/aos/ipc_lib/queue.cc b/aos/ipc_lib/queue.cc
index 5b6a0fa..3b6e5a1 100644
--- a/aos/ipc_lib/queue.cc
+++ b/aos/ipc_lib/queue.cc
@@ -172,7 +172,7 @@
MessageHeader *header = __atomic_load_n(&free_messages_, __ATOMIC_RELAXED);
do {
if (__builtin_expect(header == nullptr, 0)) {
- AOS_LOG(FATAL, "overused pool of queue %p (%s)\n", this, name_);
+ LOG(FATAL) << "overused pool of queue " << this << " (" << name_ << ")";
}
} while (__builtin_expect(
!__atomic_compare_exchange_n(&free_messages_, &header, header->next, true,
@@ -195,10 +195,8 @@
static_assert((sizeof(RawQueue::MessageHeader) % 8) == 0,
"need to revalidate size/alignent assumptions");
- if (queue_length < 1) {
- AOS_LOG(FATAL, "queue length %d of %s needs to be at least 1\n",
- queue_length, name);
- }
+ CHECK_GE(queue_length, 1) << ": queue length " << queue_length << " of "
+ << name << " needs to be at least 1";
const size_t name_size = strlen(name) + 1;
char *temp = static_cast<char *>(shm_malloc(name_size));
@@ -249,8 +247,8 @@
printf("fetching queue %s\n", name);
}
if (mutex_lock(&global_core->mem_struct->queues.lock) != 0) {
- AOS_LOG(FATAL, "mutex_lock(%p) failed\n",
- &global_core->mem_struct->queues.lock);
+ LOG(FATAL) << "mutex_lock(" << &global_core->mem_struct->queues.lock
+ << ") failed";
}
RawQueue *current =
static_cast<RawQueue *>(global_core->mem_struct->queues.pointer);
@@ -302,14 +300,14 @@
bool RawQueue::DoWriteMessage(void *msg, Options<RawQueue> options) {
if (kWriteDebug) {
- printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options.printable());
+ printf("queue: %p->WriteMessage(%p, %x), len :%zu\n", this, msg, options.printable(), msg_length_);
}
bool signal_readable;
{
IPCMutexLocker locker(&data_lock_);
- AOS_CHECK(!locker.owner_died());
+ CHECK(!locker.owner_died());
int new_end;
while (true) {
@@ -334,7 +332,7 @@
if (kWriteDebug) {
printf("queue: going to wait for writable_ of %p\n", this);
}
- AOS_CHECK(!writable_.Wait());
+ CHECK(!writable_.Wait());
}
}
data_[data_end_] = msg;
@@ -390,7 +388,7 @@
if (wait_result == Condition::WaitResult::kOk) {
break;
}
- AOS_CHECK(wait_result != Condition::WaitResult::kOwnerDied);
+ CHECK(wait_result != Condition::WaitResult::kOwnerDied);
if (wait_result == Condition::WaitResult::kTimeout) {
return false;
}
@@ -427,7 +425,7 @@
void *msg = NULL;
IPCMutexLocker locker(&data_lock_);
- AOS_CHECK(!locker.owner_died());
+ CHECK(!locker.owner_died());
if (!ReadCommonStart(options, nullptr, chrono::nanoseconds(0))) {
if (kReadDebug) {
@@ -490,7 +488,7 @@
void *msg = NULL;
IPCMutexLocker locker(&data_lock_);
- AOS_CHECK(!locker.owner_died());
+ CHECK(!locker.owner_died());
if (!ReadCommonStart(options, index, timeout)) {
if (kReadDebug) {
diff --git a/aos/ipc_lib/queue.h b/aos/ipc_lib/queue.h
index 5b68f2e..6e07993 100644
--- a/aos/ipc_lib/queue.h
+++ b/aos/ipc_lib/queue.h
@@ -5,7 +5,7 @@
#include "aos/mutex/mutex.h"
#include "aos/condition.h"
#include "aos/util/options.h"
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
// TODO(brians) add valgrind client requests to the queue and shared_mem_malloc
// code to make checking for leaks work better
@@ -90,10 +90,11 @@
static constexpr Options<RawQueue> kWriteFailureOptions =
kNonBlock | kBlock | kOverride;
if (!options.NoOthersSet(kWriteFailureOptions)) {
- AOS_LOG(FATAL, "illegal write options in %x\n", options.printable());
+ LOG(FATAL) << "illegal write options in " << std::hex
+ << options.printable();
}
if (!options.ExactlyOneSet(kWriteFailureOptions)) {
- AOS_LOG(FATAL, "invalid write options %x\n", options.printable());
+ LOG(FATAL) << "invalid write options " << std::hex << options.printable();
}
return DoWriteMessage(msg, options);
}
@@ -123,7 +124,7 @@
CheckReadOptions(options);
static constexpr Options<RawQueue> kFromEndAndPeek = kFromEnd | kPeek;
if (options.AllSet(kFromEndAndPeek)) {
- AOS_LOG(FATAL, "ReadMessageIndex(kFromEnd | kPeek) is not allowed\n");
+ LOG(FATAL) << "ReadMessageIndex(kFromEnd | kPeek) is not allowed";
}
return DoReadMessageIndex(options, index, timeout);
}
@@ -161,11 +162,12 @@
static constexpr Options<RawQueue> kValidOptions =
kPeek | kFromEnd | kNonBlock | kBlock;
if (!options.NoOthersSet(kValidOptions)) {
- AOS_LOG(FATAL, "illegal read options in %x\n", options.printable());
+ LOG(FATAL) << "illegal read options in " << std::hex
+ << options.printable();
}
static constexpr Options<RawQueue> kBlockChoices = kNonBlock | kBlock;
if (!options.ExactlyOneSet(kBlockChoices)) {
- AOS_LOG(FATAL, "invalid read options %x\n", options.printable());
+ LOG(FATAL) << "invalid read options " << std::hex << options.printable();
}
}
diff --git a/aos/ipc_lib/queue_racer.cc b/aos/ipc_lib/queue_racer.cc
index 53490a0..bb754d8 100644
--- a/aos/ipc_lib/queue_racer.cc
+++ b/aos/ipc_lib/queue_racer.cc
@@ -82,12 +82,15 @@
//
// So, grab them in order.
const uint64_t finished_writes = finished_writes_.load();
- const uint32_t latest_queue_index_uint32_t = queue.LatestQueueIndex();
+ const QueueIndex latest_queue_index_queue_index =
+ queue.LatestQueueIndex();
const uint64_t started_writes = started_writes_.load();
+ const uint32_t latest_queue_index_uint32_t =
+ queue.LatestQueueIndex().index();
uint64_t latest_queue_index = latest_queue_index_uint32_t;
- if (latest_queue_index_uint32_t != LocklessQueue::empty_queue_index()) {
+ if (latest_queue_index_queue_index != LocklessQueue::empty_queue_index()) {
// If we got smaller, we wrapped.
if (latest_queue_index_uint32_t < last_queue_index) {
++wrap_count;
@@ -104,19 +107,19 @@
// If we are at the beginning, the queue needs to always return empty.
if (started_writes == 0) {
- EXPECT_EQ(latest_queue_index_uint32_t,
+ EXPECT_EQ(latest_queue_index_queue_index,
LocklessQueue::empty_queue_index());
EXPECT_EQ(finished_writes, 0);
} else {
if (finished_writes == 0) {
// Plausible to be at the beginning.
- if (latest_queue_index_uint32_t !=
+ if (latest_queue_index_queue_index !=
LocklessQueue::empty_queue_index()) {
// Otherwise, we have started. The queue is always allowed to
EXPECT_GE(started_writes, latest_queue_index + 1);
}
} else {
- EXPECT_NE(latest_queue_index_uint32_t,
+ EXPECT_NE(latest_queue_index_queue_index,
LocklessQueue::empty_queue_index());
// latest_queue_index is an index, not a count. So it always reads 1
// low.
diff --git a/aos/ipc_lib/queue_racer.h b/aos/ipc_lib/queue_racer.h
index 59435b1..eaeedd4 100644
--- a/aos/ipc_lib/queue_racer.h
+++ b/aos/ipc_lib/queue_racer.h
@@ -36,7 +36,7 @@
size_t CurrentIndex() {
LocklessQueue queue(memory_, config_);
- return queue.LatestQueueIndex();
+ return queue.LatestQueueIndex().index();
}
private:
diff --git a/aos/ipc_lib/raw_queue_test.cc b/aos/ipc_lib/raw_queue_test.cc
index aa3b335..3048e1b 100644
--- a/aos/ipc_lib/raw_queue_test.cc
+++ b/aos/ipc_lib/raw_queue_test.cc
@@ -191,7 +191,8 @@
case 0: // child
if (kForkSleep != chrono::milliseconds(0)) {
AOS_LOG(INFO, "pid %jd sleeping for %" PRId64 "ns\n",
- static_cast<intmax_t>(getpid()), kForkSleep.count());
+ static_cast<intmax_t>(getpid()),
+ static_cast<int64_t>(kForkSleep.count()));
this_thread::sleep_for(kForkSleep);
}
::aos::testing::PreventExit();
diff --git a/aos/ipc_lib/shared_mem.c b/aos/ipc_lib/shared_mem.cc
similarity index 71%
rename from aos/ipc_lib/shared_mem.c
rename to aos/ipc_lib/shared_mem.cc
index da55ebc..93ced7d 100644
--- a/aos/ipc_lib/shared_mem.c
+++ b/aos/ipc_lib/shared_mem.cc
@@ -10,9 +10,9 @@
#include <stdlib.h>
#include <assert.h>
-#include "aos/ipc_lib/core_lib.h"
-#include "aos/logging/logging.h"
#include "aos/ipc_lib/aos_sync.h"
+#include "aos/ipc_lib/core_lib.h"
+#include "glog/logging.h"
// the path for the shared memory segment. see shm_open(3) for restrictions
#define AOS_SHM_NAME "/aos_shared_mem"
@@ -61,7 +61,7 @@
if (shm == -1 && errno == EEXIST) {
printf("shared_mem: going to shm_unlink(%s)\n", global_core->shm_name);
if (shm_unlink(global_core->shm_name) == -1) {
- AOS_PLOG(WARNING, "shm_unlink(%s) failed", global_core->shm_name);
+ PLOG(WARNING) << "shm_unlink(" << global_core->shm_name << ") failed";
break;
}
} else {
@@ -73,22 +73,22 @@
global_core->owner = 0;
}
if (shm == -1) {
- AOS_PLOG(FATAL, "shm_open(%s, O_RDWR [| O_CREAT | O_EXCL, 0|0666) failed",
- global_core->shm_name);
+ PLOG(FATAL) << "shm_open(" << global_core->shm_name
+ << ", O_RDWR [| O_CREAT | O_EXCL, 0|0666) failed";
}
if (global_core->owner) {
- if (ftruncate(shm, SIZEOFSHMSEG) == -1) {
- AOS_PLOG(FATAL, "fruncate(%d, 0x%zx) failed", shm, (size_t)SIZEOFSHMSEG);
- }
+ PCHECK(ftruncate(shm, SIZEOFSHMSEG) == 0)
+ << ": fruncate(" << shm << ", 0x" << std::hex << (size_t)SIZEOFSHMSEG
+ << ") failed";
}
int flags = MAP_SHARED | MAP_FIXED;
if (lock) flags |= MAP_LOCKED | MAP_POPULATE;
void *shm_address = mmap((void *)SHM_START, SIZEOFSHMSEG,
PROT_READ | PROT_WRITE, flags, shm, 0);
- if (shm_address == MAP_FAILED) {
- AOS_PLOG(FATAL, "shared_mem: mmap(%p, 0x%zx, stuff, %x, %d, 0) failed",
- (void *)SHM_START, (size_t)SIZEOFSHMSEG, flags, shm);
- }
+ PCHECK(shm_address != MAP_FAILED)
+ << std::hex << "shared_mem: mmap(" << (void *)SHM_START << ", 0x"
+ << (size_t)SIZEOFSHMSEG << ", stuff, " << flags << ", " << shm
+ << ", 0) failed";
if (create) {
printf("shared_mem: creating %s, shm at: %p\n", global_core->shm_name,
shm_address);
@@ -96,19 +96,18 @@
printf("shared_mem: not creating, shm at: %p\n", shm_address);
}
if (close(shm) == -1) {
- AOS_PLOG(WARNING, "close(%d(=shm) failed", shm);
+ PLOG(WARNING) << "close(" << shm << "(=shm) failed";
}
- if (shm_address != (void *)SHM_START) {
- AOS_LOG(FATAL, "shm isn't at hard-coded %p. at %p instead\n",
- (void *)SHM_START, shm_address);
- }
+ PCHECK(shm_address == (void *)SHM_START)
+ << "shm isn't at hard-coded " << (void *)SHM_START << ". at "
+ << shm_address << " instead";
aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
- AOS_LOG(INFO, "shared_mem: end of create_shared_mem owner=%d\n",
- global_core->owner);
+ LOG(INFO) << "shared_mem: end of create_shared_mem owner="
+ << global_core->owner;
}
void aos_core_use_address_as_shared_mem(void *address, size_t size) {
- global_core->mem_struct = address;
+ global_core->mem_struct = reinterpret_cast<aos_shm_core_t *>(address);
global_core->size = size;
global_core->shared_mem =
(uint8_t *)address + sizeof(*global_core->mem_struct);
@@ -118,22 +117,19 @@
futex_set(&global_core->mem_struct->creation_condition);
} else {
if (futex_wait(&global_core->mem_struct->creation_condition) != 0) {
- AOS_LOG(FATAL, "waiting on creation_condition failed\n");
+ LOG(FATAL) << "waiting on creation_condition failed";
}
}
}
void aos_core_free_shared_mem() {
void *shm_address = global_core->shared_mem;
- if (munmap((void *)SHM_START, SIZEOFSHMSEG) == -1) {
- AOS_PLOG(FATAL, "munmap(%p, 0x%zx) failed", shm_address,
- (size_t)SIZEOFSHMSEG);
- }
+ PCHECK(munmap((void *)SHM_START, SIZEOFSHMSEG) != -1)
+ << ": munmap(" << shm_address << ", 0x" << std::hex
+ << (size_t)SIZEOFSHMSEG << ") failed";
if (global_core->owner) {
- if (shm_unlink(global_core->shm_name)) {
- AOS_PLOG(FATAL, "shared_mem: shm_unlink(%s) failed",
- global_core->shm_name);
- }
+ PCHECK(shm_unlink(global_core->shm_name) == 0)
+ << ": shared_mem: shm_unlink(" << global_core->shm_name << ") failed";
}
}
diff --git a/aos/ipc_lib/signalfd.cc b/aos/ipc_lib/signalfd.cc
index 045444b..af95598 100644
--- a/aos/ipc_lib/signalfd.cc
+++ b/aos/ipc_lib/signalfd.cc
@@ -5,12 +5,12 @@
#include <unistd.h>
#include <initializer_list>
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
namespace ipc_lib {
-SignalFd::SignalFd(::std::initializer_list<int> signals) {
+SignalFd::SignalFd(::std::initializer_list<unsigned int> signals) {
// Build up the mask with the provided signals.
sigemptyset(&mask_);
for (int signal : signals) {
@@ -18,7 +18,7 @@
}
// Then build a signalfd. Make it nonblocking so it works well with an epoll
// loop, and have it close on exec.
- AOS_PCHECK(fd_ = signalfd(-1, &mask_, SFD_NONBLOCK | SFD_CLOEXEC));
+ PCHECK((fd_ = signalfd(-1, &mask_, SFD_NONBLOCK | SFD_CLOEXEC)) != 0);
// Now that we have a consumer of the signal, block the signals so the
// signalfd gets them.
pthread_sigmask(SIG_BLOCK, &mask_, nullptr);
@@ -27,7 +27,7 @@
SignalFd::~SignalFd() {
// Unwind the constructor. Unblock the signals and close the fd.
pthread_sigmask(SIG_UNBLOCK, &mask_, nullptr);
- AOS_PCHECK(close(fd_));
+ PCHECK(close(fd_) == 0);
}
signalfd_siginfo SignalFd::Read() {
diff --git a/aos/ipc_lib/signalfd.h b/aos/ipc_lib/signalfd.h
index a545a80..7d2021a 100644
--- a/aos/ipc_lib/signalfd.h
+++ b/aos/ipc_lib/signalfd.h
@@ -13,7 +13,7 @@
public:
// Constructs a SignalFd for the provided list of signals.
// Blocks the signals at the same time in this thread.
- SignalFd(::std::initializer_list<int> signals);
+ SignalFd(::std::initializer_list<unsigned int> signals);
SignalFd(const SignalFd &) = delete;
SignalFd &operator=(const SignalFd &) = delete;
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 13a3a38..34cdffe 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -152,7 +152,7 @@
const absl::string_view data, flatbuffers::uoffset_t *table_end);
// Adds *_value for the provided field. If we are in a vector, queues the
- // data up in vector_elements_. Returns true on success.
+ // data up in vector_elements. Returns true on success.
bool AddElement(int field_index, int64_t int_value);
bool AddElement(int field_index, double double_value);
bool AddElement(int field_index, const ::std::string &data);
@@ -184,19 +184,19 @@
// Field elements that need to be inserted.
::std::vector<FieldElement> elements;
+
+ // For scalar types (not strings, and not nested tables), the vector ends
+ // up being implemented as a start and end, and a block of data. So we
+ // can't just push offsets in as we go. We either need to reproduce the
+ // logic inside flatbuffers, or build up vectors of the data. Vectors will
+ // be a bit of extra stack space, but whatever.
+ //
+ // Strings and nested structures are vectors of offsets.
+ // into the vector. Once you get to the end, you build up a vector and
+ // push that into the field.
+ ::std::vector<Element> vector_elements;
};
::std::vector<FlatBufferContext> stack_;
-
- // For scalar types (not strings, and not nested tables), the vector ends
- // up being implemented as a start and end, and a block of data. So we
- // can't just push offsets in as we go. We either need to reproduce the
- // logic inside flatbuffers, or build up vectors of the data. Vectors will
- // be a bit of extra stack space, but whatever.
- //
- // Strings and nested structures are vectors of offsets.
- // into the vector. Once you get to the end, you build up a vector and
- // push that into the field.
- ::std::vector<Element> vector_elements_;
};
bool JsonParser::DoParse(const flatbuffers::TypeTable *typetable,
@@ -225,7 +225,7 @@
case Tokenizer::TokenType::kStartObject: // {
if (stack_.size() == 0) {
- stack_.push_back({typetable, false, -1, "", {}});
+ stack_.push_back({typetable, false, -1, "", {}, {}});
} else {
int field_index = stack_.back().field_index;
@@ -241,7 +241,7 @@
flatbuffers::TypeFunction type_function =
stack_.back().typetable->type_refs[type_code.sequence_ref];
- stack_.push_back({type_function(), false, -1, "", {}});
+ stack_.push_back({type_function(), false, -1, "", {}, {}});
}
break;
case Tokenizer::TokenType::kEndObject: // }
@@ -267,7 +267,7 @@
// Do the right thing if we are in a vector.
if (in_vector()) {
- vector_elements_.emplace_back(
+ stack_.back().vector_elements.emplace_back(
flatbuffers::Offset<flatbuffers::String>(end));
} else {
stack_.back().elements.emplace_back(
@@ -363,7 +363,7 @@
}
if (in_vector()) {
- vector_elements_.emplace_back(int_value);
+ stack_.back().vector_elements.emplace_back(int_value);
} else {
stack_.back().elements.emplace_back(field_index, int_value);
}
@@ -380,7 +380,7 @@
}
if (in_vector()) {
- vector_elements_.emplace_back(double_value);
+ stack_.back().vector_elements.emplace_back(double_value);
} else {
stack_.back().elements.emplace_back(field_index, double_value);
}
@@ -396,9 +396,61 @@
return false;
}
- if (in_vector()) {
- vector_elements_.emplace_back(fbb_.CreateString(data));
+ const flatbuffers::ElementaryType elementary_type =
+ static_cast<flatbuffers::ElementaryType>(type_code.base_type);
+ switch (elementary_type) {
+ case flatbuffers::ET_CHAR:
+ case flatbuffers::ET_UCHAR:
+ case flatbuffers::ET_SHORT:
+ case flatbuffers::ET_USHORT:
+ case flatbuffers::ET_INT:
+ case flatbuffers::ET_UINT:
+ case flatbuffers::ET_LONG:
+ case flatbuffers::ET_ULONG:
+ if (type_code.sequence_ref != -1) {
+ // We have an enum.
+ const flatbuffers::TypeTable *type_table = stack_.back().typetable;
+ flatbuffers::TypeFunction type_function =
+ type_table->type_refs[type_code.sequence_ref];
+ const flatbuffers::TypeTable *enum_type_table = type_function();
+
+ CHECK_EQ(enum_type_table->st, flatbuffers::ST_ENUM);
+
+ int64_t int_value = 0;
+ bool found = false;
+ for (size_t i = 0; i < enum_type_table->num_elems; ++i) {
+ if (data == enum_type_table->names[i]) {
+ int_value = i;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) {
+ printf("Enum value '%s' not found for field '%s'\n", data.c_str(),
+ type_table->names[field_index]);
+ return false;
+ }
+
+ if (in_vector()) {
+ stack_.back().vector_elements.emplace_back(int_value);
+ } else {
+ stack_.back().elements.emplace_back(field_index, int_value);
+ }
+ return true;
+ }
+ case flatbuffers::ET_UTYPE:
+ case flatbuffers::ET_BOOL:
+ case flatbuffers::ET_FLOAT:
+ case flatbuffers::ET_DOUBLE:
+ case flatbuffers::ET_STRING:
+ case flatbuffers::ET_SEQUENCE:
+ break;
+ }
+
+ if (in_vector()) {
+ stack_.back().vector_elements.emplace_back(fbb_.CreateString(data));
} else {
stack_.back().elements.emplace_back(field_index, fbb_.CreateString(data));
}
@@ -540,8 +592,6 @@
const flatbuffers::ElementaryType elementary_type =
static_cast<flatbuffers::ElementaryType>(type_code.base_type);
switch (elementary_type) {
- case flatbuffers::ET_UTYPE:
- case flatbuffers::ET_BOOL:
case flatbuffers::ET_CHAR:
case flatbuffers::ET_UCHAR:
case flatbuffers::ET_SHORT:
@@ -550,14 +600,20 @@
case flatbuffers::ET_UINT:
case flatbuffers::ET_LONG:
case flatbuffers::ET_ULONG:
+ case flatbuffers::ET_UTYPE:
+ case flatbuffers::ET_BOOL:
case flatbuffers::ET_FLOAT:
case flatbuffers::ET_DOUBLE:
printf("Mismatched type for field '%s'. Got: string, expected %s\n",
typetable->names[field_index],
ElementaryTypeName(elementary_type));
+ CHECK_EQ(type_code.sequence_ref, -1)
+ << ": Field name " << typetable->names[field_index]
+ << " Got string expected " << ElementaryTypeName(elementary_type);
return false;
- case flatbuffers::ET_SEQUENCE:
case flatbuffers::ET_STRING:
+ CHECK_EQ(type_code.sequence_ref, -1);
+ case flatbuffers::ET_SEQUENCE:
fbb->AddOffset(field_offset, offset_element);
return true;
}
@@ -573,17 +629,19 @@
// Vectors have a start (unfortunately which needs to know the size)
fbb_.StartVector(
- vector_elements_.size(),
+ stack_.back().vector_elements.size(),
flatbuffers::InlineSize(elementary_type, stack_.back().typetable));
// Then the data (in reverse order for some reason...)
- for (size_t i = vector_elements_.size(); i > 0;) {
- const Element &element = vector_elements_[--i];
+ for (size_t i = stack_.back().vector_elements.size(); i > 0;) {
+ const Element &element = stack_.back().vector_elements[--i];
switch (element.type) {
case Element::ElementType::INT:
if (!PushElement(elementary_type, element.int_element)) return false;
break;
case Element::ElementType::DOUBLE:
+ CHECK_EQ(type_code.sequence_ref, -1)
+ << ": Field index is " << field_index;
if (!PushElement(elementary_type, element.double_element)) return false;
break;
case Element::ElementType::OFFSET:
@@ -595,8 +653,8 @@
// Then an End which is placed into the buffer the same as any other offset.
stack_.back().elements.emplace_back(
field_index, flatbuffers::Offset<flatbuffers::String>(
- fbb_.EndVector(vector_elements_.size())));
- vector_elements_.clear();
+ fbb_.EndVector(stack_.back().vector_elements.size())));
+ stack_.back().vector_elements.clear();
return true;
}
diff --git a/aos/json_to_flatbuffer.fbs b/aos/json_to_flatbuffer.fbs
index 2e90a53..8942cc8 100644
--- a/aos/json_to_flatbuffer.fbs
+++ b/aos/json_to_flatbuffer.fbs
@@ -1,5 +1,26 @@
namespace aos.testing;
+enum BaseType : byte {
+ None,
+ UType,
+ Bool,
+ Byte,
+ UByte,
+ Short,
+ UShort,
+ Int,
+ UInt,
+ Long,
+ ULong,
+ Float,
+ Double,
+ String,
+ Vector,
+ Obj, // Used for tables & structs.
+ Union,
+ Array
+}
+
table Location {
name:string;
type:string;
@@ -18,6 +39,14 @@
maps:[Map];
}
+table VectorOfStrings {
+ str:[string];
+}
+
+table VectorOfVectorOfString {
+ v:[VectorOfStrings];
+}
+
table Configuration {
locations:[Location] (id: 0);
maps:[Map] (id: 1);
@@ -48,32 +77,37 @@
foo_string:string (id: 15);
+ foo_enum:BaseType (id: 16);
+ foo_enum_default:BaseType = None (id: 17);
+
// Test vectors now.
- vector_foo_byte:[byte] (id: 16);
- vector_foo_ubyte:[ubyte] (id: 17);
- vector_foo_bool:[bool] (id: 18);
+ vector_foo_byte:[byte] (id: 18);
+ vector_foo_ubyte:[ubyte] (id: 19);
+ vector_foo_bool:[bool] (id: 20);
- vector_foo_short:[short] (id: 19);
- vector_foo_ushort:[ushort] (id: 20);
+ vector_foo_short:[short] (id: 21);
+ vector_foo_ushort:[ushort] (id: 22);
- vector_foo_int:[int] (id: 21);
- vector_foo_uint:[uint] (id: 22);
+ vector_foo_int:[int] (id: 23);
+ vector_foo_uint:[uint] (id: 24);
- vector_foo_long:[long] (id: 23);
- vector_foo_ulong:[ulong] (id: 24);
+ vector_foo_long:[long] (id: 25);
+ vector_foo_ulong:[ulong] (id: 26);
- vector_foo_float:[float] (id: 25);
- vector_foo_double:[double] (id: 26);
+ vector_foo_float:[float] (id: 27);
+ vector_foo_double:[double] (id: 28);
- vector_foo_string:[string] (id: 27);
+ vector_foo_string:[string] (id: 29);
+
+ vector_foo_enum:[BaseType] (id: 30);
// And a simple nested application.
- single_application:Application (id: 28);
-
- // TODO(austin): enum
+ single_application:Application (id: 31);
// And nest this object to get some crazy coverage.
- nested_config:Configuration (id: 29);
+ nested_config:Configuration (id: 32);
+
+ vov:VectorOfVectorOfString (id: 33);
}
root_type Configuration;
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index a048e0a..84ac0ed 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -57,6 +57,12 @@
EXPECT_TRUE(JsonAndBack("{ \"foo_float\": 5.0 }"));
EXPECT_TRUE(JsonAndBack("{ \"foo_double\": 5.0 }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_enum\": \"None\" }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_enum\": \"UType\" }"));
+
+ EXPECT_TRUE(JsonAndBack("{ \"foo_enum_default\": \"None\" }"));
+ EXPECT_TRUE(JsonAndBack("{ \"foo_enum_default\": \"UType\" }"));
+
EXPECT_TRUE(JsonAndBack("{ \"foo_string\": \"baz\" }"));
}
@@ -72,6 +78,13 @@
EXPECT_FALSE(JsonAndBack("{ \"foo\": 5 }"));
}
+// Tests that an invalid enum type is handled correctly.
+TEST_F(JsonToFlatbufferTest, InvalidEnumName) {
+ EXPECT_FALSE(JsonAndBack("{ \"foo_enum\": \"5ype\" }"));
+
+ EXPECT_FALSE(JsonAndBack("{ \"foo_enum_default\": \"7ype\" }"));
+}
+
// Test that adding a duplicate field results in an error.
TEST_F(JsonToFlatbufferTest, DuplicateField) {
EXPECT_FALSE(
@@ -106,19 +119,29 @@
// Test arrays of simple types.
TEST_F(JsonToFlatbufferTest, Array) {
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_byte\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_byte\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ubyte\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ubyte\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_short\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_short\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ushort\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ushort\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_int\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_int\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_uint\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_uint\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_long\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_long\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ulong\": [ 9, 7, 1 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_ulong\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_float\": [ 9.0, 7.0, 1.0 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_float\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_double\": [ ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_float\": [ 9, 7, 1 ] }",
"{ \"vector_foo_float\": [ 9.0, 7.0, 1.0 ] }"));
@@ -126,6 +149,9 @@
"{ \"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
EXPECT_TRUE(JsonAndBack("{ \"vector_foo_string\": [ \"bar\", \"baz\" ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_string\": [ ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_enum\": [ \"None\", \"UType\", \"Bool\" ] }"));
+ EXPECT_TRUE(JsonAndBack("{ \"vector_foo_enum\": [ ] }"));
}
// Test nested messages, and arrays of nested messages.
@@ -133,8 +159,13 @@
EXPECT_TRUE(
JsonAndBack("{ \"single_application\": { \"name\": \"woot\" } }"));
+ EXPECT_TRUE(JsonAndBack("{ \"single_application\": { } }"));
+
EXPECT_TRUE(JsonAndBack(
"{ \"apps\": [ { \"name\": \"woot\" }, { \"name\": \"wow\" } ] }"));
+
+ EXPECT_TRUE(JsonAndBack(
+ "{ \"apps\": [ { }, { } ] }"));
}
// Test that we can parse an empty message.
@@ -158,8 +189,13 @@
"\"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
}
+// Tests that multiple arrays get properly handled.
+TEST_F(JsonToFlatbufferTest, NestedArrays) {
+ EXPECT_TRUE(
+ JsonAndBack("{ \"vov\": { \"v\": [ { \"str\": [ \"a\", \"b\" ] }, { \"str\": [ \"c\", \"d\" ] } ] } }"));
+}
+
// TODO(austin): Missmatched values.
-// TODO(austin): enums
//
// TODO(austin): unions?
diff --git a/aos/json_tokenizer.cc b/aos/json_tokenizer.cc
index 78bf46e..e3aa7ea 100644
--- a/aos/json_tokenizer.cc
+++ b/aos/json_tokenizer.cc
@@ -1,5 +1,7 @@
#include "aos/json_tokenizer.h"
+#include <cerrno>
+
namespace aos {
void Tokenizer::ConsumeWhitespace() {
@@ -267,7 +269,13 @@
ConsumeWhitespace();
- state_ = State::kExpectField;
+ // And then if we encounter the end again, go to the end state.
+ if (Consume("}")) {
+ ConsumeWhitespace();
+ state_ = State::kExpectObjectEnd;
+ } else {
+ state_ = State::kExpectField;
+ }
return TokenType::kStartObject;
} else if (Consume("[")) {
// Values are in arrays. Record and recurse.
@@ -293,6 +301,22 @@
field_value_ = "false";
result = TokenType::kFalseValue;
} else {
+ switch (object_type_.back()) {
+ case ObjectType::kObject:
+ if (Consume("}")) {
+ ConsumeWhitespace();
+ state_ = State::kExpectObjectEnd;
+ return Next();
+ }
+ break;
+ case ObjectType::kArray:
+ if (Consume("]")) {
+ ConsumeWhitespace();
+ state_ = State::kExpectArrayEnd;
+ return Next();
+ }
+ break;
+ }
// Couldn't parse, so we have a syntax error.
fprintf(stderr, "Error line %d, invalid field value.\n", linenumber_);
}
diff --git a/aos/libc/BUILD b/aos/libc/BUILD
index 48a9ffd..234c29a 100644
--- a/aos/libc/BUILD
+++ b/aos/libc/BUILD
@@ -9,7 +9,7 @@
"aos_strsignal.h",
],
deps = [
- "//aos/logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -20,7 +20,6 @@
],
deps = [
":aos_strsignal",
- "//aos/logging",
"//aos/testing:googletest",
],
)
diff --git a/aos/libc/aos_strsignal.cc b/aos/libc/aos_strsignal.cc
index a3331f9..9262ac6 100644
--- a/aos/libc/aos_strsignal.cc
+++ b/aos/libc/aos_strsignal.cc
@@ -2,14 +2,15 @@
#include <signal.h>
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
const char *aos_strsignal(int signal) {
static thread_local char buffer[512];
if (signal >= SIGRTMIN && signal <= SIGRTMAX) {
- AOS_CHECK(snprintf(buffer, sizeof(buffer), "Real-time signal %d",
- signal - SIGRTMIN) > 0);
+ CHECK_GT(snprintf(buffer, sizeof(buffer), "Real-time signal %d",
+ signal - SIGRTMIN),
+ 0);
return buffer;
}
@@ -17,6 +18,6 @@
return sys_siglist[signal];
}
- AOS_CHECK(snprintf(buffer, sizeof(buffer), "Unknown signal %d", signal) > 0);
+ CHECK_GT(snprintf(buffer, sizeof(buffer), "Unknown signal %d", signal), 0);
return buffer;
}
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index 60c561b..6157e52 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -20,25 +20,6 @@
],
)
-cc_library(
- name = "replay",
- srcs = [
- "replay.cc",
- ],
- hdrs = [
- "replay.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":binary_log_file",
- ":logging",
- ":queue_logging",
- "//aos:queues",
- "//aos/events:event-loop",
- "//aos/ipc_lib:queue",
- ],
-)
-
cc_binary(
name = "binary_log_writer",
srcs = [
@@ -52,7 +33,6 @@
"//aos:configuration",
"//aos:die",
"//aos:init",
- "//aos:queue_types",
"//aos/ipc_lib:queue",
"//aos/time",
],
@@ -85,7 +65,6 @@
":logging",
"//aos:configuration",
"//aos:init",
- "//aos:queue_types",
"//aos/util:string_to_num",
],
)
@@ -123,42 +102,6 @@
)
cc_library(
- name = "queue_logging",
- srcs = [
- "queue_logging.cc",
- ],
- hdrs = [
- "queue_logging.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":logging",
- ":sizes",
- "//aos:die",
- "//aos:queue_types",
- ],
-)
-
-cc_library(
- name = "matrix_logging",
- srcs = [
- "matrix_logging.cc",
- ],
- hdrs = [
- "matrix_logging.h",
- ],
- visibility = ["//visibility:public"],
- deps = [
- ":logging",
- ":sizes",
- "//aos:die",
- "//aos:generated_queue_headers",
- "//aos:queue_types",
- "//third_party/eigen",
- ],
-)
-
-cc_library(
name = "printf_formats",
hdrs = [
"printf_formats.h",
@@ -183,11 +126,11 @@
visibility = ["//visibility:public"],
deps = [
":logging",
+ ":printf_formats",
":sizes",
"//aos:die",
"//aos:macros",
"//aos:once",
- "//aos:queue_types",
"//aos/ipc_lib:queue",
"//aos/mutex",
"//aos/time",
diff --git a/aos/logging/binary_log_writer.cc b/aos/logging/binary_log_writer.cc
index be19d45..5b1dc38 100644
--- a/aos/logging/binary_log_writer.cc
+++ b/aos/logging/binary_log_writer.cc
@@ -18,7 +18,6 @@
#include "aos/die.h"
#include "aos/logging/binary_log_file.h"
#include "aos/logging/implementations.h"
-#include "aos/queue_types.h"
#include "aos/time/time.h"
#include "aos/configuration.h"
#include "aos/init.h"
@@ -29,41 +28,6 @@
namespace linux_code {
namespace {
-void CheckTypeWritten(uint32_t type_id, LogFileWriter *writer,
- ::std::unordered_set<uint32_t> *written_type_ids) {
- if (written_type_ids->count(type_id) > 0) return;
- if (MessageType::IsPrimitive(type_id)) return;
-
- const MessageType &type = type_cache::Get(type_id);
- for (int i = 0; i < type.number_fields; ++i) {
- CheckTypeWritten(type.fields[i]->type, writer, written_type_ids);
- }
-
- char buffer[1024];
- ssize_t size = type.Serialize(buffer, sizeof(buffer));
- if (size == -1) {
- AOS_LOG(WARNING, "%zu-byte buffer is too small to serialize type %s\n",
- sizeof(buffer), type.name.c_str());
- return;
- }
- LogFileMessageHeader *const output =
- writer->GetWritePosition(sizeof(LogFileMessageHeader) + size);
-
- output->time_sec = output->time_nsec = 0;
- output->source = getpid();
- output->name_size = 0;
- output->sequence = 0;
- output->level = FATAL;
-
- memcpy(output + 1, buffer, size);
- output->message_size = size;
-
- output->type = LogFileMessageHeader::MessageType::kStructType;
- futex_set(&output->marker);
-
- written_type_ids->insert(type_id);
-}
-
void AllocateLogName(char **filename, const char *directory) {
int fileindex = 0;
DIR *const d = opendir(directory);
@@ -205,7 +169,6 @@
RawQueue *queue = GetLoggingQueue();
::std::unordered_set<uint32_t> written_type_ids;
- off_t clear_type_ids_cookie = 0;
while (true) {
const LogMessage *const msg =
@@ -222,21 +185,6 @@
const size_t raw_output_length =
sizeof(LogFileMessageHeader) + msg->name_length + msg->message_length;
size_t output_length = raw_output_length;
- if (msg->type == LogMessage::Type::kStruct) {
- output_length += sizeof(msg->structure.type_id) + sizeof(uint32_t) +
- msg->structure.string_length;
- if (writer.ShouldClearSeekableData(&clear_type_ids_cookie,
- output_length)) {
- writer.ForceNewPage();
- written_type_ids.clear();
- }
- CheckTypeWritten(msg->structure.type_id, &writer, &written_type_ids);
- } else if (msg->type == LogMessage::Type::kMatrix) {
- output_length +=
- sizeof(msg->matrix.type) + sizeof(uint32_t) + sizeof(uint16_t) +
- sizeof(uint16_t) + msg->matrix.string_length;
- AOS_CHECK(MessageType::IsPrimitive(msg->matrix.type));
- }
LogFileMessageHeader *const output = writer.GetWritePosition(output_length);
char *output_strings = reinterpret_cast<char *>(output) + sizeof(*output);
output->name_size = msg->name_length;
@@ -254,50 +202,6 @@
msg->message_length);
output->type = LogFileMessageHeader::MessageType::kString;
break;
- case LogMessage::Type::kStruct: {
- char *position = output_strings + msg->name_length;
-
- memcpy(position, &msg->structure.type_id,
- sizeof(msg->structure.type_id));
- position += sizeof(msg->structure.type_id);
- output->message_size += sizeof(msg->structure.type_id);
-
- const uint32_t length = msg->structure.string_length;
- memcpy(position, &length, sizeof(length));
- position += sizeof(length);
- memcpy(position, msg->structure.serialized,
- length + msg->message_length);
- position += length + msg->message_length;
- output->message_size += sizeof(length) + length;
-
- output->type = LogFileMessageHeader::MessageType::kStruct;
- } break;
- case LogMessage::Type::kMatrix: {
- char *position = output_strings + msg->name_length;
-
- memcpy(position, &msg->matrix.type, sizeof(msg->matrix.type));
- position += sizeof(msg->matrix.type);
- output->message_size += sizeof(msg->matrix.type);
-
- uint32_t length = msg->matrix.string_length;
- memcpy(position, &length, sizeof(length));
- position += sizeof(length);
- output->message_size += sizeof(length);
-
- uint16_t rows = msg->matrix.rows, cols = msg->matrix.cols;
- memcpy(position, &rows, sizeof(rows));
- position += sizeof(rows);
- memcpy(position, &cols, sizeof(cols));
- position += sizeof(cols);
- output->message_size += sizeof(rows) + sizeof(cols);
- AOS_CHECK_EQ(msg->message_length,
- MessageType::Sizeof(msg->matrix.type) * rows * cols);
-
- memcpy(position, msg->matrix.data, msg->message_length + length);
- output->message_size += length;
-
- output->type = LogFileMessageHeader::MessageType::kMatrix;
- } break;
}
if (output->message_size - msg->message_length !=
diff --git a/aos/logging/context.cc b/aos/logging/context.cc
index 2fcb301..72c1970 100644
--- a/aos/logging/context.cc
+++ b/aos/logging/context.cc
@@ -1,5 +1,9 @@
#include "aos/logging/context.h"
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE /* See feature_test_macros(7) */
+#endif
+
#include <string.h>
#include <sys/prctl.h>
#include <sys/types.h>
@@ -7,6 +11,11 @@
#include <string>
+#include <errno.h>
+
+extern char *program_invocation_name;
+extern char *program_invocation_short_name;
+
#include "aos/die.h"
#include "aos/complex_thread_local.h"
diff --git a/aos/logging/implementations.cc b/aos/logging/implementations.cc
index 632d90b..2220e4e 100644
--- a/aos/logging/implementations.cc
+++ b/aos/logging/implementations.cc
@@ -8,7 +8,6 @@
#include "aos/die.h"
#include "aos/logging/printf_formats.h"
-#include "aos/queue_types.h"
#include "aos/time/time.h"
#include "aos/ipc_lib/queue.h"
#include "aos/once.h"
@@ -23,10 +22,15 @@
// Some of the things specified in the LogImplementation documentation doesn't
// apply here (mostly the parts about being able to use AOS_LOG) because this is
// the root one.
-class RootLogImplementation : public SimpleLogImplementation {
+class RootLogImplementation : public LogImplementation {
public:
void have_other_implementation() { only_implementation_ = false; }
+ protected:
+ virtual ::aos::monotonic_clock::time_point monotonic_now() const {
+ return ::aos::monotonic_clock::now();
+ }
+
private:
void set_next(LogImplementation *) override {
AOS_LOG(FATAL, "can't have a next logger from here\n");
@@ -101,66 +105,6 @@
} // namespace
-void FillInMessageStructure(bool add_to_type_cache, log_level level,
- monotonic_clock::time_point monotonic_now,
- const ::std::string &message_string, size_t size,
- const MessageType *type,
- const ::std::function<size_t(char *)> &serialize,
- LogMessage *message) {
- if (add_to_type_cache) {
- type_cache::AddShm(type->id);
- }
- message->structure.type_id = type->id;
-
- FillInMessageBase(level, monotonic_now, message);
-
- if (message_string.size() + size > sizeof(message->structure.serialized)) {
- AOS_LOG(
- FATAL,
- "serialized struct %s (size %zd + %zd > %zd) and message %s too big\n",
- type->name.c_str(), message_string.size(), size,
- sizeof(message->structure.serialized), message_string.c_str());
- }
- message->structure.string_length = message_string.size();
- memcpy(message->structure.serialized, message_string.data(),
- message->structure.string_length);
-
- message->message_length = serialize(
- &message->structure.serialized[message->structure.string_length]);
- message->type = LogMessage::Type::kStruct;
-}
-
-void FillInMessageMatrix(log_level level,
- monotonic_clock::time_point monotonic_now,
- const ::std::string &message_string, uint32_t type_id,
- int rows, int cols, const void *data,
- LogMessage *message) {
- AOS_CHECK(MessageType::IsPrimitive(type_id));
- message->matrix.type = type_id;
-
- const auto element_size = MessageType::Sizeof(type_id);
-
- FillInMessageBase(level, monotonic_now, message);
-
- message->message_length = rows * cols * element_size;
- if (message_string.size() + message->message_length >
- sizeof(message->matrix.data)) {
- AOS_LOG(FATAL,
- "%dx%d matrix of type %" PRIu32
- " (size %u) and message %s is too big\n",
- rows, cols, type_id, element_size, message_string.c_str());
- }
- message->matrix.string_length = message_string.size();
- memcpy(message->matrix.data, message_string.data(),
- message->matrix.string_length);
-
- message->matrix.rows = rows;
- message->matrix.cols = cols;
- SerializeMatrix(type_id, &message->matrix.data[message->matrix.string_length],
- data, rows, cols);
- message->type = LogMessage::Type::kMatrix;
-}
-
void FillInMessage(log_level level, monotonic_clock::time_point monotonic_now,
const char *format, va_list ap, LogMessage *message) {
FillInMessageBase(level, monotonic_now, message);
@@ -180,105 +124,12 @@
fprintf(output, AOS_LOGGING_BASE_FORMAT "%.*s", BASE_ARGS,
static_cast<int>(message.message_length), message.message);
break;
- case LogMessage::Type::kStruct: {
- char buffer[4096];
- size_t output_length = sizeof(buffer);
- size_t input_length = message.message_length;
- if (!PrintMessage(
- buffer, &output_length,
- message.structure.serialized + message.structure.string_length,
- &input_length, type_cache::Get(message.structure.type_id))) {
- AOS_LOG(
- FATAL,
- "printing message (%.*s) of type %s into %zu-byte buffer failed\n",
- static_cast<int>(message.message_length), message.message,
- type_cache::Get(message.structure.type_id).name.c_str(),
- sizeof(buffer));
- }
- if (input_length > 0) {
- AOS_LOG(WARNING, "%zu extra bytes on message of type %s\n",
- input_length,
- type_cache::Get(message.structure.type_id).name.c_str());
- }
- fprintf(output, AOS_LOGGING_BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
- static_cast<int>(message.structure.string_length),
- message.structure.serialized,
- static_cast<int>(sizeof(buffer) - output_length), buffer);
- } break;
- case LogMessage::Type::kMatrix: {
- char buffer[1024];
- size_t output_length = sizeof(buffer);
- if (message.message_length !=
- static_cast<size_t>(message.matrix.rows * message.matrix.cols *
- MessageType::Sizeof(message.matrix.type))) {
- AOS_LOG(FATAL, "expected %d bytes of matrix data but have %zu\n",
- message.matrix.rows * message.matrix.cols *
- MessageType::Sizeof(message.matrix.type),
- message.message_length);
- }
- if (!PrintMatrix(buffer, &output_length,
- message.matrix.data + message.matrix.string_length,
- message.matrix.type, message.matrix.rows,
- message.matrix.cols)) {
- AOS_LOG(FATAL, "printing %dx%d matrix of type %" PRIu32 " failed\n",
- message.matrix.rows, message.matrix.cols, message.matrix.type);
- }
- fprintf(output, AOS_LOGGING_BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
- static_cast<int>(message.matrix.string_length),
- message.matrix.data,
- static_cast<int>(sizeof(buffer) - output_length), buffer);
- } break;
}
#undef BASE_ARGS
}
} // namespace internal
-void SimpleLogImplementation::LogStruct(
- log_level level, const ::std::string &message, size_t size,
- const MessageType *type, const ::std::function<size_t(char *)> &serialize) {
- char serialized[1024];
- if (size > sizeof(serialized)) {
- AOS_LOG(FATAL, "structure of type %s too big to serialize\n",
- type->name.c_str());
- }
- size_t used = serialize(serialized);
- char printed[1024];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
- AOS_LOG(FATAL,
- "PrintMessage(%p, %p(=%zd), %p, %p(=%zd), %p(name=%s)) failed\n",
- printed, &printed_bytes, printed_bytes, serialized, &used, used,
- type, type->name.c_str());
- }
- DoLogVariadic(level, "%.*s: %.*s\n", static_cast<int>(message.size()),
- message.data(),
- static_cast<int>(sizeof(printed) - printed_bytes), printed);
-}
-
-void SimpleLogImplementation::LogMatrix(
- log_level level, const ::std::string &message, uint32_t type_id,
- int rows, int cols, const void *data) {
- char serialized[1024];
- if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
- sizeof(serialized)) {
- AOS_LOG(FATAL, "matrix of size %u too big to serialize\n",
- rows * cols * MessageType::Sizeof(type_id));
- }
- SerializeMatrix(type_id, serialized, data, rows, cols);
- char printed[1024];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
- AOS_LOG(FATAL,
- "PrintMatrix(%p, %p(=%zd), %p, %" PRIu32 ", %d, %d) failed\n",
- printed, &printed_bytes, printed_bytes, serialized, type_id, rows,
- cols);
- }
- DoLogVariadic(level, "%.*s: %.*s\n", static_cast<int>(message.size()),
- message.data(),
- static_cast<int>(sizeof(printed) - printed_bytes), printed);
-}
-
void HandleMessageLogImplementation::DoLog(log_level level, const char *format,
va_list ap) {
LogMessage message;
@@ -286,25 +137,6 @@
HandleMessage(message);
}
-void HandleMessageLogImplementation::LogStruct(
- log_level level, const ::std::string &message_string, size_t size,
- const MessageType *type, const ::std::function<size_t(char *)> &serialize) {
- LogMessage message;
- internal::FillInMessageStructure(fill_type_cache(), level, monotonic_now(),
- message_string, size, type, serialize,
- &message);
- HandleMessage(message);
-}
-
-void HandleMessageLogImplementation::LogMatrix(
- log_level level, const ::std::string &message_string, uint32_t type_id,
- int rows, int cols, const void *data) {
- LogMessage message;
- internal::FillInMessageMatrix(level, monotonic_now(), message_string, type_id,
- rows, cols, data, &message);
- HandleMessage(message);
-}
-
StreamLogImplementation::StreamLogImplementation(FILE *stream)
: stream_(stream) {}
@@ -415,25 +247,6 @@
internal::FillInMessage(level, monotonic_now(), format, ap, message);
Write(message);
}
-
- void LogStruct(log_level level, const ::std::string &message_string,
- size_t size, const MessageType *type,
- const ::std::function<size_t(char *)> &serialize) override {
- LogMessage *message = GetMessageOrDie();
- internal::FillInMessageStructure(fill_type_cache(), level, monotonic_now(),
- message_string, size, type, serialize,
- message);
- Write(message);
- }
-
- void LogMatrix(log_level level, const ::std::string &message_string,
- uint32_t type_id, int rows, int cols,
- const void *data) override {
- LogMessage *message = GetMessageOrDie();
- internal::FillInMessageMatrix(level, monotonic_now(), message_string,
- type_id, rows, cols, data, message);
- Write(message);
- }
};
} // namespace
diff --git a/aos/logging/implementations.h b/aos/logging/implementations.h
index 5f7e1c5..27c0472 100644
--- a/aos/logging/implementations.h
+++ b/aos/logging/implementations.h
@@ -43,7 +43,7 @@
// Contains all of the information about a given logging call.
struct LogMessage {
enum class Type : uint8_t {
- kString, kStruct, kMatrix
+ kString
};
int32_t seconds, nseconds;
@@ -100,23 +100,6 @@
return LOG_UNKNOWN;
}
-// A LogImplementation where LogStruct and LogMatrix just create a string with
-// PrintMessage and then forward on to DoLog.
-class SimpleLogImplementation : public LogImplementation {
- protected:
- virtual ::aos::monotonic_clock::time_point monotonic_now() const {
- return ::aos::monotonic_clock::now();
- }
-
- private:
- void LogStruct(log_level level, const ::std::string &message, size_t size,
- const MessageType *type,
- const ::std::function<size_t(char *)> &serialize) override;
- void LogMatrix(log_level level, const ::std::string &message,
- uint32_t type_id, int rows, int cols,
- const void *data) override;
-};
-
// Implements all of the DoLog* methods in terms of a (pure virtual in this
// class) HandleMessage method that takes a pointer to the message.
class HandleMessageLogImplementation : public LogImplementation {
@@ -128,12 +111,6 @@
private:
__attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
void DoLog(log_level level, const char *format, va_list ap) override;
- void LogStruct(log_level level, const ::std::string &message_string,
- size_t size, const MessageType *type,
- const ::std::function<size_t(char *)> &serialize) override;
- void LogMatrix(log_level level, const ::std::string &message_string,
- uint32_t type_id, int rows, int cols,
- const void *data) override;
virtual void HandleMessage(const LogMessage &message) = 0;
};
@@ -186,23 +163,6 @@
// goes.
namespace internal {
-// Fills in all the parts of message according to the given inputs (with type
-// kStruct).
-void FillInMessageStructure(bool add_to_type_cache, log_level level,
- ::aos::monotonic_clock::time_point monotonic_now,
- const ::std::string &message_string, size_t size,
- const MessageType *type,
- const ::std::function<size_t(char *)> &serialize,
- LogMessage *message);
-
-// Fills in all the parts of the message according to the given inputs (with
-// type kMatrix).
-void FillInMessageMatrix(log_level level,
- ::aos::monotonic_clock::time_point monotonic_now,
- const ::std::string &message_string, uint32_t type_id,
- int rows, int cols, const void *data,
- LogMessage *message);
-
// Fills in *message according to the given inputs (with type kString).
// Used for implementing LogImplementation::DoLog.
void FillInMessage(log_level level,
diff --git a/aos/logging/implementations_test.cc b/aos/logging/implementations_test.cc
index 2accf35..d97e165 100644
--- a/aos/logging/implementations_test.cc
+++ b/aos/logging/implementations_test.cc
@@ -19,9 +19,13 @@
namespace chrono = ::std::chrono;
-class TestLogImplementation : public SimpleLogImplementation {
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
- void DoLog(log_level level, const char *format, va_list ap) override {
+class TestLogImplementation : public LogImplementation {
+ virtual ::aos::monotonic_clock::time_point monotonic_now() const {
+ return ::aos::monotonic_clock::now();
+ }
+
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0))) void DoLog(
+ log_level level, const char *format, va_list ap) override {
internal::FillInMessage(level, monotonic_now(), format, ap, &message_);
if (level == FATAL) {
@@ -175,7 +179,8 @@
monotonic_clock::time_point end = monotonic_clock::now();
auto diff = end - start;
printf("short message took %" PRId64 " nsec for %ld\n",
- chrono::duration_cast<chrono::nanoseconds>(diff).count(),
+ static_cast<int64_t>(
+ chrono::duration_cast<chrono::nanoseconds>(diff).count()),
kTimingCycles);
start = monotonic_clock::now();
@@ -185,7 +190,8 @@
end = monotonic_clock::now();
diff = end - start;
printf("long message took %" PRId64 " nsec for %ld\n",
- chrono::duration_cast<chrono::nanoseconds>(diff).count(),
+ static_cast<int64_t>(
+ chrono::duration_cast<chrono::nanoseconds>(diff).count()),
kTimingCycles);
}
diff --git a/aos/logging/interface.cc b/aos/logging/interface.cc
index bcd8470..b72f8e1 100644
--- a/aos/logging/interface.cc
+++ b/aos/logging/interface.cc
@@ -19,7 +19,7 @@
static const char *const continued = "...\n";
const size_t size = output_size - strlen(continued);
const int ret = vsnprintf(output, size, format, ap);
- typedef ::std::common_type<typeof(ret), typeof(size)>::type RetType;
+ typedef ::std::common_type<int, size_t>::type RetType;
if (ret < 0) {
AOS_PLOG(FATAL, "vsnprintf(%p, %zd, %s, args) failed",
output, size, format);
diff --git a/aos/logging/interface.h b/aos/logging/interface.h
index 50237fc..92f0b10 100644
--- a/aos/logging/interface.h
+++ b/aos/logging/interface.h
@@ -16,19 +16,6 @@
struct MessageType;
namespace logging {
-namespace internal {
-
-// Defined in queue_logging.cc.
-void DoLogStruct(log_level level, const ::std::string &message, size_t size,
- const MessageType *type,
- const ::std::function<size_t(char *)> &serialize, int levels);
-
-// Defined in matrix_logging.cc.
-void DoLogMatrix(log_level level, const ::std::string &message,
- uint32_t type_id, int rows, int cols, const void *data,
- int levels);
-
-} // namespace internal
// Takes a message and logs it. It will set everything up and then call DoLog
// for the current LogImplementation.
@@ -75,20 +62,6 @@
va_end(ap);
}
- // Logs the contents of an auto-generated structure.
- // size and type are the result of calling Size() and Type() on the type of
- // the message.
- // serialize will call Serialize on the message.
- virtual void LogStruct(log_level level, const ::std::string &message,
- size_t size, const MessageType *type,
- const ::std::function<size_t(char *)> &serialize) = 0;
- // Similiar to LogStruct, except for matrixes.
- // type_id is the type of the elements of the matrix.
- // data points to rows*cols*type_id.Size() bytes of data in row-major order.
- virtual void LogMatrix(log_level level, const ::std::string &message,
- uint32_t type_id, int rows, int cols,
- const void *data) = 0;
-
private:
// These functions call similar methods on the "current" LogImplementation or
// Die if they can't find one.
@@ -97,14 +70,6 @@
__attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 2, 0)));
friend void VLog(log_level, const char *, va_list);
- friend void internal::DoLogStruct(
- log_level level, const ::std::string &message, size_t size,
- const MessageType *type, const ::std::function<size_t(char *)> &serialize,
- int levels);
- friend void internal::DoLogMatrix(log_level level,
- const ::std::string &message,
- uint32_t type_id, int rows, int cols,
- const void *data, int levels);
LogImplementation *next_;
};
diff --git a/aos/logging/log_displayer.cc b/aos/logging/log_displayer.cc
index e63bdc3..fe902c0 100644
--- a/aos/logging/log_displayer.cc
+++ b/aos/logging/log_displayer.cc
@@ -13,7 +13,6 @@
#include "aos/configuration.h"
#include "aos/logging/binary_log_file.h"
-#include "aos/queue_types.h"
#include "aos/logging/logging.h"
#include "aos/logging/implementations.h"
#include "aos/logging/printf_formats.h"
@@ -267,28 +266,6 @@
return 0;
}
- if (msg->type == LogFileMessageHeader::MessageType::kStructType) {
- size_t bytes = msg->message_size;
- ::aos::MessageType *type = ::aos::MessageType::Deserialize(
- reinterpret_cast<const char *>(msg + 1), &bytes);
- if (type == nullptr) {
- AOS_LOG(INFO, "Trying old version of type decoding.\n");
- bytes = msg->message_size;
- type = ::aos::MessageType::Deserialize(
- reinterpret_cast<const char *>(msg + 1), &bytes, false);
- }
-
- if (type == nullptr) {
- AOS_LOG(WARNING,
- "Error deserializing MessageType of size %" PRIx32
- " starting at %zx.\n",
- msg->message_size, reader.file_offset(msg + 1));
- } else {
- ::aos::type_cache::Add(*type);
- }
- continue;
- }
-
if (source_pid >= 0 && msg->source != source_pid) {
// Message is from the wrong process.
continue;
@@ -342,69 +319,10 @@
fprintf(stdout, AOS_LOGGING_BASE_FORMAT "%.*s", BASE_ARGS,
static_cast<int>(msg->message_size), position);
break;
- case LogFileMessageHeader::MessageType::kStruct: {
- uint32_t type_id;
- memcpy(&type_id, position, sizeof(type_id));
- position += sizeof(type_id);
-
- uint32_t string_length;
- memcpy(&string_length, position, sizeof(string_length));
- position += sizeof(string_length);
-
- char buffer[4096];
- size_t output_length = sizeof(buffer);
- size_t input_length =
- msg->message_size -
- (sizeof(type_id) + sizeof(uint32_t) + string_length);
- if (!PrintMessage(buffer, &output_length, position + string_length,
- &input_length, ::aos::type_cache::Get(type_id))) {
- AOS_LOG(FATAL,
- "printing message (%.*s) of type %s into %zu-byte buffer "
- "failed\n",
- static_cast<int>(string_length), position,
- ::aos::type_cache::Get(type_id).name.c_str(), sizeof(buffer));
- }
- if (input_length > 0) {
- AOS_LOG(WARNING, "%zu extra bytes on message of type %s\n",
- input_length, ::aos::type_cache::Get(type_id).name.c_str());
- }
- fprintf(stdout, AOS_LOGGING_BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
- static_cast<int>(string_length), position,
- static_cast<int>(sizeof(buffer) - output_length), buffer);
- } break;
- case LogFileMessageHeader::MessageType::kMatrix: {
- uint32_t type;
- memcpy(&type, position, sizeof(type));
- position += sizeof(type);
-
- uint32_t string_length;
- memcpy(&string_length, position, sizeof(string_length));
- position += sizeof(string_length);
-
- uint16_t rows;
- memcpy(&rows, position, sizeof(rows));
- position += sizeof(rows);
- uint16_t cols;
- memcpy(&cols, position, sizeof(cols));
- position += sizeof(cols);
-
- const size_t matrix_bytes =
- msg->message_size -
- (sizeof(type) + sizeof(uint32_t) + sizeof(uint16_t) +
- sizeof(uint16_t) + string_length);
- AOS_CHECK_EQ(matrix_bytes,
- ::aos::MessageType::Sizeof(type) * rows * cols);
- char buffer[4096];
- size_t output_length = sizeof(buffer);
- if (!::aos::PrintMatrix(buffer, &output_length,
- position + string_length, type, rows, cols)) {
- AOS_LOG(FATAL, "printing %dx%d matrix of type %" PRIu32 " failed\n",
- rows, cols, type);
- }
- fprintf(stdout, AOS_LOGGING_BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
- static_cast<int>(string_length), position,
- static_cast<int>(sizeof(buffer) - output_length), buffer);
- } break;
+ case LogFileMessageHeader::MessageType::kStruct:
+ case LogFileMessageHeader::MessageType::kMatrix:
+ AOS_LOG(FATAL, "Unsupported matrix or struct\n");
+ break;
case LogFileMessageHeader::MessageType::kStructType:
AOS_LOG(FATAL, "shouldn't get here\n");
break;
diff --git a/aos/logging/matrix_logging.cc b/aos/logging/matrix_logging.cc
deleted file mode 100644
index e17c8a6..0000000
--- a/aos/logging/matrix_logging.cc
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "aos/logging/matrix_logging.h"
-
-#include "aos/queue_types.h"
-#include "aos/logging/sizes.h"
-
-namespace aos {
-namespace logging {
-namespace internal {
-
-void DoLogMatrix(log_level level, const ::std::string &message,
- uint32_t type_id, int rows, int cols, const void *data,
- int levels) {
- {
- auto fn = [&](LogImplementation *implementation) {
- implementation->LogMatrix(level, message, type_id, rows, cols, data);
- };
- RunWithCurrentImplementation(levels, ::std::ref(fn));
- }
-
- if (level == FATAL) {
- char serialized[1024];
- if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
- sizeof(serialized)) {
- Die("LOG(FATAL) matrix too big to serialize");
- }
- SerializeMatrix(type_id, serialized, data, rows, cols);
- char printed[LOG_MESSAGE_LEN];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
- Die("LOG(FATAL) PrintMatrix call failed");
- }
- Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
- static_cast<int>(printed_bytes), printed);
- }
-}
-
-} // namespace internal
-} // namespace logging
-} // namespace aos
diff --git a/aos/logging/matrix_logging.h b/aos/logging/matrix_logging.h
deleted file mode 100644
index ef51d56..0000000
--- a/aos/logging/matrix_logging.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef AOS_LOGGING_MATRIX_LOGGING_H_
-#define AOS_LOGGING_MATRIX_LOGGING_H_
-
-#include <string>
-#include <functional>
-
-#include "Eigen/Dense"
-
-#include "aos/logging/interface.h"
-#include "aos/die.h"
-#include "aos/queue_primitives.h"
-
-namespace aos {
-namespace logging {
-
-// Logs the contents of a matrix and a constant string.
-// matrix must be an instance of an Eigen matrix (or something similar).
-#define AOS_LOG_MATRIX(level, message, matrix) \
- do { \
- static const ::std::string kAosLoggingMessage( \
- LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " message); \
- ::aos::logging::DoLogMatrixTemplated(level, kAosLoggingMessage, \
- (matrix).eval()); \
- /* so that GCC knows that it won't return */ \
- if (level == FATAL) { \
- ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
- } \
- } while (false)
-
-template <class T>
-void DoLogMatrixTemplated(log_level level, const ::std::string &message,
- const T &matrix) {
- if (T::IsRowMajor) {
- typename T::Scalar data[matrix.rows() * matrix.cols()];
- ::Eigen::Map<T>(data, matrix.rows(), matrix.cols()) = matrix;
- internal::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
- matrix.rows(), matrix.cols(), data, 1);
- } else {
- internal::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
- matrix.rows(), matrix.cols(), matrix.data(), 1);
- }
-}
-
-} // namespace logging
-} // namespace aos
-
-#endif // AOS_LOGGING_MATRIX_LOGGING_H_
diff --git a/aos/logging/queue_logging.cc b/aos/logging/queue_logging.cc
deleted file mode 100644
index 541ddb1..0000000
--- a/aos/logging/queue_logging.cc
+++ /dev/null
@@ -1,39 +0,0 @@
-#include "aos/logging/queue_logging.h"
-
-#include "aos/logging/interface.h"
-#include "aos/logging/sizes.h"
-#include "aos/queue_types.h"
-
-namespace aos {
-namespace logging {
-namespace internal {
-
-void DoLogStruct(log_level level, const ::std::string &message, size_t size,
- const MessageType *type,
- const ::std::function<size_t(char *)> &serialize, int levels) {
- {
- auto fn = [&](LogImplementation *implementation) {
- implementation->LogStruct(level, message, size, type, serialize);
- };
- RunWithCurrentImplementation(levels, ::std::ref(fn));
- }
-
- if (level == FATAL) {
- char serialized[1024];
- if (size > sizeof(serialized)) {
- Die("LOG(FATAL) structure too big to serialize");
- }
- size_t used = serialize(serialized);
- char printed[LOG_MESSAGE_LEN];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
- Die("LOG(FATAL) PrintMessage call failed");
- }
- Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
- static_cast<int>(printed_bytes), printed);
- }
-}
-
-} // namespace internal
-} // namespace logging
-} // namespace aos
diff --git a/aos/logging/queue_logging.h b/aos/logging/queue_logging.h
deleted file mode 100644
index fdebcc3..0000000
--- a/aos/logging/queue_logging.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef AOS_LOGGING_QUEUE_LOGGING_H_
-#define AOS_LOGGING_QUEUE_LOGGING_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-
-#include <functional>
-#include <string>
-
-#include "aos/logging/interface.h"
-#include "aos/die.h"
-
-namespace aos {
-namespace logging {
-
-// Logs the contents of a structure (or Queue message) and a constant string.
-// structure must be an instance of one of the generated queue types.
-#define AOS_LOG_STRUCT(level, message, structure) \
- do { \
- static const ::std::string kAosLoggingMessage( \
- LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " message); \
- ::aos::logging::DoLogStructTemplated(level, kAosLoggingMessage, \
- structure); \
- /* so that GCC knows that it won't return */ \
- if (level == FATAL) { \
- ::aos::Die("DoLogStruct(FATAL) fell through!!!!!\n"); \
- } \
- } while (false)
-
-template <class T>
-void DoLogStructTemplated(log_level level, const ::std::string &message,
- const T &structure) {
- auto fn = [&structure](char *buffer)
- -> size_t { return structure.Serialize(buffer); };
-
- internal::DoLogStruct(level, message, T::Size(), T::GetType(), ::std::ref(fn),
- 1);
-}
-
-} // namespace logging
-} // namespace aos
-
-#endif // AOS_LOGGING_QUEUE_LOGGING_H_
diff --git a/aos/logging/replay.cc b/aos/logging/replay.cc
deleted file mode 100644
index c58ee8c..0000000
--- a/aos/logging/replay.cc
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "aos/logging/replay.h"
-
-#include <chrono>
-
-namespace aos {
-namespace logging {
-namespace linux_code {
-
-namespace chrono = ::std::chrono;
-
-bool LogReplayer::ProcessMessage() {
- const LogFileMessageHeader *message = reader_->ReadNextMessage(false);
- if (message == nullptr) return true;
- if (message->type != LogFileMessageHeader::MessageType::kStruct) return false;
-
- const char *position = reinterpret_cast<const char *>(message + 1);
-
- ::std::string process(position, message->name_size);
- position += message->name_size;
-
- uint32_t type_id;
- memcpy(&type_id, position, sizeof(type_id));
- position += sizeof(type_id);
-
- uint32_t message_length;
- memcpy(&message_length, position, sizeof(message_length));
- position += sizeof(message_length);
- ::std::string message_text(position, message_length);
- position += message_length;
-
- size_t split_index = message_text.find_first_of(':') + 2;
- split_index = message_text.find_first_of(':', split_index) + 2;
- message_text = message_text.substr(split_index);
-
- auto handler = handlers_.find(Key(process, message_text));
- if (handler == handlers_.end()) return false;
-
- handler->second->HandleStruct(
- monotonic_clock::time_point(chrono::seconds(message->time_sec) +
- chrono::nanoseconds(message->time_nsec)),
- type_id, position,
- message->message_size -
- (sizeof(type_id) + sizeof(message_length) + message_length));
- return false;
-}
-
-} // namespace linux_code
-} // namespace logging
-} // namespace aos
diff --git a/aos/logging/replay.h b/aos/logging/replay.h
deleted file mode 100644
index 6604591..0000000
--- a/aos/logging/replay.h
+++ /dev/null
@@ -1,171 +0,0 @@
-#ifndef AOS_LOGGING_REPLAY_H_
-#define AOS_LOGGING_REPLAY_H_
-
-#include <unordered_map>
-#include <string>
-#include <functional>
-#include <memory>
-
-#include "aos/events/event-loop.h"
-#include "aos/ipc_lib/queue.h"
-#include "aos/logging/binary_log_file.h"
-#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/macros.h"
-#include "aos/queue.h"
-#include "aos/queue_types.h"
-
-namespace aos {
-namespace logging {
-namespace linux_code {
-
-// Manages pulling logged queue messages out of log files.
-//
-// Basic usage:
-// - Use the Add* methods to register handlers for various message sources.
-// - Call OpenFile to open a log file.
-// - Call ProcessMessage repeatedly until it returns true.
-//
-// This code could do something to adapt similar-but-not-identical
-// messages to the current versions, but currently it will LOG(FATAL) if any of
-// the messages don't match up exactly.
-class LogReplayer {
- public:
- LogReplayer() {}
-
- // Gets ready to read messages from fd.
- // Does not take ownership of fd.
- void OpenFile(int fd) {
- reader_.reset(new LogFileReader(fd));
- }
- // Closes the currently open file.
- void CloseCurrentFile() { reader_.reset(); }
- // Returns true if we have a file which is currently open.
- bool HasCurrentFile() const { return reader_.get() != nullptr; }
-
- // Processes a single message from the currently open file.
- // Returns true if there are no more messages in the file.
- // This will not call any of the handlers if the next message either has no
- // registered handlers or is not a queue message.
- bool ProcessMessage();
-
- // Adds a handler for messages with a certain string from a certain process.
- // T must be a Message with the same format as the messages generated by
- // the .q files.
- // LOG(FATAL)s for duplicate handlers.
- template <class T>
- void AddHandler(const ::std::string &process_name,
- const ::std::string &log_message,
- ::std::function<void(const T &message)> handler) {
- AOS_CHECK(handlers_
- .emplace(::std::piecewise_construct,
- ::std::forward_as_tuple(process_name, log_message),
- ::std::forward_as_tuple(
- ::std::unique_ptr<StructHandlerInterface>(
- new TypedStructHandler<T>(handler))))
- .second);
- }
-
- // Adds a handler which takes messages and places them directly on a queue.
- // T must be a Message with the same format as the messages generated by
- // the .q files.
- template <class T>
- void AddDirectQueueSender(const ::std::string &process_name,
- const ::std::string &log_message,
- const ::std::string &name) {
- AddHandler(process_name, log_message,
- ::std::function<void(const T &)>(
- QueueDumpStructHandler<T>(name.c_str())));
- }
-
- private:
- // A generic handler of struct log messages.
- class StructHandlerInterface {
- public:
- virtual ~StructHandlerInterface() {}
-
- virtual void HandleStruct(::aos::monotonic_clock::time_point log_time,
- uint32_t type_id, const void *data,
- size_t data_size) = 0;
- };
-
- // Converts struct log messages to a message type and passes it to an
- // ::std::function.
- template <class T>
- class TypedStructHandler : public StructHandlerInterface {
- public:
- TypedStructHandler(::std::function<void(const T &message)> handler)
- : handler_(handler) {}
-
- void HandleStruct(::aos::monotonic_clock::time_point log_time,
- uint32_t type_id, const void *data,
- size_t data_size) override {
- AOS_CHECK_EQ(type_id, T::GetType()->id);
- T message;
- AOS_CHECK_EQ(data_size, T::Size());
- AOS_CHECK_EQ(data_size,
- message.Deserialize(static_cast<const char *>(data)));
- message.sent_time = log_time;
- handler_(message);
- }
-
- private:
- const ::std::function<void(T message)> handler_;
- };
-
- // A callable class which dumps messages straight to a queue.
- template <class T>
- class QueueDumpStructHandler {
- public:
- QueueDumpStructHandler(const ::std::string &queue_name)
- : queue_(RawQueue::Fetch(queue_name.c_str(), sizeof(T), T::kHash,
- T::kQueueLength)) {}
-
- void operator()(const T &message) {
- AOS_LOG_STRUCT(DEBUG, "re-sending", message);
- void *raw_message = queue_->GetMessage();
- AOS_CHECK_NOTNULL(raw_message);
- memcpy(raw_message, &message, sizeof(message));
- AOS_CHECK(queue_->WriteMessage(raw_message, RawQueue::kOverride));
- }
-
- private:
- ::aos::RawQueue *const queue_;
- };
-
- // A key for specifying log messages to give to a certain handler.
- struct Key {
- Key(const ::std::string &process_name, const ::std::string &log_message)
- : process_name(process_name), log_message(log_message) {}
-
- ::std::string process_name;
- ::std::string log_message;
- };
-
- struct KeyHash {
- size_t operator()(const Key &key) const {
- return string_hash(key.process_name) ^
- (string_hash(key.log_message) << 1);
- }
-
- private:
- const ::std::hash<::std::string> string_hash = ::std::hash<::std::string>();
- };
- struct KeyEqual {
- bool operator()(const Key &a, const Key &b) const {
- return a.process_name == b.process_name && a.log_message == b.log_message;
- }
- };
-
- ::std::unordered_map<const Key, ::std::unique_ptr<StructHandlerInterface>,
- KeyHash, KeyEqual> handlers_;
- ::std::unique_ptr<LogFileReader> reader_;
-
- DISALLOW_COPY_AND_ASSIGN(LogReplayer);
-};
-
-} // namespace linux_code
-} // namespace logging
-} // namespace aos
-
-#endif // AOS_LOGGING_REPLAY_H_
diff --git a/aos/mutex/BUILD b/aos/mutex/BUILD
index 43c9616..e698f62 100644
--- a/aos/mutex/BUILD
+++ b/aos/mutex/BUILD
@@ -9,10 +9,9 @@
"mutex.h",
],
deps = [
- "//aos:die",
- "//aos/type_traits:type_traits",
- "//aos/logging",
"//aos/ipc_lib:aos_sync",
+ "//aos/type_traits",
+ "@com_github_google_glog//:glog",
],
)
@@ -22,14 +21,13 @@
"mutex_test.cc",
],
deps = [
- "//aos:die",
":mutex",
- "//aos/time:time",
- "//aos/logging",
- "//aos/util:death_test_log_implementation",
- "//aos/util:thread",
"//aos/testing:googletest",
"//aos/testing:test_logging",
"//aos/testing:test_shm",
+ "//aos/time",
+ "//aos/util:death_test_log_implementation",
+ "//aos/util:thread",
+ "@com_github_google_glog//:glog",
],
)
diff --git a/aos/mutex/mutex.cc b/aos/mutex/mutex.cc
index c5570aa..ef4fbbe 100644
--- a/aos/mutex/mutex.cc
+++ b/aos/mutex/mutex.cc
@@ -5,7 +5,7 @@
#include <string.h>
#include "aos/type_traits/type_traits.h"
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
@@ -19,8 +19,8 @@
} else if (ret == 1) {
return true;
} else {
- AOS_LOG(FATAL, "mutex_grab(%p(=%" PRIu32 ")) failed with %d\n", &impl_,
- impl_.futex, ret);
+ LOG(FATAL) << "mutex_grab(" << &impl_ << "(=" << std::hex << impl_.futex
+ << ")) failed with " << ret;
}
}
@@ -38,8 +38,8 @@
case 4:
return State::kLockFailed;
default:
- AOS_LOG(FATAL, "mutex_trylock(%p(=%" PRIu32 ")) failed with %d\n", &impl_,
- impl_.futex, ret);
+ LOG(FATAL) << "mutex_trylock(" << &impl_ << "(=" << std::hex
+ << impl_.futex << ")) failed with " << ret;
}
}
diff --git a/aos/mutex/mutex.h b/aos/mutex/mutex.h
index ad678cc..a2af7ae 100644
--- a/aos/mutex/mutex.h
+++ b/aos/mutex/mutex.h
@@ -1,10 +1,10 @@
#ifndef AOS_MUTEX_H_
#define AOS_MUTEX_H_
+#include "aos/ipc_lib/aos_sync.h"
#include "aos/macros.h"
#include "aos/type_traits/type_traits.h"
-#include "aos/die.h"
-#include "aos/ipc_lib/aos_sync.h"
+#include "glog/logging.h"
namespace aos {
@@ -73,8 +73,8 @@
public:
explicit MutexLocker(Mutex *mutex) : mutex_(mutex) {
if (__builtin_expect(mutex_->Lock(), false)) {
- ::aos::Die("previous owner of mutex %p died but it shouldn't be able to",
- this);
+ LOG(FATAL) << "previous owner of mutex " << this
+ << " died but it shouldn't be able to";
}
}
~MutexLocker() {
@@ -95,13 +95,14 @@
: mutex_(mutex), owner_died_(mutex_->Lock()) {}
~IPCMutexLocker() {
if (__builtin_expect(!owner_died_checked_, false)) {
- ::aos::Die("nobody checked if the previous owner of mutex %p died", this);
+ LOG(FATAL) << "nobody checked if the previous owner of mutex " << this
+ << " died";
}
mutex_->Unlock();
}
// Whether or not the previous owner died. If this is not called at least
- // once, the destructor will ::aos::Die.
+ // once, the destructor will LOG(FATAL)
__attribute__((warn_unused_result)) bool owner_died() {
owner_died_checked_ = true;
return __builtin_expect(owner_died_, false);
@@ -125,13 +126,14 @@
owner_died_(locked_ ? mutex_->Lock() : false) {}
~IPCRecursiveMutexLocker() {
if (__builtin_expect(!owner_died_checked_, false)) {
- ::aos::Die("nobody checked if the previous owner of mutex %p died", this);
+ LOG(FATAL) << "nobody checked if the previous owner of mutex " << this
+ << " died";
}
if (locked_) mutex_->Unlock();
}
// Whether or not the previous owner died. If this is not called at least
- // once, the destructor will ::aos::Die.
+ // once, the destructor will LOG(FATAL)
__attribute__((warn_unused_result)) bool owner_died() {
owner_died_checked_ = true;
return __builtin_expect(owner_died_, false);
diff --git a/aos/mutex/mutex_test.cc b/aos/mutex/mutex_test.cc
index 9d371b7..cc659a5 100644
--- a/aos/mutex/mutex_test.cc
+++ b/aos/mutex/mutex_test.cc
@@ -10,13 +10,13 @@
#include "gtest/gtest.h"
#include "aos/die.h"
-#include "aos/time/time.h"
-#include "aos/util/death_test_log_implementation.h"
-#include "aos/util/thread.h"
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/core_lib.h"
#include "aos/testing/test_logging.h"
#include "aos/testing/test_shm.h"
+#include "aos/time/time.h"
+#include "aos/util/death_test_log_implementation.h"
+#include "aos/util/thread.h"
namespace aos {
namespace testing {
diff --git a/aos/queue-tmpl.h b/aos/queue-tmpl.h
deleted file mode 100644
index 2ca79b8..0000000
--- a/aos/queue-tmpl.h
+++ /dev/null
@@ -1,106 +0,0 @@
-namespace aos {
-
-template <class T>
-bool ScopedMessagePtr<T>::Send() {
- assert(msg_ != NULL);
- msg_->SetTimeToNow();
- assert(queue_ != NULL);
- bool return_value = queue_->WriteMessage(msg_, RawQueue::kOverride);
- msg_ = NULL;
- return return_value;
-}
-
-template <class T>
-void ScopedMessagePtr<T>::reset(T *msg) {
- if (queue_ != NULL && msg_ != NULL) {
- queue_->FreeMessage(msg_);
- }
- msg_ = msg;
-}
-
-template <class T>
-void Queue<T>::Init() {
- if (queue_ == NULL) {
- queue_ = RawQueue::Fetch(queue_name_, sizeof(T),
- static_cast<int>(T::kHash),
- T::kQueueLength);
- queue_msg_.set_queue(queue_);
- }
-}
-
-template <class T>
-void Queue<T>::Clear() {
- if (queue_ != NULL) {
- queue_msg_.reset();
- queue_ = NULL;
- queue_msg_.set_queue(NULL);
- }
- index_ = 0;
-}
-
-template <class T>
-bool Queue<T>::FetchNext() {
- Init();
- const T *msg = static_cast<const T *>(
- queue_->ReadMessageIndex(RawQueue::kNonBlock, &index_));
- // Only update the internal pointer if we got a new message.
- if (msg != NULL) {
- queue_msg_.reset(msg);
- }
- return msg != NULL;
-}
-
-template <class T>
-void Queue<T>::FetchNextBlocking() {
- Init();
- const T *msg = static_cast<const T *>(
- queue_->ReadMessageIndex(RawQueue::kBlock, &index_));
- queue_msg_.reset(msg);
- assert (msg != NULL);
-}
-
-template <class T>
-bool Queue<T>::FetchLatest() {
- Init();
- static constexpr Options<RawQueue> kOptions =
- RawQueue::kFromEnd | RawQueue::kNonBlock;
- const T *msg =
- static_cast<const T *>(queue_->ReadMessageIndex(kOptions, &index_));
- // Only update the internal pointer if we got a new message.
- if (msg != NULL && msg != queue_msg_.get()) {
- queue_msg_.reset(msg);
- return true;
- }
- // The message has to get freed if we didn't use it (and RawQueue::FreeMessage
- // is ok to call on NULL).
- queue_->FreeMessage(msg);
- return false;
-}
-
-template <class T>
-void Queue<T>::FetchAnother() {
- if (!FetchLatest()) FetchNextBlocking();
-}
-
-template <class T>
-ScopedMessagePtr<T> Queue<T>::MakeMessage() {
- Init();
- return ScopedMessagePtr<T>(queue_, MakeRawMessage());
-}
-
-template <class T>
-T *Queue<T>::MakeRawMessage() {
- T *ret = static_cast<T *>(queue_->GetMessage());
- assert(ret != NULL);
- ret->Zero();
- return ret;
-}
-
-template <class T>
-aos::MessageBuilder<T> Queue<T>::MakeWithBuilder() {
- Init();
- T *const ret = MakeRawMessage();
- return aos::MessageBuilder<T>(queue_, ret);
-}
-
-} // namespace aos
diff --git a/aos/queue.h b/aos/queue.h
deleted file mode 100644
index fb11f4f..0000000
--- a/aos/queue.h
+++ /dev/null
@@ -1,219 +0,0 @@
-#ifndef AOS_QUEUE_H_
-#define AOS_QUEUE_H_
-
-#include <assert.h>
-
-#include "aos/macros.h"
-#include "aos/ipc_lib/queue.h"
-#include "aos/messages/message.h"
-
-namespace aos {
-
-template <class T> class Queue;
-
-// A ScopedMessagePtr<> manages a queue message pointer.
-// It frees it properly when the ScopedMessagePtr<> goes out of scope or gets
-// sent. By design, there is no way to get the ScopedMessagePtr to release the
-// message pointer to external code.
-template <class T>
-class ScopedMessagePtr {
- public:
- // Returns a pointer to the message.
- // This stays valid until Send or the destructor have been called.
- const T *get() const { return msg_; }
- T *get() { return msg_; }
-
- const T &operator*() const {
- const T *msg = get();
- assert(msg != NULL);
- return *msg;
- }
-
- T &operator*() {
- T *msg = get();
- assert(msg != NULL);
- return *msg;
- }
-
- const T *operator->() const {
- const T *msg = get();
- assert(msg != NULL);
- return msg;
- }
-
- T *operator->() {
- T *msg = get();
- assert(msg != NULL);
- return msg;
- }
-
- operator bool() {
- return msg_ != NULL;
- }
-
- // Sends the message and removes our reference to it.
- // If the queue is full, over-ride the oldest message in it with our new
- // message.
- // Returns true on success, and false otherwise.
- // The message will be freed.
- bool Send();
-
- // Frees the contained message.
- ~ScopedMessagePtr() {
- reset();
- }
-
- // Implements a move constructor. This only takes rvalue references
- // because we want to allow someone to say
- // ScopedMessagePtr<X> ptr = queue.MakeMessage();
- // but we don't want to allow them to then say
- // ScopedMessagePtr<X> new_ptr = ptr;
- // And, if they do actually want to move the pointer, then it will correctly
- // clear out the source so there aren't 2 pointers to the message lying
- // around.
- ScopedMessagePtr(ScopedMessagePtr<T> &&ptr)
- : queue_(ptr.queue_), msg_(ptr.msg_) {
- ptr.msg_ = NULL;
- }
-
- private:
- // Provide access to set_queue and the constructor for init.
- friend class aos::Queue<typename std::remove_const<T>::type>;
- // Provide access to the copy constructor for MakeWithBuilder.
- friend class aos::MessageBuilder<T>;
-
- // Only Queue should be able to build a message.
- ScopedMessagePtr(RawQueue *queue, T *msg)
- : queue_(queue), msg_(msg) {}
-
- // Sets the pointer to msg, freeing the old value if it was there.
- // This is private because nobody should be able to get a pointer to a message
- // that needs to be scoped without using the queue.
- void reset(T *msg = NULL);
-
- // Sets the queue that owns this message.
- void set_queue(RawQueue *queue) { queue_ = queue; }
-
- // The queue that the message is a part of.
- RawQueue *queue_;
- // The message or NULL.
- T *msg_;
-
- // Protect evil constructors.
- DISALLOW_COPY_AND_ASSIGN(ScopedMessagePtr<T>);
-};
-
-// TODO(aschuh): Base class
-// T must be a Message with the same format as the messages generated by
-// the .q files.
-template <class T>
-class Queue {
- public:
- typedef T Message;
-
- explicit Queue(const char *queue_name)
- : queue_name_(queue_name), queue_(NULL), queue_msg_(NULL, NULL) {
- static_assert(shm_ok<T>::value,
- "The provided message type can't be put in shmem.");
- }
-
- // Initializes the queue. This may optionally be called to do any one time
- // work before sending information, and may be be called multiple times.
- // Init will be called when a message is sent, but this will cause sending to
- // take a different amount of time the first cycle.
- void Init();
-
- // Removes all internal references to shared memory so shared memory can be
- // restarted safely. This should only be used in testing.
- void Clear();
-
- // Fetches the next message from the queue.
- // Returns true if there was a new message available and we successfully
- // fetched it.
- bool FetchNext();
-
- // Fetches the next message from the queue, waiting if necessary until there
- // is one.
- void FetchNextBlocking();
-
- // Fetches the last message from the queue.
- // Returns true if there was a new message available and we successfully
- // fetched it.
- bool FetchLatest();
-
- // Fetches the latest message from the queue, or blocks if we have already
- // fetched it until another is avilable.
- void FetchAnother();
-
- // Returns the age of the message.
- const monotonic_clock::duration Age() const {
- return monotonic_clock::now() - queue_msg_->sent_time;
- }
-
- bool IsNewerThan(const monotonic_clock::duration age) const {
- return get() != nullptr && Age() < age;
- }
-
- // Returns a pointer to the current message.
- // This pointer will be valid until a new message is fetched.
- const T *get() const { return queue_msg_.get(); }
-
- // Returns a reference to the message.
- // The message will be valid until a new message is fetched.
- const T &operator*() const {
- const T *msg = get();
- assert(msg != NULL);
- return *msg;
- }
-
- // Returns a pointer to the current message.
- // This pointer will be valid until a new message is fetched.
- const T *operator->() const {
- const T *msg = get();
- assert(msg != NULL);
- return msg;
- }
-
- // Returns a scoped_ptr containing a message.
- // GCC will optimize away the copy constructor, so this is safe.
- ScopedMessagePtr<T> MakeMessage();
-
- // Returns a message builder that contains a pre-allocated message.
- // This message will start out completely zeroed.
- aos::MessageBuilder<T> MakeWithBuilder();
-
- const char *name() const { return queue_name_; }
-
- private:
- const char *queue_name_;
-
- T *MakeRawMessage();
-
- // Pointer to the queue that this object fetches from.
- RawQueue *queue_;
- int index_ = 0;
- // Scoped pointer holding the latest message or NULL.
- ScopedMessagePtr<const T> queue_msg_;
-
- DISALLOW_COPY_AND_ASSIGN(Queue<T>);
-};
-
-// Base class for all queue groups.
-class QueueGroup {
- public:
- // Constructs a queue group given its name and a unique hash of the name and
- // type.
- QueueGroup(const char *name) : name_(name) {}
-
- // Returns the name of the queue group.
- const char *name() const { return name_.c_str(); }
-
- private:
- std::string name_;
-};
-
-} // namespace aos
-
-#include "aos/queue-tmpl.h"
-
-#endif // AOS_QUEUE_H_
diff --git a/aos/queue_test.cc b/aos/queue_test.cc
deleted file mode 100644
index 1c4ebdd..0000000
--- a/aos/queue_test.cc
+++ /dev/null
@@ -1,316 +0,0 @@
-#include <unistd.h>
-
-#include <chrono>
-#include <memory>
-
-#include "gtest/gtest.h"
-
-#include "aos/die.h"
-#include "aos/test_queue.q.h"
-#include "aos/util/thread.h"
-#include "aos/testing/test_shm.h"
-
-namespace aos {
-namespace common {
-namespace testing {
-
-namespace chrono = ::std::chrono;
-
-class QueueTest : public ::testing::Test {
- protected:
- void SetUp() override {
- SetDieTestMode(true);
- }
-
- ::aos::testing::TestSharedMemory my_shm_;
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::aos::Queue<TestingMessage> my_test_queue;
-
- QueueTest() : my_test_queue(".aos.common.testing.test_queue") {}
-};
-
-class MyThread : public util::Thread {
- public:
- MyThread() : threaded_test_queue(".aos.common.testing.test_queue") {}
-
- virtual void Run() {
- threaded_test_queue.FetchNextBlocking();
- EXPECT_TRUE(threaded_test_queue->test_bool);
- EXPECT_EQ(0x971, threaded_test_queue->test_int);
- }
-
- ::aos::Queue<TestingMessage> threaded_test_queue;
- private:
- DISALLOW_COPY_AND_ASSIGN(MyThread);
-};
-
-
-// Tests that we can send a message to another thread and it blocking receives
-// it at the correct time.
-TEST_F(QueueTest, FetchBlocking) {
- MyThread t;
- t.Start();
- usleep(50000);
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
- t.Join();
- EXPECT_LE(t.threaded_test_queue.Age(), chrono::milliseconds(57));
-}
-
-// Tests that we can send a message with the message pointer and get it back.
-TEST_F(QueueTest, SendMessage) {
- ScopedMessagePtr<TestingMessage> msg = my_test_queue.MakeMessage();
- msg->test_bool = true;
- msg->test_int = 0x971;
- msg.Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_TRUE(my_test_queue->test_bool);
- EXPECT_EQ(0x971, my_test_queue->test_int);
-}
-
-// Tests that we can send a message with the builder and get it back.
-TEST_F(QueueTest, SendWithBuilder) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_EQ(true, my_test_queue->test_bool);
- EXPECT_EQ(0x971, my_test_queue->test_int);
- EXPECT_EQ(true, my_test_queue.IsNewerThan(chrono::milliseconds(10000)));
-}
-
-// Tests that multiple queue instances don't break each other.
-TEST_F(QueueTest, MultipleQueues) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_TRUE(my_test_queue.get());
-
- {
- ::aos::Queue<TestingMessage> my_other_test_queue(
- ".aos.common.testing.queue_name");
- my_other_test_queue.MakeMessage();
- EXPECT_FALSE(my_other_test_queue.FetchLatest());
- EXPECT_FALSE(my_test_queue.FetchLatest());
- }
-
- EXPECT_TRUE(my_test_queue.get());
-}
-
-// Tests that using a queue from multiple threads works correctly.
-TEST_F(QueueTest, MultipleThreads) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
- ASSERT_TRUE(my_test_queue.FetchLatest());
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
- EXPECT_EQ(0x971, my_test_queue->test_int);
-
- ::aos::util::FunctionThread::RunInOtherThread([this]() {
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_EQ(0x254, my_test_queue->test_int);
- });
- EXPECT_EQ(0x254, my_test_queue->test_int);
-}
-
-// Makes sure that MakeWithBuilder zeros the message initially.
-// This might randomly succeed sometimes, but it will fail with asan if it
-// doesn't.
-TEST_F(QueueTest, BuilderZero) {
- my_test_queue.MakeWithBuilder().Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_FALSE(my_test_queue->test_bool);
- EXPECT_EQ(0, my_test_queue->test_int);
-}
-
-// Tests that various pointer deref functions at least seem to work.
-TEST_F(QueueTest, PointerDeref) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- const TestingMessage *msg_ptr = my_test_queue.get();
- ASSERT_NE(static_cast<TestingMessage*>(NULL), msg_ptr);
- EXPECT_EQ(0x971, msg_ptr->test_int);
- EXPECT_EQ(msg_ptr, &(*my_test_queue));
-}
-
-// Tests that FetchNext doesn't miss any messages.
-TEST_F(QueueTest, FetchNext) {
- for (int i = 0; i < 10; ++i) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(i).Send();
- }
-
- for (int i = 0; i < 10; ++i) {
- ASSERT_TRUE(my_test_queue.FetchNext());
- EXPECT_EQ(i, my_test_queue->test_int);
- }
-}
-
-// Tests that FetchLatest skips a missing message.
-TEST_F(QueueTest, FetchLatest) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_EQ(0x971, my_test_queue->test_int);
-}
-
-// Tests that FetchLatest works with multiple readers.
-TEST_F(QueueTest, FetchLatestMultiple) {
- ::aos::Queue<TestingMessage> my_second_test_queue(
- ".aos.common.testing.test_queue");
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_EQ(0x971, my_test_queue->test_int);
- ASSERT_TRUE(my_second_test_queue.FetchLatest());
- ASSERT_TRUE(my_second_test_queue.get() != NULL);
- EXPECT_EQ(0x971, my_second_test_queue->test_int);
-}
-
-
-// Tests that fetching without a new message returns false.
-TEST_F(QueueTest, FetchLatestWithoutMessage) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
- EXPECT_TRUE(my_test_queue.FetchLatest());
- EXPECT_FALSE(my_test_queue.FetchLatest());
- EXPECT_FALSE(my_test_queue.FetchLatest());
- EXPECT_EQ(0x254, my_test_queue->test_int);
-}
-
-// Tests that fetching without a message returns false.
-TEST_F(QueueTest, FetchOnFreshQueue) {
- EXPECT_FALSE(my_test_queue.FetchLatest());
- EXPECT_EQ(static_cast<TestingMessage*>(NULL), my_test_queue.get());
-}
-
-// Tests that fetch next without a message returns false.
-TEST_F(QueueTest, FetchNextOnFreshQueue) {
- EXPECT_FALSE(my_test_queue.FetchNext());
- EXPECT_EQ(static_cast<TestingMessage*>(NULL), my_test_queue.get());
-}
-
-// Tests that fetch next without a new message returns false.
-TEST_F(QueueTest, FetchNextWithoutMessage) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
- EXPECT_TRUE(my_test_queue.FetchNext());
- EXPECT_FALSE(my_test_queue.FetchNext());
- EXPECT_NE(static_cast<TestingMessage*>(NULL), my_test_queue.get());
-}
-
-// Tests that age makes some sense.
-TEST_F(QueueTest, Age) {
- my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
-
- ASSERT_TRUE(my_test_queue.FetchLatest());
- EXPECT_TRUE(my_test_queue.IsNewerThan(chrono::milliseconds(100)));
- const auto age = my_test_queue.Age();
- EXPECT_GE(chrono::nanoseconds(100000000), age);
-}
-
-
-class GroupTest : public ::testing::Test {
- protected:
- GroupTest()
- : my_test_queuegroup(".aos.common.testing.test_queuegroup",
- ".aos.common.testing.test_queuegroup.first",
- ".aos.common.testing.test_queuegroup.second") {}
-
- // Create a new instance of the test group so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- TwoQueues my_test_queuegroup;
-
- private:
- ::aos::testing::TestSharedMemory my_shm_;
-};
-
-// Tests that name works.
-TEST_F(GroupTest, Name) {
- EXPECT_EQ(std::string(".aos.common.testing.test_queuegroup"),
- std::string(my_test_queuegroup.name()));
-}
-
-
-class MessageTest : public ::testing::Test {
- public:
- TestingMessage msg;
-};
-
-TEST_F(MessageTest, Zeroing) {
- msg.test_bool = true;
- msg.test_int = 0x254;
- msg.SetTimeToNow();
-
- msg.Zero();
-
- EXPECT_FALSE(msg.test_bool);
- EXPECT_EQ(0, msg.test_int);
- EXPECT_EQ(monotonic_clock::min_time, msg.sent_time);
-}
-
-TEST_F(MessageTest, Size) {
- EXPECT_EQ(static_cast<size_t>(13), msg.Size());
-}
-
-TEST_F(MessageTest, Serialize) {
- char serialized_data[msg.Size()];
- msg.test_bool = true;
- msg.test_int = 0x254;
- msg.SetTimeToNow();
-
- msg.Serialize(serialized_data);
-
- TestingMessage new_msg;
- new_msg.Deserialize(serialized_data);
-
- EXPECT_EQ(msg.test_bool, new_msg.test_bool);
- EXPECT_EQ(msg.test_int, new_msg.test_int);
- EXPECT_EQ(msg.sent_time, new_msg.sent_time);
-}
-
-// Tests that Print prints out a message nicely.
-TEST_F(MessageTest, Print) {
- char printdata[1024];
- msg.test_bool = true;
- msg.test_int = 2056;
- msg.sent_time = monotonic_clock::time_point(chrono::seconds(971) +
- chrono::nanoseconds(254));
-
- std::string golden("971.000000254s, T, 2056");
- EXPECT_EQ(golden.size(), msg.Print(printdata, sizeof(printdata)));
-
- EXPECT_EQ(golden, std::string(printdata));
-}
-
-// Tests that the hash never changes. If it changes, then someone broke the
-// hash routine or changed the message declaration. Both changes need to be
-// validated by hand.
-TEST_F(MessageTest, Hash) {
- EXPECT_EQ(static_cast<uint32_t>(0xc33651ac),
- static_cast<uint32_t>(TestingMessage::kHash));
-}
-
-TEST_F(MessageTest, SetNow) {
- msg.SetTimeToNow();
- EXPECT_LE(msg.sent_time - monotonic_clock::now(), chrono::milliseconds(20));
-}
-
-// Tests that EqualsNoTime works.
-TEST_F(MessageTest, EqualsNoTime) {
- msg.test_bool = true;
- msg.test_int = 971;
- TestingMessage other;
- other.test_int = 971;
- EXPECT_FALSE(other.EqualsNoTime(msg));
- EXPECT_FALSE(msg.EqualsNoTime(other));
- other.test_bool = true;
- EXPECT_TRUE(other.EqualsNoTime(msg));
- EXPECT_TRUE(msg.EqualsNoTime(other));
- msg.SetTimeToNow();
- EXPECT_TRUE(msg.EqualsNoTime(other));
-}
-
-} // namespace testing
-} // namespace common
-} // namespace aos
diff --git a/aos/queue_types.cc b/aos/queue_types.cc
deleted file mode 100644
index a48c5a0..0000000
--- a/aos/queue_types.cc
+++ /dev/null
@@ -1,462 +0,0 @@
-#include "aos/queue_types.h"
-
-#include <inttypes.h>
-
-#include <memory>
-#include <unordered_map>
-
-#include "aos/byteorder.h"
-#include "aos/ipc_lib/shared_mem.h"
-#include "aos/logging/logging.h"
-#include "aos/ipc_lib/core_lib.h"
-#include "aos/mutex/mutex.h"
-
-namespace aos {
-
-ssize_t MessageType::Serialize(char *buffer, size_t max_bytes) const {
- char *const buffer_start = buffer;
- uint16_t fields_size = 0;
- for (int i = 0; i < number_fields; ++i) {
- fields_size += sizeof(fields[i]->type);
- fields_size += sizeof(fields[i]->length);
- fields_size += sizeof(uint16_t);
- fields_size += fields[i]->name.size();
- }
- if (max_bytes < sizeof(id) + sizeof(super_size) + sizeof(uint16_t) +
- sizeof(number_fields) + name.size() + fields_size) {
- return -1;
- }
-
- uint16_t length;
-
- to_network(&super_size, buffer);
- buffer += sizeof(super_size);
- to_network(&id, buffer);
- buffer += sizeof(id);
- length = name.size();
- to_network(&length, buffer);
- buffer += sizeof(length);
- to_network(&number_fields, buffer);
- buffer += sizeof(number_fields);
- memcpy(buffer, name.data(), length);
- buffer += name.size();
-
- for (int i = 0; i < number_fields; ++i) {
- to_network(&fields[i]->type, buffer);
- buffer += sizeof(fields[i]->type);
- to_network(&fields[i]->length, buffer);
- buffer += sizeof(fields[i]->length);
- length = fields[i]->name.size();
- to_network(&length, buffer);
- buffer += sizeof(length);
- memcpy(buffer, fields[i]->name.data(), length);
- buffer += length;
- }
-
- return buffer - buffer_start;
-}
-
-MessageType *MessageType::Deserialize(const char *buffer, size_t *bytes,
- bool deserialize_length) {
- uint16_t name_length;
- decltype(MessageType::super_size) super_size;
- decltype(MessageType::id) id;
- decltype(MessageType::number_fields) number_fields;
- if (*bytes < sizeof(super_size) + sizeof(id) + sizeof(name_length) +
- sizeof(number_fields)) {
- return nullptr;
- }
- *bytes -= sizeof(super_size) + sizeof(id) + sizeof(name_length) +
- sizeof(number_fields);
-
- to_host(buffer, &super_size);
- buffer += sizeof(super_size);
- to_host(buffer, &id);
- buffer += sizeof(id);
- to_host(buffer, &name_length);
- buffer += sizeof(name_length);
- to_host(buffer, &number_fields);
- buffer += sizeof(number_fields);
-
- if (*bytes < name_length) {
- return nullptr;
- }
- *bytes -= name_length;
-
- Field **fields = new Field *[number_fields];
- ::std::unique_ptr<MessageType> r(
- new MessageType(super_size, id, ::std::string(buffer, name_length),
- number_fields, fields));
- buffer += name_length;
-
- for (int i = 0; i < number_fields; ++i) {
- uint16_t field_name_length;
- if (*bytes < sizeof(fields[i]->type) + sizeof(field_name_length) +
- (deserialize_length ? sizeof(fields[i]->length) : 0)) {
- return nullptr;
- }
- *bytes -= sizeof(fields[i]->type) + sizeof(field_name_length);
-
- to_host(buffer, &fields[i]->type);
- buffer += sizeof(fields[i]->type);
- if (deserialize_length) {
- to_host(buffer, &fields[i]->length);
- buffer += sizeof(fields[i]->length);
- *bytes -= sizeof(fields[i]->length);
- }
- to_host(buffer, &field_name_length);
- buffer += sizeof(field_name_length);
-
- if (*bytes < field_name_length) {
- return nullptr;
- }
- *bytes -= field_name_length;
- fields[i]->name = ::std::string(buffer, field_name_length);
- buffer += field_name_length;
- }
-
- return r.release();
-}
-
-bool PrintArray(char *output, size_t *output_bytes, const void *input,
- size_t *input_bytes, uint32_t type_id, uint32_t length) {
- if (*output_bytes < 1) return false;
- *output_bytes -= 1;
- *(output++) = '[';
-
- bool first = true;
- for (uint32_t i = 0; i < length; ++i) {
- if (first) {
- first = false;
- } else {
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ',';
- *(output++) = ' ';
- }
-
- const size_t output_bytes_before = *output_bytes,
- input_bytes_before = *input_bytes;
- if (MessageType::IsPrimitive(type_id)) {
- if (!PrintField(output, output_bytes, input, input_bytes, type_id)) {
- return false;
- }
- } else {
- if (!PrintMessage(output, output_bytes, input, input_bytes,
- type_cache::Get(type_id))) {
- return false;
- }
- // Ignore the trailing '\0' that the subcall put on.
- *output_bytes += 1;
- }
-
- // Update the input and output pointers.
- output += output_bytes_before - *output_bytes;
- input =
- static_cast<const char *>(input) + input_bytes_before - *input_bytes;
- }
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ']';
- *(output++) = '\0';
- return true;
-}
-
-bool PrintMessage(char *output, size_t *output_bytes, const void *input,
- size_t *input_bytes, const MessageType &type) {
- if (*output_bytes < type.name.size() + 1) return false;
- *output_bytes -= type.name.size() + 1;
- memcpy(output, type.name.data(), type.name.size());
- output += type.name.size();
- *output = '{';
- ++output;
-
- bool first = true;
- if (type.super_size == 8) {
- size_t start_input_bytes = *input_bytes;
-
- // Default value is -633437440.3735158
- if (*output_bytes < 10) return false;
- memcpy(output, "sendtime: ", 10);
- output += 10;
- *output_bytes -= 10;
-
- size_t output_bytes_before = *output_bytes;
- if (!PrintField(output, output_bytes, input, input_bytes, 0x5f372008u)) {
- return false;
- }
- output += output_bytes_before - *output_bytes;
- input = static_cast<const char *>(input) + type.super_size;
- first = false;
- AOS_CHECK(start_input_bytes == (*input_bytes + 8));
- } else {
- *input_bytes -= type.super_size;
- input = static_cast<const char *>(input) + type.super_size;
- }
-
- for (int i = 0; i < type.number_fields; ++i) {
- if (first) {
- first = false;
- } else {
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ',';
- *(output++) = ' ';
- }
-
- if (*output_bytes < type.fields[i]->name.size() + 1) return false;
- *output_bytes -= type.fields[i]->name.size() + 1;
- memcpy(output, type.fields[i]->name.data(), type.fields[i]->name.size());
- output += type.fields[i]->name.size();
- *(output++) = ':';
-
- const size_t output_bytes_before = *output_bytes,
- input_bytes_before = *input_bytes;
- const uint32_t type_id = type.fields[i]->type;
- if (type.fields[i]->length > 0) {
- if (!PrintArray(output, output_bytes, input, input_bytes, type_id,
- type.fields[i]->length)) {
- return false;
- }
- // Ignore the trailing '\0' that the subcall put on.
- *output_bytes += 1;
- } else if (MessageType::IsPrimitive(type_id)) {
- if (!PrintField(output, output_bytes, input, input_bytes, type_id)) {
- return false;
- }
- } else {
- if (!PrintMessage(output, output_bytes, input, input_bytes,
- type_cache::Get(type_id))) {
- return false;
- }
- // Ignore the trailing '\0' that the subcall put on.
- *output_bytes += 1;
- }
-
- // Update the input and output pointers.
- output += output_bytes_before - *output_bytes;
- input =
- static_cast<const char *>(input) + input_bytes_before - *input_bytes;
- }
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = '}';
- *(output++) = '\0';
- return true;
-}
-
-bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
- uint32_t type_id, int rows, int cols) {
- AOS_CHECK(MessageType::IsPrimitive(type_id));
- const size_t element_size = MessageType::Sizeof(type_id);
-
- if (*output_bytes < 1) return false;
- *output_bytes -= 1;
- *(output++) = '[';
-
- bool first_row = true;
- for (int row = 0; row < rows; ++row) {
- if (first_row) {
- first_row = false;
- } else {
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ',';
- *(output++) = ' ';
- }
-
- if (*output_bytes < 1) return false;
- *output_bytes -= 1;
- *(output++) = '[';
-
- bool first_col = true;
- for (int col = 0; col < cols; ++col) {
- if (first_col) {
- first_col = false;
- } else {
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ',';
- *(output++) = ' ';
- }
-
- const size_t output_bytes_before = *output_bytes;
- size_t input_bytes = element_size;
- if (!PrintField(output, output_bytes,
- static_cast<const char *>(input) +
- (row + col * rows) * element_size,
- &input_bytes, type_id)) {
- return false;
- }
- AOS_CHECK_EQ(0u, input_bytes);
- // Update the output pointer.
- output += output_bytes_before - *output_bytes;
- }
-
- if (*output_bytes < 1) return false;
- *output_bytes -= 1;
- *(output++) = ']';
- }
- if (*output_bytes < 2) return false;
- *output_bytes -= 2;
- *(output++) = ']';
- *(output++) = '\0';
- return true;
-}
-
-void SerializeMatrix(int type_id, void *output_void, const void *input_void,
- int rows, int cols) {
- char *const output = static_cast<char *>(output_void);
- const char *const input = static_cast<const char *>(input_void);
-
- AOS_CHECK(MessageType::IsPrimitive(type_id));
- const size_t element_size = MessageType::Sizeof(type_id);
-
- for (int i = 0; i < rows * cols; ++i) {
- switch(element_size) {
- case 1:
- to_network<1>(&input[i * element_size], &output[i * element_size]);
- break;
- case 2:
- to_network<2>(&input[i * element_size], &output[i * element_size]);
- break;
- case 4:
- to_network<4>(&input[i * element_size], &output[i * element_size]);
- break;
- case 8:
- to_network<8>(&input[i * element_size], &output[i * element_size]);
- break;
- default:
- AOS_LOG(FATAL, "illegal primitive type size %zu\n", element_size);
- }
- }
-}
-
-namespace type_cache {
-namespace {
-
-struct CacheEntry {
- const MessageType &type;
- bool in_shm;
-
- CacheEntry(const MessageType &type, bool in_shm)
- : type(type), in_shm(in_shm) {}
-};
-
-struct ShmType {
- uint32_t id;
- volatile ShmType *next;
-
- size_t serialized_size;
- char serialized[];
-};
-
-::std::unordered_map<uint32_t, CacheEntry> cache;
-::aos::Mutex cache_lock;
-
-} // namespace
-
-void Add(const MessageType &type) {
- ::aos::MutexLocker locker(&cache_lock);
- if (cache.count(type.id) == 0) {
- cache.emplace(::std::piecewise_construct, ::std::forward_as_tuple(type.id),
- ::std::forward_as_tuple(type, false));
- }
-}
-
-const MessageType &Get(uint32_t type_id) {
- ::aos::MutexLocker locker(&cache_lock);
-
- {
- const auto cached = cache.find(type_id);
- if (cached != cache.end()) {
- return cached->second.type;
- }
- }
-
- if (aos_core_is_init()) {
- // No need to lock because the only thing that happens is somebody adds on
- // to the end, and they shouldn't be adding the one we're looking for.
- const volatile ShmType *c = static_cast<volatile ShmType *>(
- global_core->mem_struct->queue_types.pointer);
- while (c != nullptr) {
- if (c->id == type_id) {
- size_t bytes = c->serialized_size;
- MessageType *type = MessageType::Deserialize(
- const_cast<const char *>(c->serialized), &bytes);
- cache.emplace(::std::piecewise_construct,
- ::std::forward_as_tuple(type_id),
- ::std::forward_as_tuple(*type, true));
- return *type;
- }
- c = c->next;
- }
- } else {
- AOS_LOG(INFO, "FYI: no shm. going to LOG(FATAL) now\n");
- }
-
- AOS_LOG(FATAL, "MessageType for id 0x%" PRIx32 " not found\n", type_id);
-}
-
-void AddShm(uint32_t type_id) {
- if (!aos_core_is_init()) {
- AOS_LOG(FATAL, "can't AddShm(%" PRIu32 ") without shm!\n", type_id);
- }
-
- const MessageType::Field **fields;
- int number_fields;
- {
- ::aos::MutexLocker locker(&cache_lock);
- CacheEntry &cached = cache.at(type_id);
- if (cached.in_shm) return;
-
- fields = cached.type.fields;
- number_fields = cached.type.number_fields;
-
- if (mutex_lock(&global_core->mem_struct->queue_types.lock) != 0) {
- AOS_LOG(FATAL, "locking queue_types lock failed\n");
- }
- volatile ShmType *current = static_cast<volatile ShmType *>(
- global_core->mem_struct->queue_types.pointer);
- if (current != nullptr) {
- while (true) {
- if (current->id == type_id) {
- cached.in_shm = true;
- mutex_unlock(&global_core->mem_struct->queue_types.lock);
- return;
- }
- if (current->next == nullptr) break;
- current = current->next;
- }
- }
- char buffer[1024];
- ssize_t size = cached.type.Serialize(buffer, sizeof(buffer));
- if (size == -1) {
- AOS_LOG(FATAL, "type %s is too big to fit into %zd bytes\n",
- cached.type.name.c_str(), sizeof(buffer));
- }
-
- volatile ShmType *shm =
- static_cast<volatile ShmType *>(shm_malloc(sizeof(ShmType) + size));
- shm->id = type_id;
- shm->next = nullptr;
- shm->serialized_size = size;
- memcpy(const_cast<char *>(shm->serialized), buffer, size);
-
- if (current == NULL) {
- global_core->mem_struct->queue_types.pointer = const_cast<ShmType *>(shm);
- } else {
- current->next = shm;
- }
- mutex_unlock(&global_core->mem_struct->queue_types.lock);
- }
-
- for (int i = 0; i < number_fields; ++i) {
- if (!MessageType::IsPrimitive(fields[i]->type)) {
- AddShm(fields[i]->type);
- }
- }
-}
-
-} // namespace type_cache
-} // namespace aos
diff --git a/aos/queue_types.h b/aos/queue_types.h
deleted file mode 100644
index bff06bb..0000000
--- a/aos/queue_types.h
+++ /dev/null
@@ -1,148 +0,0 @@
-#ifndef AOS_QUEUE_TYPES_H_
-#define AOS_QUEUE_TYPES_H_
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <string.h>
-
-#include <initializer_list>
-#include <string>
-
-#include "aos/macros.h"
-#include "aos/byteorder.h"
-
-namespace aos {
-
-// The type IDs this uses are 2 parts: a 16 bit size and a 16 bit hash. Sizes
-// for primitive types are stored with 8192 (0x2000) added.
-//
-// Serializing/deserializing includes all of the fields too.
-struct MessageType {
- struct Field {
- // The type ID for the type of this field.
- uint32_t type;
- // The length of the array if it is one or 0.
- uint32_t length;
- ::std::string name;
- };
-
- // Takes ownership of all the Field pointers in fields_initializer.
- MessageType(size_t super_size, uint32_t id, const ::std::string &name,
- ::std::initializer_list<const Field *> fields_initializer)
- : super_size(super_size), id(id), name(name) {
- number_fields = fields_initializer.size();
- fields = new const Field *[number_fields];
- int i = 0;
- for (const Field *c : fields_initializer) {
- fields[i++] = c;
- }
- }
-
- ~MessageType() {
- for (int i = 0; i < number_fields; ++i) {
- delete fields[i];
- }
- delete[] fields;
- }
-
- // Returns -1 if max_bytes is too small.
- ssize_t Serialize(char *buffer, size_t max_bytes) const;
- // bytes should start out as the number of bytes available in buffer and gets
- // reduced by the number actually read before returning.
- // deserialize_length is whether to look for a length field in the serialized
- // data.
- // Returns a new instance allocated with new or nullptr for error.
- static MessageType *Deserialize(const char *buffer, size_t *bytes,
- bool deserialize_length = true);
-
- static bool IsPrimitive(uint32_t type_id) {
- return (type_id & 0x2000) != 0;
- }
-
- static unsigned int Sizeof(uint32_t type_id) {
- if (IsPrimitive(type_id)) {
- return (type_id & 0xFFFF) - 0x2000;
- } else {
- return type_id & 0xFFFF;
- }
- }
-
- // How many (serialized) bytes the superclass takes up.
- uint16_t super_size;
- // The type ID for this.
- uint32_t id;
- ::std::string name;
-
- uint16_t number_fields;
- const Field **fields;
-
- private:
- // Internal constructor for Deserialize to use.
- MessageType(uint16_t super_size, uint32_t id, ::std::string &&name,
- uint16_t number_fields, Field **fields)
- : super_size(super_size),
- id(id),
- name(name),
- number_fields(number_fields),
- fields(const_cast<const Field **>(fields)) {
- for (int i = 0; i < number_fields; ++i) {
- fields[i] = new Field();
- }
- }
-
- DISALLOW_COPY_AND_ASSIGN(MessageType);
-};
-
-// The arguments are the same for both of these.
-//
-// output is where to write the text representation ('\0'-terminated
-// human-readable string).
-// output_bytes should point to the number of bytes available to write in
-// output. It will be reduced by the number of bytes that were actually written.
-// input is where to read the data in from (in network byte order, aka from
-// Serialize).
-// input_bytes should point to the number of bytes available to read from input.
-// It will be reduced by the number of bytes that were actually read.
-// type is which type to print.
-// Returns true for success and false for not.
-//
-// Prints the value from 1 primitive message field into output.
-// The implementation of this is generated by the ruby code.
-// Does not include a trailing '\0' in what it subtracts from *output_bytes.
-bool PrintField(char *output, size_t *output_bytes, const void *input,
- size_t *input_bytes, uint32_t type)
- __attribute__((warn_unused_result));
-// Prints a whole message into output.
-// This calls itself recursively and PrintField to print out the whole thing.
-bool PrintMessage(char *output, size_t *output_bytes, const void *input,
- size_t *input_bytes, const MessageType &type)
- __attribute__((warn_unused_result));
-// Calls PrintField to print out a matrix of values.
-bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
- uint32_t type, int rows, int cols);
-
-// "Serializes" a matrix (basically just converts to network byte order). The
-// result can be passed to PrintMatrix.
-void SerializeMatrix(int type_id, void *output_void, const void *input_void,
- int rows, int cols);
-
-// Implements a cache of types which generally works per-process but can (when
-// instructed) put a type in shared memory which other processes will
-// automatically be able to retrieve.
-// All of these functions are thread-safe.
-namespace type_cache {
-
-// Makes sure a type is in the type cache. This will store a reference to type.
-// The types of any non-primitive fields of type must already be added.
-void Add(const MessageType &type);
-// Retrieves a type from the type cache or shm. LOG(FATAL)s if it can't find it.
-const MessageType &Get(uint32_t type_id);
-// Makes sure a type is in the list in shm. Add must have already been called
-// for type.
-// Also adds the types of any non-primitive fields of type.
-void AddShm(uint32_t type_id);
-
-} // namespace type_cache
-} // namespace aos
-
-#endif // AOS_QUEUE_TYPES_H_
diff --git a/aos/queue_types_test.cc b/aos/queue_types_test.cc
deleted file mode 100644
index 6503f02..0000000
--- a/aos/queue_types_test.cc
+++ /dev/null
@@ -1,274 +0,0 @@
-#include "aos/queue_types.h"
-
-#include <memory>
-#include <limits>
-#include <string>
-
-#include "gtest/gtest.h"
-
-#include "aos/test_queue.q.h"
-#include "aos/byteorder.h"
-#include "aos/queue_primitives.h"
-#include "aos/logging/logging.h"
-#include "aos/testing/test_logging.h"
-
-using ::aos::common::testing::Structure;
-using ::aos::common::testing::MessageWithStructure;
-using ::aos::common::testing::OtherTestingMessage;
-using ::aos::common::testing::MessageWithArrays;
-
-namespace aos {
-namespace testing {
-
-typedef MessageType::Field Field;
-
-static const MessageType kTestType1(5, 0x1234, "TestType1",
- {new Field{0, 0, "field1"},
- new Field{0, 0, "field2"},
- new Field{0, 0, "field3"}});
-
-class QueueTypesTest : public ::testing::Test {
- public:
- QueueTypesTest() {
- ::aos::testing::EnableTestLogging();
- }
-
- ::testing::AssertionResult Equal(const MessageType &l, const MessageType &r) {
- using ::testing::AssertionFailure;
- if (l.id != r.id) {
- return AssertionFailure() << "id " << l.id << " != " << r.id;
- }
- if (l.name != r.name) {
- return AssertionFailure() << "name '" << l.name << "' != '" << r.name
- << "'";
- }
- if (l.number_fields != r.number_fields) {
- return AssertionFailure() << "number_fields " << l.number_fields
- << " != " << r.number_fields;
- }
- for (int i = 0; i < l.number_fields; ++i) {
- SCOPED_TRACE("field " + ::std::to_string(i));
- if (l.fields[i]->type != r.fields[i]->type) {
- return AssertionFailure() << "type " << l.fields[i]->type
- << " != " << r.fields[i]->type;
- }
- if (l.fields[i]->name != r.fields[i]->name) {
- return AssertionFailure() << "name '" << l.fields[i]->name << "' != '"
- << r.fields[i]->name << "'";
- }
- }
- return ::testing::AssertionSuccess();
- }
-};
-
-TEST_F(QueueTypesTest, Serialization) {
- char buffer[512];
- ssize_t size;
- size_t out_size;
- ::std::unique_ptr<MessageType> deserialized;
-
- size = kTestType1.Serialize(buffer, sizeof(buffer));
- ASSERT_GT(size, 1);
- ASSERT_LE(static_cast<size_t>(size), sizeof(buffer));
-
- out_size = size;
- deserialized.reset(MessageType::Deserialize(buffer, &out_size));
- EXPECT_EQ(0u, out_size);
- EXPECT_TRUE(Equal(kTestType1, *deserialized));
-
- out_size = size - 1;
- deserialized.reset(MessageType::Deserialize(buffer, &out_size));
- EXPECT_EQ(nullptr, deserialized.get());
-
- out_size = size + 1;
- ASSERT_LE(out_size, sizeof(buffer));
- deserialized.reset(MessageType::Deserialize(buffer, &out_size));
- EXPECT_EQ(1u, out_size);
- EXPECT_TRUE(Equal(kTestType1, *deserialized));
-}
-
-class PrintMessageTest : public ::testing::Test {
- public:
- char input[512], output[512];
- size_t input_bytes, output_bytes;
-};
-
-class PrintFieldTest : public PrintMessageTest {
- public:
- template <typename T>
- void TestInteger(T value, ::std::string result) {
- input_bytes = sizeof(value);
- to_network(&value, input);
- output_bytes = sizeof(output);
- ASSERT_TRUE(
- PrintField(output, &output_bytes, input, &input_bytes, TypeID<T>::id));
- EXPECT_EQ(0u, input_bytes);
- EXPECT_EQ(sizeof(output) - result.size(), output_bytes);
- EXPECT_EQ(result, ::std::string(output, sizeof(output) - output_bytes));
- }
-
- template <typename T>
- void TestAllIntegers(T multiple) {
- for (T i = ::std::numeric_limits<T>::min();
- i < ::std::numeric_limits<T>::max() - multiple; i += multiple) {
- TestInteger<T>(i, ::std::to_string(i));
- }
- }
-};
-
-TEST_F(PrintFieldTest, Basic) {
- TestInteger<uint16_t>(971, "971");
- TestInteger<uint8_t>(8, "8");
- TestInteger<uint8_t>(254, "254");
- TestInteger<uint8_t>(0, "0");
- TestInteger<int8_t>(254, "-2");
- TestInteger<int8_t>(67, "67");
- TestInteger<int8_t>(0, "0");
-
- input_bytes = 1;
- input[0] = 1;
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintField(output, &output_bytes, input, &input_bytes,
- queue_primitive_types::bool_p));
- EXPECT_EQ(0u, input_bytes);
- EXPECT_EQ(sizeof(output) - 1, output_bytes);
- EXPECT_EQ(::std::string("T"),
- ::std::string(output, sizeof(output) - output_bytes));
-
- input_bytes = 1;
- input[0] = 0;
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintField(output, &output_bytes, input, &input_bytes,
- queue_primitive_types::bool_p));
- EXPECT_EQ(0u, input_bytes);
- EXPECT_EQ(sizeof(output) - 1, output_bytes);
- EXPECT_EQ(::std::string("f"),
- ::std::string(output, sizeof(output) - output_bytes));
-}
-
-// Runs through lots of integers and makes sure PrintField gives the same result
-// as ::std::to_string.
-TEST_F(PrintFieldTest, Integers) {
- TestAllIntegers<uint8_t>(1);
- TestAllIntegers<int8_t>(1);
- TestAllIntegers<uint16_t>(1);
- TestAllIntegers<int16_t>(3);
- TestAllIntegers<uint32_t>(43129);
- TestAllIntegers<int32_t>(654321);
- TestAllIntegers<uint64_t>(123456789101112);
- TestAllIntegers<int64_t>(91011121249856532);
-}
-
-// Tests PrintField with trailing input bytes and only 1 extra output byte.
-TEST_F(PrintFieldTest, OtherSizes) {
- static const float kData = 16.78;
- static const ::std::string kString("16.780001");
- static const size_t kExtraInputBytes = 4;
- input_bytes = sizeof(kData) + kExtraInputBytes;
- to_network(&kData, input);
- output_bytes = kString.size() + 1;
- ASSERT_LE(output_bytes, sizeof(output));
- ASSERT_TRUE(PrintField(output, &output_bytes, input, &input_bytes,
- Structure::GetType()->fields[2]->type));
- EXPECT_EQ(kExtraInputBytes, input_bytes);
- EXPECT_EQ(1u, output_bytes);
- EXPECT_EQ(kString, ::std::string(output));
-}
-
-TEST_F(PrintFieldTest, InputTooSmall) {
- static const float kData = 0;
- input_bytes = sizeof(kData) - 1;
- output_bytes = sizeof(output);
- EXPECT_FALSE(PrintField(output, &output_bytes, input, &input_bytes,
- Structure::GetType()->fields[2]->type));
-}
-
-TEST_F(PrintFieldTest, OutputTooSmall) {
- static const uint16_t kData = 12345;
- input_bytes = sizeof(input);
- to_network(&kData, input);
- output_bytes = 4;
- EXPECT_FALSE(PrintField(output, &output_bytes, input, &input_bytes,
- Structure::GetType()->fields[1]->type));
-}
-
-static const OtherTestingMessage kTestMessage1(true, 8971, 3.2);
-static const ::std::string kTestMessage1String =
- ".aos.common.testing.OtherTestingMessage{sendtime: -633437440.3735158s, test_bool:T, test_int:8971"
- ", test_double:3.200000}";
-static const Structure kTestStructure1(false, 973, 8.56);
-static const ::std::string kTestStructure1String =
- ".aos.common.testing.Structure{struct_bool:f, struct_int:973, "
- "struct_float:8.560000}";
-static const ::aos::common::testing::Structure kStructureValue(true, 973, 3.14);
-static const MessageWithArrays kTestMessageWithArrays({{971, 254, 1678}},
- {{kStructureValue,
- kStructureValue}});
-static const ::std::string kTestMessageWithArraysString =
- ".aos.common.testing.MessageWithArrays{sendtime: -633437440.3735158s, test_int:[971, 254, 1678], "
- "test_struct:[.aos.common.testing.Structure{struct_bool:T, struct_int:973, "
- "struct_float:3.140000}, .aos.common.testing.Structure{struct_bool:T, "
- "struct_int:973, struct_float:3.140000}]}";
-
-TEST_F(PrintMessageTest, Basic) {
- ASSERT_GE(sizeof(input), kTestMessage1.Size());
- input_bytes = kTestMessage1.Serialize(input);
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintMessage(output, &output_bytes, input, &input_bytes,
- *kTestMessage1.GetType()));
- EXPECT_EQ(kTestMessage1String, ::std::string(output));
- EXPECT_EQ(kTestMessage1String.size() + 1, sizeof(output) - output_bytes);
-}
-
-TEST_F(PrintMessageTest, OutputTooSmall) {
- ASSERT_GE(sizeof(input), kTestMessage1.Size());
- input_bytes = kTestMessage1.Serialize(input);
- output_bytes = kTestMessage1String.size();
- EXPECT_FALSE(PrintMessage(output, &output_bytes, input, &input_bytes,
- *kTestMessage1.GetType()));
-}
-
-TEST_F(PrintMessageTest, InputTooSmall) {
- input_bytes = kTestMessage1.Size() - 1;
- output_bytes = sizeof(output);
- EXPECT_FALSE(PrintMessage(output, &output_bytes, input, &input_bytes,
- *kTestMessage1.GetType()));
-}
-
-TEST_F(PrintMessageTest, Structure) {
- ASSERT_GE(sizeof(input), kTestStructure1.Size());
- input_bytes = kTestStructure1.Serialize(input);
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintMessage(output, &output_bytes, input, &input_bytes,
- *kTestStructure1.GetType()));
- EXPECT_EQ(kTestStructure1String, ::std::string(output));
- EXPECT_EQ(kTestStructure1String.size() + 1, sizeof(output) - output_bytes);
-}
-
-TEST_F(PrintMessageTest, Matrix) {
- static const uint16_t kTestMatrix[] = {971, 254, 1768, 8971, 9971, 973};
- uint16_t test_matrix[sizeof(kTestMatrix) / sizeof(kTestMatrix[0])];
- SerializeMatrix(queue_primitive_types::uint16_t_p, test_matrix, kTestMatrix,
- 3, 2);
- static const ::std::string kOutput =
- "[[971, 8971], [254, 9971], [1768, 973]]";
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintMatrix(output, &output_bytes, test_matrix,
- queue_primitive_types::uint16_t_p, 3, 2));
- EXPECT_EQ(kOutput, ::std::string(output));
- EXPECT_EQ(kOutput.size() + 1, sizeof(output) - output_bytes);
-}
-
-TEST_F(PrintMessageTest, Array) {
- ASSERT_GE(sizeof(input), kTestMessageWithArrays.Size());
- input_bytes = kTestMessageWithArrays.Serialize(input);
- output_bytes = sizeof(output);
- ASSERT_TRUE(PrintMessage(output, &output_bytes, input, &input_bytes,
- *kTestMessageWithArrays.GetType()));
- EXPECT_EQ(kTestMessageWithArraysString, ::std::string(output));
- EXPECT_EQ(kTestMessageWithArraysString.size() + 1,
- sizeof(output) - output_bytes);
-}
-
-} // namespace testing
-} // namespace aos
diff --git a/aos/realtime.cc b/aos/realtime.cc
new file mode 100644
index 0000000..6a80550
--- /dev/null
+++ b/aos/realtime.cc
@@ -0,0 +1,127 @@
+#include "aos/realtime.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <errno.h>
+#include <sched.h>
+#include <sys/resource.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <sys/prctl.h>
+#include <malloc.h>
+
+#include "glog/logging.h"
+
+namespace FLAG__namespace_do_not_use_directly_use_DECLARE_double_instead {
+extern double FLAGS_tcmalloc_release_rate __attribute__((weak));
+}
+using FLAG__namespace_do_not_use_directly_use_DECLARE_double_instead::
+ FLAGS_tcmalloc_release_rate;
+
+namespace aos {
+namespace logging {
+namespace internal {
+
+// Implemented in aos/logging/context.cc.
+void ReloadThreadName() __attribute__((weak));
+
+} // namespace internal
+} // namespace logging
+
+namespace {
+
+void SetSoftRLimit(int resource, rlim64_t soft, bool set_for_root) {
+ bool am_root = getuid() == 0;
+ if (set_for_root || !am_root) {
+ struct rlimit64 rlim;
+ PCHECK(getrlimit64(resource, &rlim) == 0)
+ << ": " << program_invocation_short_name << "-init: getrlimit64("
+ << resource << ") failed";
+
+ rlim.rlim_cur = soft;
+ rlim.rlim_max = ::std::max(rlim.rlim_max, soft);
+
+ PCHECK(setrlimit64(resource, &rlim) == 0)
+ << ": " << program_invocation_short_name << "-init: setrlimit64("
+ << resource << ", {cur=" << (uintmax_t)rlim.rlim_cur
+ << ",max=" << (uintmax_t)rlim.rlim_max << "}) failed";
+ }
+}
+
+} // namespace
+
+void LockAllMemory() {
+ // Allow locking as much as we want into RAM.
+ SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY, false);
+
+ WriteCoreDumps();
+ PCHECK(mlockall(MCL_CURRENT | MCL_FUTURE) == 0)
+ << ": " << program_invocation_short_name << "-init: mlockall failed";
+
+ // Don't give freed memory back to the OS.
+ CHECK_EQ(1, mallopt(M_TRIM_THRESHOLD, -1));
+ // Don't use mmap for large malloc chunks.
+ CHECK_EQ(1, mallopt(M_MMAP_MAX, 0));
+
+ if (&FLAGS_tcmalloc_release_rate) {
+ // Tell tcmalloc not to return memory.
+ FLAGS_tcmalloc_release_rate = 0.0;
+ }
+
+ // Forces the memory pages for all the stack space that we're ever going to
+ // use to be loaded into memory (so it can be locked there).
+ uint8_t data[4096 * 8];
+ // Not 0 because linux might optimize that to a 0-filled page.
+ memset(data, 1, sizeof(data));
+
+ static const size_t kHeapPreallocSize = 512 * 1024;
+ char *const heap_data = static_cast<char *>(malloc(kHeapPreallocSize));
+ memset(heap_data, 1, kHeapPreallocSize);
+ free(heap_data);
+}
+
+void InitRT() {
+ LockAllMemory();
+
+ // Only let rt processes run for 3 seconds straight.
+ SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
+
+ // Allow rt processes up to priority 40.
+ SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
+}
+
+void UnsetCurrentThreadRealtimePriority() {
+ struct sched_param param;
+ param.sched_priority = 0;
+ PCHECK(sched_setscheduler(0, SCHED_OTHER, ¶m) == 0)
+ << ": sched_setscheduler(0, SCHED_OTHER, 0) failed";
+}
+
+void SetCurrentThreadName(const ::std::string &name) {
+ CHECK_LE(name.size(), 16u) << ": thread name '" << name << "' too long";
+ VLOG(1) << "This thread is changing to '" << name << "'";
+ PCHECK(prctl(PR_SET_NAME, name.c_str()) == 0);
+ if (&logging::internal::ReloadThreadName != nullptr) {
+ logging::internal::ReloadThreadName();
+ }
+}
+
+void SetCurrentThreadRealtimePriority(int priority) {
+ // Make sure we will only be allowed to run for 3 seconds straight.
+ SetSoftRLimit(RLIMIT_RTTIME, 3000000, true);
+
+ struct sched_param param;
+ param.sched_priority = priority;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0)
+ << ": sched_setscheduler(0, SCHED_FIFO, " << priority << ") failed";
+}
+
+void WriteCoreDumps() {
+ // Do create core files of unlimited size.
+ SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, true);
+}
+
+} // namespace aos
diff --git a/aos/realtime.h b/aos/realtime.h
new file mode 100644
index 0000000..a6b4223
--- /dev/null
+++ b/aos/realtime.h
@@ -0,0 +1,33 @@
+#ifndef AOS_REALTIME_H_
+#define AOS_REALTIME_H_
+
+#include <string>
+
+namespace aos {
+
+// Locks everything into memory and sets the limits. This plus InitNRT are
+// everything you need to do before SetCurrentThreadRealtimePriority will make
+// your thread RT. Called as part of ShmEventLoop::Run()
+void InitRT();
+
+// Sets the current thread back down to non-realtime priority.
+void UnsetCurrentThreadRealtimePriority();
+
+// Sets the name of the current thread.
+// This will displayed by `top -H`, dump_rtprio, and show up in logs.
+// name can have a maximum of 16 characters.
+void SetCurrentThreadName(const ::std::string &name);
+
+// Sets the current thread's realtime priority.
+void SetCurrentThreadRealtimePriority(int priority);
+
+// Sets up this process to write core dump files.
+// This is called by Init*, but it's here for other files that want this
+// behavior without calling Init*.
+void WriteCoreDumps();
+
+void LockAllMemory();
+
+} // namespace aos
+
+#endif // AOS_REALTIME_H_
diff --git a/aos/robot_state/BUILD b/aos/robot_state/BUILD
index e392c56..5a9c255 100644
--- a/aos/robot_state/BUILD
+++ b/aos/robot_state/BUILD
@@ -1,10 +1,26 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos:config.bzl", "aos_config")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = 'robot_state',
- srcs = [
- 'robot_state.q',
- ],
+flatbuffer_cc_library(
+ name = "robot_state_fbs",
+ srcs = ["robot_state.fbs"],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "joystick_state_fbs",
+ srcs = ["joystick_state.fbs"],
+ gen_reflections = 1,
+)
+
+aos_config(
+ name = "config",
+ src = "robot_state_config.json",
+ flatbuffers = [
+ "//aos/robot_state:joystick_state_fbs",
+ "//aos/robot_state:robot_state_fbs",
+ ],
+ visibility = ["//visibility:public"],
)
diff --git a/aos/robot_state/joystick_state.fbs b/aos/robot_state/joystick_state.fbs
new file mode 100644
index 0000000..f21b211
--- /dev/null
+++ b/aos/robot_state/joystick_state.fbs
@@ -0,0 +1,41 @@
+namespace aos;
+
+table Joystick {
+ // A bitmask of the butotn state.
+ buttons:ushort;
+
+ // The 6 joystick axes.
+ // TODO: Should have size of 6
+ axis:[double];
+
+ // The POV axis.
+ pov:int;
+}
+
+// This message is checked by all control loops to make sure that the
+// joystick code hasn't died. It is published on "/aos"
+table JoystickState {
+ //TODO: should have fixed size.
+ joysticks:[Joystick];
+
+ test_mode:bool;
+ fms_attached:bool;
+ enabled:bool;
+ autonomous:bool;
+ team_id:ushort;
+
+ // 2018 scale and switch positions.
+ // TODO(austin): Push these out to a new message?
+ switch_left:bool;
+ scale_left:bool;
+
+ // If this is true, then this message isn't actually from the control
+ // system and so should not be trusted as evidence that the button inputs
+ // etc are actually real and should be acted on.
+ // However, most things should ignore this so that sending fake messages is
+ // useful for testing. The only difference in behavior should be motors not
+ // actually turning on.
+ fake:bool;
+}
+
+root_type JoystickState;
diff --git a/aos/robot_state/robot_state.fbs b/aos/robot_state/robot_state.fbs
new file mode 100644
index 0000000..437cfd7
--- /dev/null
+++ b/aos/robot_state/robot_state.fbs
@@ -0,0 +1,35 @@
+namespace aos;
+
+// This message is sent out on this queue when sensors are read. It contains
+// global robot state and information about whether the process reading sensors
+// has been restarted, along with all counters etc it keeps track of. It is
+// published on "/aos"
+table RobotState {
+ // The PID of the process reading sensors.
+ // This is here so control loops can tell when it changes.
+ reader_pid:int;
+
+ // True when outputs are enabled.
+ // Motor controllers keep going for a bit after this goes to false.
+ outputs_enabled:bool;
+ // Indicates whether something is browned out (I think motor controller
+ // outputs). IMPORTANT: This is NOT !outputs_enabled. outputs_enabled goes to
+ // false for other reasons too (disabled, e-stopped, maybe more).
+ browned_out:bool;
+
+ // Whether the two sensor rails are currently working.
+ is_3v3_active:bool;
+ is_5v_active:bool;
+ // The current voltages measured on the two sensor rails.
+ voltage_3v3:double;
+ voltage_5v:double;
+
+ // The input voltage to the roboRIO.
+ voltage_roborio_in:double;
+
+ // From the DriverStation object, aka what FMS sees and what shows up on the
+ // actual driver's station.
+ voltage_battery:double;
+}
+
+root_type RobotState;
diff --git a/aos/robot_state/robot_state.q b/aos/robot_state/robot_state.q
deleted file mode 100644
index e482c5d..0000000
--- a/aos/robot_state/robot_state.q
+++ /dev/null
@@ -1,70 +0,0 @@
-package aos;
-
-struct Joystick {
- // A bitmask of the button state.
- uint16_t buttons;
-
- // The 6 joystick axes.
- double[6] axis;
-
- // The POV axis.
- int32_t pov;
-};
-
-// This message is checked by all control loops to make sure that the
-// joystick code hasn't died. It is published on ".aos.joystick_state"
-message JoystickState {
- Joystick[6] joysticks;
-
- bool test_mode;
- bool fms_attached;
- bool enabled;
- bool autonomous;
- uint16_t team_id;
-
- // 2018 scale and switch positions.
- // TODO(austin): Push these out to a new message?
- bool switch_left;
- bool scale_left;
-
- // If this is true, then this message isn't actually from the control
- // system and so should not be trusted as evidence that the button inputs
- // etc are actually real and should be acted on.
- // However, most things should ignore this so that sending fake messages is
- // useful for testing. The only difference in behavior should be motors not
- // actually turning on.
- bool fake;
-};
-
-// This message is sent out on this queue when sensors are read. It contains
-// global robot state and information about whether the process reading sensors
-// has been restarted, along with all counters etc it keeps track of. It is
-// published on ".aos.robot_state"
-message RobotState {
- // The PID of the process reading sensors.
- // This is here so control loops can tell when it changes.
- int32_t reader_pid;
-
- // True when outputs are enabled.
- // Motor controllers keep going for a bit after this goes to false.
- bool outputs_enabled;
- // Indicates whether something is browned out (I think motor controller
- // outputs). IMPORTANT: This is NOT !outputs_enabled. outputs_enabled goes to
- // false for other reasons too (disabled, e-stopped, maybe more).
- bool browned_out;
-
- // Whether the two sensor rails are currently working.
- bool is_3v3_active;
- bool is_5v_active;
- // The current voltages measured on the two sensor rails.
- double voltage_3v3;
- double voltage_5v;
-
- // The input voltage to the roboRIO.
- double voltage_roborio_in;
-
- // From the DriverStation object, aka what FMS sees and what shows up on the
- // actual driver's station.
- double voltage_battery;
-};
-
diff --git a/aos/robot_state/robot_state_config.json b/aos/robot_state/robot_state_config.json
new file mode 100644
index 0000000..0b2a5cc
--- /dev/null
+++ b/aos/robot_state/robot_state_config.json
@@ -0,0 +1,15 @@
+{
+ "channels":
+ [
+ {
+ "name": "/aos",
+ "type": "aos.JoystickState",
+ "frequency": 75
+ },
+ {
+ "name": "/aos",
+ "type": "aos.RobotState",
+ "frequency": 200
+ }
+ ]
+}
diff --git a/aos/scoped/BUILD b/aos/scoped/BUILD
index f5b3832..1ec0f1b 100644
--- a/aos/scoped/BUILD
+++ b/aos/scoped/BUILD
@@ -9,6 +9,7 @@
"scoped_fd.h",
],
deps = [
- "//aos/logging",
+ "//aos:macros",
+ "@com_github_google_glog//:glog",
],
)
diff --git a/aos/scoped/scoped_fd.cc b/aos/scoped/scoped_fd.cc
index 74f8882..846736d 100644
--- a/aos/scoped/scoped_fd.cc
+++ b/aos/scoped/scoped_fd.cc
@@ -1,13 +1,13 @@
#include "aos/scoped/scoped_fd.h"
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
void ScopedFD::Close() {
if (fd_ != -1) {
if (close(fd_) == -1) {
- AOS_PLOG(WARNING, "close(%d) failed", fd_);
+ PLOG(WARNING) << "close(" << fd_ << ") failed";
}
}
}
diff --git a/aos/test_queue.q b/aos/test_queue.q
deleted file mode 100644
index 827b607..0000000
--- a/aos/test_queue.q
+++ /dev/null
@@ -1,38 +0,0 @@
-package aos.common.testing;
-
-struct Structure {
- bool struct_bool;
- uint16_t struct_int;
- float struct_float;
-};
-
-message MessageWithStructure {
- bool other_member;
- Structure struct1;
- Structure struct2;
-};
-
-message TestingMessage {
- bool test_bool;
- int32_t test_int;
-};
-
-message OtherTestingMessage {
- bool test_bool;
- int32_t test_int;
- double test_double;
-};
-
-message MessageWithArrays {
- uint16_t[3] test_int;
- Structure[2] test_struct;
-};
-
-queue TestingMessage test_queue;
-
-queue_group TwoQueues {
- queue TestingMessage first;
- queue OtherTestingMessage second;
-};
-
-queue_group TwoQueues test_queuegroup;
diff --git a/aos/testdata/backwards.json b/aos/testdata/backwards.json
new file mode 100644
index 0000000..192005e
--- /dev/null
+++ b/aos/testdata/backwards.json
@@ -0,0 +1,13 @@
+{
+ "channels": [
+ {
+ "name": ".aos.robot_state",
+ "type": "aos.RobotState",
+ "max_size": 5
+ },
+ {
+ "name": ".aos.joystick_state",
+ "type": "aos.JoystickState"
+ }
+ ]
+}
diff --git a/aos/testing/BUILD b/aos/testing/BUILD
index 0c214ed..99a3569 100644
--- a/aos/testing/BUILD
+++ b/aos/testing/BUILD
@@ -6,9 +6,9 @@
],
visibility = ["//visibility:public"],
deps = [
- "//third_party/googletest:gtest",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
+ "@com_google_googletest//:gtest",
],
)
@@ -53,7 +53,7 @@
],
visibility = ["//visibility:public"],
deps = [
- "//aos/logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -69,7 +69,7 @@
visibility = ["//visibility:public"],
deps = [
":test_logging",
- "//aos:queues",
+ "//aos/ipc_lib:queue",
"//aos/ipc_lib:shared_mem",
"//aos/logging",
],
diff --git a/aos/testing/gtest_main.cc b/aos/testing/gtest_main.cc
index c8c5062..52159c3 100644
--- a/aos/testing/gtest_main.cc
+++ b/aos/testing/gtest_main.cc
@@ -25,6 +25,7 @@
FLAGS_logtostderr = true;
google::InitGoogleLogging(argv[0]);
::gflags::ParseCommandLineFlags(&argc, &argv, false);
+ google::InstallFailureSignalHandler();
if (FLAGS_print_logs) {
if (::aos::testing::ForcePrintLogsDuringTests) {
diff --git a/aos/testing/prevent_exit.cc b/aos/testing/prevent_exit.cc
index ed6644b..8624c93 100644
--- a/aos/testing/prevent_exit.cc
+++ b/aos/testing/prevent_exit.cc
@@ -3,7 +3,7 @@
#include <stdlib.h>
#include <unistd.h>
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
namespace testing {
@@ -16,7 +16,7 @@
} // namespace
void PreventExit() {
- AOS_CHECK_EQ(atexit(TerminateExitHandler), 0);
+ CHECK_EQ(atexit(TerminateExitHandler), 0);
}
} // namespace testing
diff --git a/aos/testing/test_shm.cc b/aos/testing/test_shm.cc
index e9a637a..b1c3848 100644
--- a/aos/testing/test_shm.cc
+++ b/aos/testing/test_shm.cc
@@ -2,7 +2,7 @@
#include <sys/mman.h>
-#include "aos/queue.h"
+#include "aos/ipc_lib/queue.h"
#include "aos/logging/logging.h"
#include "aos/testing/test_logging.h"
diff --git a/aos/time/BUILD b/aos/time/BUILD
index e08ef7b..619642e 100644
--- a/aos/time/BUILD
+++ b/aos/time/BUILD
@@ -11,7 +11,6 @@
visibility = ["//visibility:public"],
deps = [
"//aos:macros",
- "//aos/ipc_lib:shared_mem",
"//aos/mutex",
"//aos/type_traits",
"@com_github_google_glog//:glog",
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 9736009..6c2d254 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -8,10 +8,6 @@
#ifdef __linux__
-// We only use global_core from here, which is weak, so we don't really have a
-// dependency on it.
-#include "aos/ipc_lib/shared_mem.h"
-
#include "aos/mutex/mutex.h"
#include "glog/logging.h"
@@ -103,15 +99,6 @@
SetMockTime(monotonic_clock::now() + amount);
}
-void OffsetToNow(monotonic_clock::time_point now) {
- CHECK_NOTNULL(&global_core);
- CHECK_NOTNULL(global_core);
- CHECK_NOTNULL(global_core->mem_struct);
- const auto offset = now - monotonic_clock::now();
- global_core->mem_struct->time_offset =
- chrono::duration_cast<chrono::nanoseconds>(offset).count();
-}
-
#endif // __linux__
struct timespec to_timespec(
@@ -150,14 +137,8 @@
<< ": clock_gettime(" << static_cast<uintmax_t>(CLOCK_MONOTONIC) << ", "
<< ¤t_time << ") failed";
- const chrono::nanoseconds offset =
- (&global_core == nullptr || global_core == nullptr ||
- global_core->mem_struct == nullptr)
- ? chrono::nanoseconds(0)
- : chrono::nanoseconds(global_core->mem_struct->time_offset);
-
return time_point(::std::chrono::seconds(current_time.tv_sec) +
- ::std::chrono::nanoseconds(current_time.tv_nsec)) + offset;
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
#else // __linux__
diff --git a/aos/time/time.h b/aos/time/time.h
index 469534f..881f0f1 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -82,14 +82,6 @@
// Disables mocking time.
void DisableMockTime();
-// Sets the global offset for all times so monotonic_clock::now() will return
-// now.
-// There is no synchronization here, so this is only safe when only a single
-// task is running.
-// This is only allowed when the shared memory core infrastructure has been
-// initialized in this process.
-void OffsetToNow(const monotonic_clock::time_point now);
-
// Construct a time representing the period of hertz.
constexpr ::std::chrono::nanoseconds FromRate(int hertz) {
return ::std::chrono::duration_cast<::std::chrono::nanoseconds>(
diff --git a/aos/type_traits/type_traits.h b/aos/type_traits/type_traits.h
index 437cb3e..45268d5 100644
--- a/aos/type_traits/type_traits.h
+++ b/aos/type_traits/type_traits.h
@@ -12,8 +12,10 @@
// 4.6 seems like a reasonable place to switch.
#if ((__GNUC__ < 4) || (__GNUC__ == 4 && __GNUC_MINOR__ < 6)) && !defined(__clang__)
::std::has_trivial_assign<Tp>::value> {};
-#else
+#elif (__clang_major__ < 5)
::std::has_trivial_copy_assign<Tp>::value> {};
+#else
+ ::std::is_trivially_copy_assignable<Tp>::value> {};
#endif
} // namespace
@@ -31,10 +33,15 @@
struct shm_ok : public std::integral_constant<
bool,
#if ((__GNUC__ < 5))
- (::std::has_trivial_copy_constructor<Tp>::value &&
+#if (__clang_major__ > 5)
+ (::std::is_trivially_copy_constructible<Tp>::value &&
::aos::has_trivial_copy_assign<Tp>::value)
#else
- (::std::is_trivially_copyable<Tp>::value)
+ (::std::has_trivial_copy_constructor<Tp>::value &&
+ ::aos::has_trivial_copy_assign<Tp>::value)
+#endif
+#else
+ (::std::is_trivially_copy_constructible<Tp>::value)
#endif
> {
};
diff --git a/aos/util/BUILD b/aos/util/BUILD
index f18fd82..4d7a3d1 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -65,7 +65,7 @@
name = "math",
hdrs = ["math.h"],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -111,7 +111,6 @@
"phased_loop.h",
],
deps = [
- "//aos/logging",
"//aos/time",
"@com_github_google_glog//:glog",
],
@@ -156,7 +155,7 @@
],
deps = [
"//aos:macros",
- "//aos/logging",
+ "@com_github_google_glog//:glog",
],
)
@@ -174,7 +173,7 @@
deps = [
"//aos/logging",
"//aos/time",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -296,8 +295,8 @@
"file.h",
],
deps = [
- "//aos/logging",
"//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
],
)
@@ -311,7 +310,6 @@
deps = [
":file",
"//aos/testing:googletest",
- "//aos/testing:test_logging",
],
)
diff --git a/aos/util/file.cc b/aos/util/file.cc
index 5418aab..f78239a 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -4,8 +4,8 @@
#include <unistd.h>
#include "absl/strings/string_view.h"
-#include "aos/logging/logging.h"
#include "aos/scoped/scoped_fd.h"
+#include "glog/logging.h"
namespace aos {
namespace util {
@@ -13,17 +13,12 @@
::std::string ReadFileToStringOrDie(const absl::string_view filename) {
::std::string r;
ScopedFD fd(open(::std::string(filename).c_str(), O_RDONLY));
- if (fd.get() == -1) {
- AOS_PLOG(FATAL, "opening %*s", static_cast<int>(filename.size()),
- filename.data());
- }
+ PCHECK(fd.get() != -1) << ": opening " << filename;
while (true) {
char buffer[1024];
const ssize_t result = read(fd.get(), buffer, sizeof(buffer));
- if (result < 0) {
- AOS_PLOG(FATAL, "reading from %*s", static_cast<int>(filename.size()),
- filename.data());
- } else if (result == 0) {
+ PCHECK(result >= 0) << ": reading from " << filename;
+ if (result == 0) {
break;
}
r.append(buffer, result);
@@ -31,5 +26,24 @@
return r;
}
+void WriteStringToFileOrDie(const absl::string_view filename,
+ const absl::string_view contents) {
+ ::std::string r;
+ ScopedFD fd(open(::std::string(filename).c_str(),
+ O_CREAT | O_WRONLY | O_TRUNC, S_IRWXU));
+ PCHECK(fd.get() != -1) << ": opening " << filename;
+ size_t size_written = 0;
+ while (size_written != contents.size()) {
+ const ssize_t result = write(fd.get(), contents.data() + size_written,
+ contents.size() - size_written);
+ PCHECK(result >= 0) << ": reading from " << filename;
+ if (result == 0) {
+ break;
+ }
+
+ size_written += result;
+ }
+}
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
index 385aba7..5d354a7 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -12,6 +12,10 @@
// encountered.
::std::string ReadFileToStringOrDie(const absl::string_view filename);
+// Creates filename if it doesn't exist and sets the contents to contents.
+void WriteStringToFileOrDie(const absl::string_view filename,
+ const absl::string_view contents);
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file_test.cc b/aos/util/file_test.cc
index f904259..fa259e8 100644
--- a/aos/util/file_test.cc
+++ b/aos/util/file_test.cc
@@ -6,21 +6,12 @@
#include "gtest/gtest.h"
-#include "aos/testing/test_logging.h"
-
namespace aos {
namespace util {
namespace testing {
-class FileTest : public ::testing::Test {
- protected:
- FileTest() {
- ::aos::testing::EnableTestLogging();
- }
-};
-
// Basic test of reading a normal file.
-TEST_F(FileTest, ReadNormalFile) {
+TEST(FileTest, ReadNormalFile) {
const ::std::string tmpdir(getenv("TEST_TMPDIR"));
const ::std::string test_file = tmpdir + "/test_file";
ASSERT_EQ(0, system(("echo contents > " + test_file).c_str()));
@@ -28,7 +19,7 @@
}
// Tests reading a file with 0 size, among other weird things.
-TEST_F(FileTest, ReadSpecialFile) {
+TEST(FileTest, ReadSpecialFile) {
const ::std::string stat = ReadFileToStringOrDie("/proc/self/stat");
EXPECT_EQ('\n', stat[stat.size() - 1]);
const ::std::string my_pid = ::std::to_string(getpid());
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 0a0d5db..4a80522 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -10,11 +10,7 @@
using ::std::chrono::milliseconds;
-class PhasedLoopTest : public ::testing::Test {
- protected:
- PhasedLoopTest() { ::aos::testing::EnableTestLogging(); }
-};
-
+typedef ::testing::Test PhasedLoopTest;
typedef PhasedLoopTest PhasedLoopDeathTest;
monotonic_clock::time_point InMs(int ms) {
diff --git a/aos/util/thread.cc b/aos/util/thread.cc
index ae18fe9..bb5999f 100644
--- a/aos/util/thread.cc
+++ b/aos/util/thread.cc
@@ -3,7 +3,7 @@
#include <pthread.h>
#include <signal.h>
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
namespace util {
@@ -11,24 +11,24 @@
Thread::Thread() : started_(false), joined_(false), should_terminate_(false) {}
Thread::~Thread() {
- AOS_CHECK(!(started_ && !joined_));
+ CHECK(!(started_ && !joined_));
}
void Thread::Start() {
- AOS_CHECK(!started_);
+ CHECK(!started_);
started_ = true;
- AOS_CHECK(pthread_create(&thread_, NULL, &Thread::StaticRun, this) == 0);
+ CHECK(pthread_create(&thread_, NULL, &Thread::StaticRun, this) == 0);
}
void Thread::Join() {
- AOS_CHECK(!joined_ && started_);
+ CHECK(!joined_ && started_);
joined_ = true;
should_terminate_.store(true);
- AOS_CHECK(pthread_join(thread_, NULL) == 0);
+ CHECK(pthread_join(thread_, NULL) == 0);
}
bool Thread::TryJoin() {
- AOS_CHECK(!joined_ && started_);
+ CHECK(!joined_ && started_);
#ifdef AOS_SANITIZER_thread
// ThreadSanitizer misses the tryjoin and then complains about leaking the
// thread. Instead, we'll just check if the thread is still around and then
@@ -39,7 +39,8 @@
if (kill_ret == 0) return false;
// If it died, we'll get ESRCH. Otherwise, something went wrong.
if (kill_ret != ESRCH) {
- AOS_PELOG(FATAL, kill_ret, "pthread_kill(thread_, 0) failed");
+ errno = kill_ret;
+ PLOG(FATAL) << "pthread_kill(thread_, 0) failed";
}
Join();
return true;
@@ -51,20 +52,22 @@
} else if (ret == EBUSY) {
return false;
} else {
- AOS_PELOG(FATAL, ret, "pthread_tryjoin_np(thread_, nullptr) failed");
+ errno = ret;
+ PLOG(FATAL) << "pthread_tryjoin_np(thread_, nullptr) failed";
+ return false;
}
#endif
}
void Thread::RequestStop() {
- AOS_CHECK(!joined_ && started_);
+ CHECK(!joined_ && started_);
should_terminate_.store(true);
}
void Thread::WaitUntilDone() {
- AOS_CHECK(!joined_ && started_);
+ CHECK(!joined_ && started_);
joined_ = true;
- AOS_CHECK(pthread_join(thread_, NULL) == 0);
+ CHECK(pthread_join(thread_, NULL) == 0);
}
void *Thread::StaticRun(void *self) {
diff --git a/aos/util/thread.h b/aos/util/thread.h
index 5992810..337ea48 100644
--- a/aos/util/thread.h
+++ b/aos/util/thread.h
@@ -13,7 +13,7 @@
// A nice wrapper around a pthreads thread.
//
-// TODO(aschuh): Test this.
+// TODO(aschuh): replace this with std::thread
class Thread {
public:
Thread();
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index 7d5f930..e025ed3 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -11,7 +11,7 @@
"//aos/vision/image:image_types",
"//aos/vision/math:segment",
"//aos/vision/math:vector",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -72,7 +72,7 @@
":contour",
":disjoint_set",
":range_image",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -90,7 +90,7 @@
":range_image",
"//aos/vision/debug:overlay",
"//aos/vision/math:segment",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -102,7 +102,7 @@
":range_image",
"//aos/vision/debug:overlay",
"//aos/vision/math:segment",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/aos/vision/blob/threshold.h b/aos/vision/blob/threshold.h
index 8251b3a..f126b14 100644
--- a/aos/vision/blob/threshold.h
+++ b/aos/vision/blob/threshold.h
@@ -1,6 +1,7 @@
#ifndef AOS_VISION_BLOB_THRESHOLD_H_
#define AOS_VISION_BLOB_THRESHOLD_H_
+#include <array>
#include <condition_variable>
#include <mutex>
#include <thread>
diff --git a/aos/vision/blob/threshold_test.cc b/aos/vision/blob/threshold_test.cc
index 108da35..3bbef1d 100644
--- a/aos/vision/blob/threshold_test.cc
+++ b/aos/vision/blob/threshold_test.cc
@@ -49,84 +49,84 @@
std::vector<char> image;
image.resize(8 * 8 * 2);
// --+-----
- image[0 * 2 + 0 * 16] = 0;
- image[1 * 2 + 0 * 16] = 0;
- image[2 * 2 + 0 * 16] = 128;
- image[3 * 2 + 0 * 16] = 127;
- image[4 * 2 + 0 * 16] = 0;
- image[5 * 2 + 0 * 16] = 0;
- image[6 * 2 + 0 * 16] = 0;
- image[7 * 2 + 0 * 16] = 0;
+ image[0 * 2 + 0 * 16] = static_cast<char>(0);
+ image[1 * 2 + 0 * 16] = static_cast<char>(0);
+ image[2 * 2 + 0 * 16] = static_cast<char>(128);
+ image[3 * 2 + 0 * 16] = static_cast<char>(127);
+ image[4 * 2 + 0 * 16] = static_cast<char>(0);
+ image[5 * 2 + 0 * 16] = static_cast<char>(0);
+ image[6 * 2 + 0 * 16] = static_cast<char>(0);
+ image[7 * 2 + 0 * 16] = static_cast<char>(0);
expected_ranges.push_back({{{2, 3}}});
// +------+
- image[0 * 2 + 1 * 16] = 128;
- image[1 * 2 + 1 * 16] = 0;
- image[2 * 2 + 1 * 16] = 0;
- image[3 * 2 + 1 * 16] = 10;
- image[4 * 2 + 1 * 16] = 30;
- image[5 * 2 + 1 * 16] = 50;
- image[6 * 2 + 1 * 16] = 70;
- image[7 * 2 + 1 * 16] = 255;
+ image[0 * 2 + 1 * 16] = static_cast<char>(128);
+ image[1 * 2 + 1 * 16] = static_cast<char>(0);
+ image[2 * 2 + 1 * 16] = static_cast<char>(0);
+ image[3 * 2 + 1 * 16] = static_cast<char>(10);
+ image[4 * 2 + 1 * 16] = static_cast<char>(30);
+ image[5 * 2 + 1 * 16] = static_cast<char>(50);
+ image[6 * 2 + 1 * 16] = static_cast<char>(70);
+ image[7 * 2 + 1 * 16] = static_cast<char>(255);
expected_ranges.push_back({{{0, 1}, {7, 8}}});
// -++++++-
- image[0 * 2 + 2 * 16] = 73;
- image[1 * 2 + 2 * 16] = 246;
- image[2 * 2 + 2 * 16] = 247;
- image[3 * 2 + 2 * 16] = 248;
- image[4 * 2 + 2 * 16] = 249;
- image[5 * 2 + 2 * 16] = 250;
- image[6 * 2 + 2 * 16] = 250;
- image[7 * 2 + 2 * 16] = 45;
+ image[0 * 2 + 2 * 16] = static_cast<char>(73);
+ image[1 * 2 + 2 * 16] = static_cast<char>(246);
+ image[2 * 2 + 2 * 16] = static_cast<char>(247);
+ image[3 * 2 + 2 * 16] = static_cast<char>(248);
+ image[4 * 2 + 2 * 16] = static_cast<char>(249);
+ image[5 * 2 + 2 * 16] = static_cast<char>(250);
+ image[6 * 2 + 2 * 16] = static_cast<char>(250);
+ image[7 * 2 + 2 * 16] = static_cast<char>(45);
expected_ranges.push_back({{{1, 7}}});
// +++-++++
- image[0 * 2 + 3 * 16] = 128;
- image[1 * 2 + 3 * 16] = 134;
- image[2 * 2 + 3 * 16] = 250;
- image[3 * 2 + 3 * 16] = 0;
- image[4 * 2 + 3 * 16] = 230;
- image[5 * 2 + 3 * 16] = 230;
- image[6 * 2 + 3 * 16] = 230;
- image[7 * 2 + 3 * 16] = 210;
+ image[0 * 2 + 3 * 16] = static_cast<char>(128);
+ image[1 * 2 + 3 * 16] = static_cast<char>(134);
+ image[2 * 2 + 3 * 16] = static_cast<char>(250);
+ image[3 * 2 + 3 * 16] = static_cast<char>(0);
+ image[4 * 2 + 3 * 16] = static_cast<char>(230);
+ image[5 * 2 + 3 * 16] = static_cast<char>(230);
+ image[6 * 2 + 3 * 16] = static_cast<char>(230);
+ image[7 * 2 + 3 * 16] = static_cast<char>(210);
expected_ranges.push_back({{{0, 3}, {4, 8}}});
// --------
- image[0 * 2 + 4 * 16] = 7;
- image[1 * 2 + 4 * 16] = 120;
- image[2 * 2 + 4 * 16] = 127;
- image[3 * 2 + 4 * 16] = 0;
- image[4 * 2 + 4 * 16] = 50;
- image[5 * 2 + 4 * 16] = 80;
- image[6 * 2 + 4 * 16] = 110;
- image[7 * 2 + 4 * 16] = 25;
+ image[0 * 2 + 4 * 16] = static_cast<char>(7);
+ image[1 * 2 + 4 * 16] = static_cast<char>(120);
+ image[2 * 2 + 4 * 16] = static_cast<char>(127);
+ image[3 * 2 + 4 * 16] = static_cast<char>(0);
+ image[4 * 2 + 4 * 16] = static_cast<char>(50);
+ image[5 * 2 + 4 * 16] = static_cast<char>(80);
+ image[6 * 2 + 4 * 16] = static_cast<char>(110);
+ image[7 * 2 + 4 * 16] = static_cast<char>(25);
expected_ranges.push_back({{}});
// ++++-+++
- image[0 * 2 + 5 * 16] = 140;
- image[1 * 2 + 5 * 16] = 140;
- image[2 * 2 + 5 * 16] = 140;
- image[3 * 2 + 5 * 16] = 140;
- image[4 * 2 + 5 * 16] = 0;
- image[5 * 2 + 5 * 16] = 140;
- image[6 * 2 + 5 * 16] = 140;
- image[7 * 2 + 5 * 16] = 140;
+ image[0 * 2 + 5 * 16] = static_cast<char>(140);
+ image[1 * 2 + 5 * 16] = static_cast<char>(140);
+ image[2 * 2 + 5 * 16] = static_cast<char>(140);
+ image[3 * 2 + 5 * 16] = static_cast<char>(140);
+ image[4 * 2 + 5 * 16] = static_cast<char>(0);
+ image[5 * 2 + 5 * 16] = static_cast<char>(140);
+ image[6 * 2 + 5 * 16] = static_cast<char>(140);
+ image[7 * 2 + 5 * 16] = static_cast<char>(140);
expected_ranges.push_back({{{0, 4}, {5, 8}}});
// ++++++++
- image[0 * 2 + 6 * 16] = 128;
- image[1 * 2 + 6 * 16] = 128;
- image[2 * 2 + 6 * 16] = 128;
- image[3 * 2 + 6 * 16] = 128;
- image[4 * 2 + 6 * 16] = 128;
- image[5 * 2 + 6 * 16] = 128;
- image[6 * 2 + 6 * 16] = 128;
- image[7 * 2 + 6 * 16] = 128;
+ image[0 * 2 + 6 * 16] = static_cast<char>(128);
+ image[1 * 2 + 6 * 16] = static_cast<char>(128);
+ image[2 * 2 + 6 * 16] = static_cast<char>(128);
+ image[3 * 2 + 6 * 16] = static_cast<char>(128);
+ image[4 * 2 + 6 * 16] = static_cast<char>(128);
+ image[5 * 2 + 6 * 16] = static_cast<char>(128);
+ image[6 * 2 + 6 * 16] = static_cast<char>(128);
+ image[7 * 2 + 6 * 16] = static_cast<char>(128);
expected_ranges.push_back({{{0, 8}}});
// +-+-+--+
- image[0 * 2 + 7 * 16] = 200;
- image[1 * 2 + 7 * 16] = 0;
- image[2 * 2 + 7 * 16] = 200;
- image[3 * 2 + 7 * 16] = 0;
- image[4 * 2 + 7 * 16] = 200;
- image[5 * 2 + 7 * 16] = 0;
- image[6 * 2 + 7 * 16] = 0;
- image[7 * 2 + 7 * 16] = 200;
+ image[0 * 2 + 7 * 16] = static_cast<char>(200);
+ image[1 * 2 + 7 * 16] = static_cast<char>(0);
+ image[2 * 2 + 7 * 16] = static_cast<char>(200);
+ image[3 * 2 + 7 * 16] = static_cast<char>(0);
+ image[4 * 2 + 7 * 16] = static_cast<char>(200);
+ image[5 * 2 + 7 * 16] = static_cast<char>(0);
+ image[6 * 2 + 7 * 16] = static_cast<char>(0);
+ image[7 * 2 + 7 * 16] = static_cast<char>(200);
expected_ranges.push_back({{{0, 1}, {2, 3}, {4, 5}, {7, 8}}});
const RangeImage expected_result(0, std::move(expected_ranges));
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index b887872..a991968 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -52,6 +52,7 @@
deps = [
"//aos:macros",
"//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
],
)
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 8734443..7ac72e8 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -2,23 +2,23 @@
#include <string.h>
-#include "aos/logging/logging.h"
+#include "glog/logging.h"
namespace aos {
namespace events {
TXUdpSocket::TXUdpSocket(const std::string &ip_addr, int port)
- : fd_(AOS_PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
+ : fd_(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) {
+ PCHECK(fd_.get() != -1);
sockaddr_in destination_in;
memset(&destination_in, 0, sizeof(destination_in));
destination_in.sin_family = AF_INET;
destination_in.sin_port = htons(port);
- if (inet_aton(ip_addr.c_str(), &destination_in.sin_addr) == 0) {
- AOS_LOG(FATAL, "invalid IP address %s\n", ip_addr.c_str());
- }
+ CHECK(inet_aton(ip_addr.c_str(), &destination_in.sin_addr) != 0)
+ << ": invalid IP address " << ip_addr;
- AOS_PCHECK(connect(fd_.get(), reinterpret_cast<sockaddr *>(&destination_in),
- sizeof(destination_in)));
+ PCHECK(connect(fd_.get(), reinterpret_cast<sockaddr *>(&destination_in),
+ sizeof(destination_in)) == 0);
}
int TXUdpSocket::Send(const char *data, int size) {
@@ -27,7 +27,8 @@
}
int RXUdpSocket::SocketBindListenOnPort(int port) {
- int fd = AOS_PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP));
+ int fd;
+ PCHECK((fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) != -1);
sockaddr_in bind_address;
memset(&bind_address, 0, sizeof(bind_address));
@@ -35,15 +36,17 @@
bind_address.sin_port = htons(port);
bind_address.sin_addr.s_addr = htonl(INADDR_ANY);
- AOS_PCHECK(bind(fd, reinterpret_cast<sockaddr *>(&bind_address),
- sizeof(bind_address)));
+ PCHECK(bind(fd, reinterpret_cast<sockaddr *>(&bind_address),
+ sizeof(bind_address)) == 0);
return fd;
}
RXUdpSocket::RXUdpSocket(int port) : fd_(SocketBindListenOnPort(port)) {}
int RXUdpSocket::Recv(void *data, int size) {
- return AOS_PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
+ int result;
+ PCHECK((result = recv(fd_.get(), static_cast<char *>(data), size, 0)) != -1);
+ return result;
}
} // namespace events
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
index 21dbbb8..9b6d30a 100644
--- a/aos/vision/image/BUILD
+++ b/aos/vision/image/BUILD
@@ -5,6 +5,9 @@
cc_library(
name = "image_types",
hdrs = ["image_types.h"],
+ deps = [
+ "@com_google_absl//absl/strings",
+ ],
)
cc_proto_library(
diff --git a/aos/vision/image/image_dataset.cc b/aos/vision/image/image_dataset.cc
index d4dde95..33bf0e4 100644
--- a/aos/vision/image/image_dataset.cc
+++ b/aos/vision/image/image_dataset.cc
@@ -37,10 +37,10 @@
res.reserve(pos.size() + 1);
i = 0;
for (auto p : pos) {
- res.emplace_back(inp.substr(i, p - i).to_string());
+ res.emplace_back(std::string(inp.substr(i, p - i)));
i = p + 1;
}
- res.emplace_back(inp.substr(i).to_string());
+ res.emplace_back(std::string(inp.substr(i)));
return res;
}
} // namespace
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
index a620d8a..aff7056 100644
--- a/aos/vision/image/image_types.h
+++ b/aos/vision/image/image_types.h
@@ -6,7 +6,7 @@
#include <memory>
#include <sstream>
-#include <experimental/string_view>
+#include "absl/strings/string_view.h"
namespace aos {
namespace vision {
@@ -20,7 +20,7 @@
};
// This will go into c++17. No sense writing my own version.
-using DataRef = std::experimental::string_view;
+using DataRef = absl::string_view;
// Represents the dimensions of an image.
struct ImageFormat {
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index 0fbdf7a..2c71e8f 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -12,8 +12,7 @@
"vector.h",
],
deps = [
- "//third_party/eigen",
- "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/build_tests/BUILD b/build_tests/BUILD
index bf6e9ee..9dd539a 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -1,5 +1,3 @@
-load("//tools/build_rules:ruby.bzl", "ruby_binary")
-load("//aos/build:queues.bzl", "queue_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("//tools/cpp/emscripten:defs.bzl", "emcc_binary")
@@ -38,46 +36,6 @@
],
)
-ruby_binary(
- name = "ruby_binary",
- srcs = [
- "ruby.rb",
- "ruby_to_require.rb",
- ],
- data = [
- "ruby_to_require.rb",
- ],
-)
-
-sh_test(
- name = "ruby_build_test",
- size = "small",
- srcs = [
- "ruby_check.sh",
- ],
- data = [
- ":ruby_binary",
- ],
-)
-
-queue_library(
- name = "queue_library",
- srcs = [
- "queue.q",
- ],
-)
-
-cc_test(
- name = "queue_build_test",
- size = "small",
- srcs = [
- "queue.cc",
- ],
- deps = [
- ":queue_library",
- ],
-)
-
cc_binary(
name = "tcmalloc_build_test_binary",
srcs = [
diff --git a/build_tests/queue.cc b/build_tests/queue.cc
deleted file mode 100644
index 7f67f6a..0000000
--- a/build_tests/queue.cc
+++ /dev/null
@@ -1,12 +0,0 @@
-#include <stdio.h>
-
-#include "build_tests/queue.q.h"
-
-int main() {
- ::build_tests::TestMessage s;
- s.field = 971;
- char buffer[1024];
- s.Print(buffer, sizeof(buffer));
- buffer[sizeof(buffer) - 1] = '\0';
- printf("s = {%s}\n", buffer);
-}
diff --git a/build_tests/queue.q b/build_tests/queue.q
deleted file mode 100644
index 9a32a7c..0000000
--- a/build_tests/queue.q
+++ /dev/null
@@ -1,5 +0,0 @@
-package build_tests;
-
-message TestMessage {
- int32_t field;
-};
diff --git a/build_tests/ruby.rb b/build_tests/ruby.rb
deleted file mode 100644
index 12af7fe..0000000
--- a/build_tests/ruby.rb
+++ /dev/null
@@ -1,18 +0,0 @@
-# ruby_check.sh runs this and verifies the output matches.
-# The first argument is the absolute path to the runfiles directory to use for
-# testing require with an absolute path.
-
-$loaded = false
-raise unless require_relative 'ruby_to_require.rb'
-raise unless $loaded
-
-# We already loaded this above, so it won't happen again.
-raise if require 'build_tests/ruby_to_require'
-
-$loaded = false
-raise unless require ARGV[0] + '/build_tests/ruby_to_require'
-raise unless $loaded
-
-raise unless ''.encoding == Encoding::UTF_8
-
-puts 'Hi from ruby'
diff --git a/build_tests/ruby_check.sh b/build_tests/ruby_check.sh
deleted file mode 100755
index 70d3149..0000000
--- a/build_tests/ruby_check.sh
+++ /dev/null
@@ -1,14 +0,0 @@
-#!/bin/bash
-
-# Checks the output from ruby.rb.
-
-set -e
-set -u
-
-OUTPUT="$("./build_tests/ruby_binary" "${PWD}")"
-
-if [[ "${OUTPUT}" != "Hi from ruby" ]]; then
- echo "Output is actually:" >&2
- echo "${OUTPUT}" >&2
- exit 1
-fi
diff --git a/build_tests/ruby_to_require.rb b/build_tests/ruby_to_require.rb
deleted file mode 100644
index b63cda2..0000000
--- a/build_tests/ruby_to_require.rb
+++ /dev/null
@@ -1,4 +0,0 @@
-# This file is loaded by ruby.rb through several mechanisms to make sure they
-# all work.
-
-$loaded = true
diff --git a/debian/BUILD b/debian/BUILD
index f7d3113..503274c 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -48,10 +48,6 @@
":python_gtk.bzl",
python_gtk_debs = "files",
)
-load(
- ":ruby.bzl",
- ruby_debs = "files",
-)
load("//debian:packages.bzl", "download_packages", "generate_deb_tarball")
filegroup(
@@ -130,16 +126,6 @@
)
download_packages(
- name = "download_ruby_deps",
- excludes = [
- "ca-certificates",
- ],
- packages = [
- "ruby",
- ],
-)
-
-download_packages(
name = "download_patch_deps",
packages = [
"patch",
@@ -293,8 +279,3 @@
name = "python_gtk",
files = python_gtk_debs,
)
-
-generate_deb_tarball(
- name = "ruby",
- files = ruby_debs,
-)
diff --git a/debian/ruby.BUILD b/debian/ruby.BUILD
deleted file mode 100644
index 73f9e4a..0000000
--- a/debian/ruby.BUILD
+++ /dev/null
@@ -1,9 +0,0 @@
-filegroup(
- name = "ruby",
- srcs = glob(
- [
- "usr/**",
- ],
- ),
- visibility = ["//visibility:public"],
-)
diff --git a/debian/ruby.bzl b/debian/ruby.bzl
deleted file mode 100644
index e839fac..0000000
--- a/debian/ruby.bzl
+++ /dev/null
@@ -1,16 +0,0 @@
-files = {
- "libffi6_3.1-2+deb8u1_amd64.deb": "100343fca79ff265abc62467c7085fca68b8764e8c2551302ab741c771e7f0aa",
- "libgdbm3_1.8.3-13.1_amd64.deb": "5d5566359c3a5dc86e2f386b9d9d84c94f580b2e5f2553097cf3e10bd3090de0",
- "libgmp10_6.0.0+dfsg-6_amd64.deb": "155a31b0f716aa3dcd7ee68e9bd57e0b76a6b31f4e41fb2d953e986315437082",
- "libncurses5_5.9+20140913-1+deb8u3_amd64.deb": "529417741277e91ed343e1e01aa68a0e4bca33b1fee5bdfb19093634b21ed0c5",
- "libreadline6_6.3-8+b3_amd64.deb": "647948737fcfea4749368aa233b2d8b89032546ba4db2f0338239e9a7f4bda3e",
- "libruby2.1_2.1.5-2+deb8u5_amd64.deb": "5cbe0b9d52ec9e8f0ecc1f361763f1188d721362bae4b63a34e30c3bfcbe2473",
- "libssl1.0.0_1.0.1t-1+deb8u9_amd64.deb": "4edf4d85387f2bd57c8eb372e81373562d57c2f952b6956f8378a0cacf199ba5",
- "libtinfo5_5.9+20140913-1+deb8u3_amd64.deb": "36a0f120da15f82c0c729535ac48d95e126c5d7fc2c4aceb94bf27ced5a4cecc",
- "libyaml-0-2_0.1.6-3_amd64.deb": "5885db15ac425eb7231c436903525b78381e034bcc53928a97997a745295d222",
- "openssl_1.0.1t-1+deb8u9_amd64.deb": "4725726faeba1f58d64dbb56230eb81607bc926d7a6dc8367e82717bf8cfa527",
- "readline-common_6.3-8_all.deb": "8b91bce988c38798e565820919a600f1a58ca483d8406860cc37e847a55a6bfd",
- "ruby2.1_2.1.5-2+deb8u5_amd64.deb": "254c0e1506b12b2c4872839a42fe73cec08b2053d268a6603650190d406865ac",
- "ruby_2.1.5+deb8u2_all.deb": "aced2bae409b1aa503363a98b6dc7eb799f59f3a7ee48ac357f458643aa729fb",
- "rubygems-integration_1.8_all.deb": "944202a99a704fd708d03a768981cf63406fb41f6a311464993f39a11a215bc1",
-}
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index 2d74bf2..2eb4be0 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -1,15 +1,11 @@
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 = "auto_queue",
- srcs = [
- "auto.q",
- ],
- deps = [
- "//aos/actions:action_queue",
- ],
+flatbuffer_cc_library(
+ name = "auto_fbs",
+ srcs = ["auto.fbs"],
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -21,13 +17,15 @@
"base_autonomous_actor.h",
],
deps = [
- ":auto_queue",
+ ":auto_fbs",
"//aos/actions:action_lib",
"//aos/logging",
"//aos/util:phased_loop",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
- "//y2019/control_loops/drivetrain:target_selector_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
],
)
diff --git a/frc971/autonomous/auto.fbs b/frc971/autonomous/auto.fbs
new file mode 100644
index 0000000..92ca532
--- /dev/null
+++ b/frc971/autonomous/auto.fbs
@@ -0,0 +1,17 @@
+namespace frc971.autonomous;
+
+// Published on ".frc971.autonomous.auto_mode"
+table AutonomousMode {
+ // Mode read from the mode setting sensors.
+ mode:int;
+}
+
+table AutonomousActionParams {
+ // The mode from the sensors when auto starts.
+ mode:int;
+}
+
+table Goal {
+ run:uint;
+ params:AutonomousActionParams;
+}
diff --git a/frc971/autonomous/auto.q b/frc971/autonomous/auto.q
deleted file mode 100644
index f107c90..0000000
--- a/frc971/autonomous/auto.q
+++ /dev/null
@@ -1,26 +0,0 @@
-package frc971.autonomous;
-
-import "aos/actions/actions.q";
-
-// Published on ".frc971.autonomous.auto_mode"
-message AutonomousMode {
- // Mode read from the mode setting sensors.
- int32_t mode;
-};
-
-struct AutonomousActionParams {
- // The mode from the sensors when auto starts.
- int32_t mode;
-};
-
-queue_group AutonomousActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- AutonomousActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 367e849..dee2931 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -8,13 +8,15 @@
#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
+namespace drivetrain = frc971::control_loops::drivetrain;
namespace frc971 {
namespace autonomous {
@@ -22,41 +24,37 @@
BaseAutonomousActor::BaseAutonomousActor(
::aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
- : aos::common::actions::ActorBase<AutonomousActionQueueGroup>(
- event_loop, ".frc971.autonomous.autonomous_action"),
+ : aos::common::actions::ActorBase<Goal>(event_loop, "/autonomous"),
dt_config_(dt_config),
initial_drivetrain_({0.0, 0.0}),
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
+ "/drivetrain")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ event_loop->MakeSender<drivetrain::Goal>("/drivetrain")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<drivetrain::Status>("/drivetrain")),
drivetrain_goal_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeFetcher<drivetrain::Goal>("/drivetrain")) {}
void BaseAutonomousActor::ResetDrivetrain() {
AOS_LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
goal_spline_handle_ = 0;
- auto drivetrain_goal_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_goal_message->controller_type = 0;
- drivetrain_goal_message->highgear = true;
- drivetrain_goal_message->wheel = 0.0;
- drivetrain_goal_message->throttle = 0.0;
- drivetrain_goal_message->left_goal = initial_drivetrain_.left;
- drivetrain_goal_message->right_goal = initial_drivetrain_.right;
- drivetrain_goal_message->max_ss_voltage = max_drivetrain_voltage_;
- drivetrain_goal_message.Send();
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(drivetrain::ControllerType_POLYDRIVE);
+ goal_builder.add_highgear(true);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_left_goal(initial_drivetrain_.left);
+ goal_builder.add_right_goal(initial_drivetrain_.right);
+ goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
+ builder.Send(goal_builder.Finish());
}
void BaseAutonomousActor::InitializeEncoders() {
@@ -64,14 +62,14 @@
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
initial_drivetrain_.left =
- drivetrain_status_fetcher_->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position();
initial_drivetrain_.right =
- drivetrain_status_fetcher_->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position();
}
void BaseAutonomousActor::StartDrive(double distance, double angle,
- ProfileParameters linear,
- ProfileParameters angular) {
+ ProfileParametersT linear,
+ ProfileParametersT angular) {
AOS_LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
{
const double dangle = angle * dt_config_.robot_radius;
@@ -79,20 +77,25 @@
initial_drivetrain_.right += distance + dangle;
}
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 1;
- drivetrain_message->highgear = true;
- drivetrain_message->wheel = 0.0;
- drivetrain_message->throttle = 0.0;
- drivetrain_message->left_goal = initial_drivetrain_.left;
- drivetrain_message->right_goal = initial_drivetrain_.right;
- drivetrain_message->max_ss_voltage = max_drivetrain_voltage_;
- drivetrain_message->linear = linear;
- drivetrain_message->angular = angular;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+ auto linear_offset = ProfileParameters::Pack(*builder.fbb(), &linear);
+ auto angular_offset = ProfileParameters::Pack(*builder.fbb(), &angular);
- drivetrain_message.Send();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+
+ goal_builder.add_controller_type(drivetrain::ControllerType_MOTION_PROFILE);
+ goal_builder.add_highgear(true);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_left_goal(initial_drivetrain_.left);
+ goal_builder.add_right_goal(initial_drivetrain_.right);
+ goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+
+ builder.Send(goal_builder.Finish());
}
void BaseAutonomousActor::WaitUntilDoneOrCanceled(
@@ -137,17 +140,17 @@
static constexpr double kProfileTolerance = 0.001;
if (drivetrain_status_fetcher_.get()) {
- if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal -
+ if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal() -
initial_drivetrain_.left) < kProfileTolerance &&
- ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal -
+ ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal() -
initial_drivetrain_.right) < kProfileTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_left_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_position() -
initial_drivetrain_.left) < kPositionTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_right_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_position() -
initial_drivetrain_.right) < kPositionTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity()) <
kVelocityTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity()) <
kVelocityTolerance) {
AOS_LOG(INFO, "Finished drive\n");
return true;
@@ -170,7 +173,7 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle > angle) {
+ if (drivetrain_status_fetcher_->ground_angle() > angle) {
return true;
}
}
@@ -191,7 +194,7 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle < angle) {
+ if (drivetrain_status_fetcher_->ground_angle() < angle) {
return true;
}
}
@@ -213,10 +216,10 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle > max_angle) {
- max_angle = drivetrain_status_fetcher_->ground_angle;
+ if (drivetrain_status_fetcher_->ground_angle() > max_angle) {
+ max_angle = drivetrain_status_fetcher_->ground_angle();
}
- if (drivetrain_status_fetcher_->ground_angle < max_angle - angle) {
+ if (drivetrain_status_fetcher_->ground_angle() < max_angle - angle) {
return true;
}
}
@@ -243,17 +246,17 @@
if (drivetrain_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal);
+ drivetrain_status_fetcher_->profiled_left_position_goal());
const double right_profile_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal);
+ drivetrain_status_fetcher_->profiled_right_position_goal());
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double profile_distance_to_go =
(left_profile_error + right_profile_error) / 2.0;
@@ -306,9 +309,9 @@
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal(),
0.0, initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal(),
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> linear_error =
@@ -342,9 +345,9 @@
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal(),
0.0, initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal(),
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> angular_error =
@@ -369,10 +372,10 @@
if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
return (left_error + right_error) / 2.0;
} else {
@@ -385,9 +388,11 @@
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
return base_autonomous_actor_->drivetrain_status_fetcher_
- ->trajectory_logging.is_executing &&
+ ->trajectory_logging()
+ ->is_executing() &&
base_autonomous_actor_->drivetrain_status_fetcher_
- ->trajectory_logging.distance_remaining < distance;
+ ->trajectory_logging()
+ ->distance_remaining() < distance;
}
return false;
}
@@ -408,61 +413,89 @@
}
}
-void BaseAutonomousActor::LineFollowAtVelocity(double velocity, int hint) {
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 3;
- // TODO(james): Currently the 4.0 is copied from the
- // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
- // factor it out in some way.
- drivetrain_message->throttle = velocity / 4.0;
- drivetrain_message.Send();
- auto target_hint = target_selector_hint_sender_.MakeMessage();
- target_hint->suggested_target = hint;
- target_hint.Send();
+void BaseAutonomousActor::LineFollowAtVelocity(
+ double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(
+ drivetrain::ControllerType_SPLINE_FOLLOWER);
+ // TODO(james): Currently the 4.0 is copied from the
+ // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
+ // factor it out in some way.
+ goal_builder.add_throttle(velocity / 4.0);
+ builder.Send(goal_builder.Finish());
+ }
+
+ {
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ ::y2019::control_loops::drivetrain::TargetSelectorHint::Builder
+ target_hint_builder = builder.MakeBuilder<
+ ::y2019::control_loops::drivetrain::TargetSelectorHint>();
+
+ target_hint_builder.add_suggested_target(hint);
+ builder.Send(target_hint_builder.Finish());
+ }
}
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
- const ::frc971::MultiSpline &spline, SplineDirection direction) {
+ std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+ &&multispline_builder,
+ SplineDirection direction) {
AOS_LOG(INFO, "Planning spline\n");
int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
drivetrain_goal_fetcher_.Fetch();
- auto drivetrain_message =
- drivetrain_goal_sender_.MakeMessage();
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- int controller_type = 2;
+ flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
+ multispline_builder(&builder);
+
+ drivetrain::SplineGoal::Builder spline_builder =
+ builder.MakeBuilder<drivetrain::SplineGoal>();
+ spline_builder.add_spline_idx(spline_handle);
+ spline_builder.add_drive_spline_backwards(direction ==
+ SplineDirection::kBackward);
+ spline_builder.add_spline(multispline_offset);
+
+ flatbuffers::Offset<drivetrain::SplineGoal> spline_offset =
+ spline_builder.Finish();
+
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+
+ drivetrain::ControllerType controller_type =
+ drivetrain::ControllerType_SPLINE_FOLLOWER;
if (drivetrain_goal_fetcher_.get()) {
- controller_type = drivetrain_goal_fetcher_->controller_type;
- drivetrain_message->throttle = drivetrain_goal_fetcher_->throttle;
+ controller_type = drivetrain_goal_fetcher_->controller_type();
+ goal_builder.add_throttle(drivetrain_goal_fetcher_->throttle());
}
- drivetrain_message->controller_type = controller_type;
+ goal_builder.add_controller_type(controller_type);
+ goal_builder.add_spline_handle(goal_spline_handle_);
- drivetrain_message->spline = spline;
- drivetrain_message->spline.spline_idx = spline_handle;
- drivetrain_message->spline_handle = goal_spline_handle_;
- drivetrain_message->spline.drive_spline_backwards =
- direction == SplineDirection::kBackward;
+ goal_builder.add_spline(spline_offset);
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
-
- drivetrain_message.Send();
+ builder.Send(goal_builder.Finish());
return BaseAutonomousActor::SplineHandle(spline_handle, this);
}
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- AOS_LOG_STRUCT(DEBUG, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
+ VLOG(1) << aos::FlatbufferToJson(
+ base_autonomous_actor_->drivetrain_status_fetcher_.get());
+
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
- ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_spline_idx == spline_handle_ &&
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_state == 3) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .current_spline_idx == spline_handle_)) {
+ ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_spline_idx() == spline_handle_ &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_state() == 3) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->current_spline_idx() == spline_handle_)) {
return true;
}
return false;
@@ -485,24 +518,21 @@
}
void BaseAutonomousActor::SplineHandle::Start() {
- auto drivetrain_message =
- base_autonomous_actor_->drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 2;
+ auto builder = base_autonomous_actor_->drivetrain_goal_sender_.MakeBuilder();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(drivetrain::ControllerType_SPLINE_FOLLOWER);
AOS_LOG(INFO, "Starting spline\n");
- drivetrain_message->spline_handle = spline_handle_;
+ goal_builder.add_spline_handle(spline_handle_);
base_autonomous_actor_->goal_spline_handle_ = spline_handle_;
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
-
- drivetrain_message.Send();
+ builder.Send(goal_builder.Finish());
}
bool BaseAutonomousActor::SplineHandle::IsDone() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- AOS_LOG_STRUCT(INFO, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
// We check that the spline we are waiting on is neither currently planning
// nor executing (we check is_executed because it is possible to receive
@@ -510,12 +540,13 @@
// We check for planning so that the user can go straight from starting the
// planner to executing without a WaitForPlan in between.
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
- ((!base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .is_executed &&
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .current_spline_idx == spline_handle_) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_spline_idx == spline_handle_)) {
+ ((!base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->is_executed() &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->current_spline_idx() == spline_handle_) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_spline_idx() == spline_handle_)) {
return false;
}
return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 5a5bcea..6035551 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -5,20 +5,20 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/shm-event-loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
namespace frc971 {
namespace autonomous {
-class BaseAutonomousActor
- : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+class BaseAutonomousActor : public ::aos::common::actions::ActorBase<Goal> {
public:
- typedef ::aos::common::actions::TypedActionFactory<AutonomousActionQueueGroup>
- Factory;
+ typedef ::aos::common::actions::TypedActionFactory<Goal> Factory;
explicit BaseAutonomousActor(
::aos::EventLoop *event_loop,
@@ -62,13 +62,16 @@
// Starts planning the spline, and returns a handle to be used to manipulate
// it.
- SplineHandle PlanSpline(const ::frc971::MultiSpline &spline,
- SplineDirection direction);
+ SplineHandle PlanSpline(
+ std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder)> &&multispline_builder,
+ SplineDirection direction);
void ResetDrivetrain();
void InitializeEncoders();
- void StartDrive(double distance, double angle, ProfileParameters linear,
- ProfileParameters angular);
+ void StartDrive(double distance, double angle, ProfileParametersT linear,
+ ProfileParametersT angular);
void WaitUntilDoneOrCanceled(
::std::unique_ptr<aos::common::actions::Action> action);
@@ -79,7 +82,10 @@
// Returns true if the drive has finished.
bool IsDriveDone();
- void LineFollowAtVelocity(double velocity, int hint = 0);
+ void LineFollowAtVelocity(
+ double velocity,
+ y2019::control_loops::drivetrain::SelectionHint hint =
+ y2019::control_loops::drivetrain::SelectionHint_NONE);
// Waits until the robot is pitched up above the specified angle, or the move
// finishes. Returns true on success, and false if it cancels.
@@ -115,11 +121,11 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
private:
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
index 0fe9205..6302443 100644
--- a/frc971/codelab/BUILD
+++ b/frc971/codelab/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_binary(
name = "basic_test",
@@ -8,10 +8,9 @@
srcs = ["basic_test.cc"],
deps = [
":basic",
- ":basic_queue",
- "//aos:queues",
+ ":basic_fbs",
"//aos/controls:control_loop_test",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
"//frc971/control_loops:team_number_test_environment",
@@ -23,18 +22,14 @@
srcs = ["basic.cc"],
hdrs = ["basic.h"],
deps = [
- ":basic_queue",
+ ":basic_fbs",
"//aos/controls:control_loop",
],
)
-queue_library(
- name = "basic_queue",
+flatbuffer_cc_library(
+ name = "basic_fbs",
srcs = [
- "basic.q",
- ],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ "basic.fbs",
],
)
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
index d06e285..29ffcf5 100644
--- a/frc971/codelab/basic.cc
+++ b/frc971/codelab/basic.cc
@@ -4,12 +4,12 @@
namespace codelab {
Basic::Basic(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<BasicQueue>(event_loop, name) {}
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name) {}
-void Basic::RunIteration(const BasicQueue::Goal *goal,
- const BasicQueue::Position *position,
- BasicQueue::Output *output,
- BasicQueue::Status *status) {
+void Basic::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
// TODO(you): Set the intake_voltage to 12 Volts when
// intake is requested (via intake in goal). Make sure not to set
// the motor to anything but 0 V when the limit_sensor is pressed.
diff --git a/frc971/codelab/basic.fbs b/frc971/codelab/basic.fbs
new file mode 100644
index 0000000..82c9607
--- /dev/null
+++ b/frc971/codelab/basic.fbs
@@ -0,0 +1,35 @@
+namespace frc971.codelab;
+
+// The theme of this basic test is a simple intake system.
+//
+// The system will have a motor driven by the voltage returned
+// by output, and then eventually this motor, when run enough,
+// will trigger the limit_sensor. The hypothetical motor should shut
+// off in that hypothetical situation to avoid hypothetical burnout.
+table Goal {
+ // The control loop needs to intake now.
+ intake:bool;
+}
+
+table Position {
+ // This is a potential incoming sensor value letting us know
+ // if we need to be intaking.
+ limit_sensor:bool;
+}
+
+table Status {
+ // Lets consumers of basic_queue.status know if
+ // the requested intake is finished.
+ intake_complete:bool;
+}
+
+table Output {
+ // This would be set up to drive a hypothetical motor that would
+ // hope to intake something.
+ intake_voltage:double;
+}
+
+root_type Goal;
+root_type Position;
+root_type Status;
+root_type Output;
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
index 6f33718..acaec0c 100644
--- a/frc971/codelab/basic.h
+++ b/frc971/codelab/basic.h
@@ -4,7 +4,7 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
-#include "frc971/codelab/basic.q.h"
+#include "frc971/codelab/basic_generated.h"
namespace frc971 {
namespace codelab {
@@ -41,16 +41,16 @@
// Order of approaching this should be:
// - Read the BUILD file and learn about what code is being generated.
// - Read basic.q, and familiarize yourself on the inputs and types involved.
-class Basic : public ::aos::controls::ControlLoop<BasicQueue> {
+class Basic
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
explicit Basic(::aos::EventLoop *event_loop,
- const ::std::string &name = ".frc971.codelab.basic_queue");
+ const ::std::string &name = "/codelab");
protected:
- void RunIteration(const BasicQueue::Goal *goal,
- const BasicQueue::Position *position,
- BasicQueue::Output *output,
- BasicQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
};
} // namespace codelab
diff --git a/frc971/codelab/basic.q b/frc971/codelab/basic.q
deleted file mode 100644
index 58fd69e..0000000
--- a/frc971/codelab/basic.q
+++ /dev/null
@@ -1,44 +0,0 @@
-package frc971.codelab;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// The theme of this basic test is a simple intake system.
-//
-// The system will have a motor driven by the voltage returned
-// by output, and then eventually this motor, when run enough,
-// will trigger the limit_sensor. The hypothetical motor should shut
-// off in that hypothetical situation to avoid hypothetical burnout.
-queue_group BasicQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The control loop needs to intake now.
- bool intake;
- };
-
- message Position {
- // This is a potential incoming sensor value letting us know
- // if we need to be intaking.
- bool limit_sensor;
- };
-
- message Status {
- // Lets consumers of basic_queue.status know if
- // the requested intake is finished.
- bool intake_complete;
- };
-
- message Output {
- // This would be set up to drive a hypothetical motor that would
- // hope to intake something.
- double intake_voltage;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-queue_group BasicQueue basic_queue;
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index 91059f6..487a09b 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -6,9 +6,8 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/events/shm-event-loop.h"
-#include "aos/queue.h"
-#include "frc971/codelab/basic.q.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/codelab/basic_generated.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -25,12 +24,9 @@
public:
BasicSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- position_sender_(event_loop_->MakeSender<BasicQueue::Position>(
- ".frc971.codelab.basic_queue.position")),
- status_fetcher_(event_loop_->MakeFetcher<BasicQueue::Status>(
- ".frc971.codelab.basic_queue.status")),
- output_fetcher_(event_loop_->MakeFetcher<BasicQueue::Output>(
- ".frc971.codelab.basic_queue.output")) {
+ position_sender_(event_loop_->MakeSender<Position>("/codelab")),
+ status_fetcher_(event_loop_->MakeFetcher<Status>("/codelab")),
+ output_fetcher_(event_loop_->MakeFetcher<Output>("/codelab")) {
event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -45,11 +41,13 @@
// Sends a queue message with the position data.
void SendPositionMessage() {
- auto position = position_sender_.MakeMessage();
+ auto builder = position_sender_.MakeBuilder();
- position->limit_sensor = limit_sensor_;
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
- position.Send();
+ position_builder.add_limit_sensor(limit_sensor_);
+
+ builder.Send(position_builder.Finish());
}
void VerifyResults(double voltage, bool status) {
@@ -59,8 +57,8 @@
ASSERT_TRUE(output_fetcher_.get() != nullptr);
ASSERT_TRUE(status_fetcher_.get() != nullptr);
- EXPECT_EQ(output_fetcher_->intake_voltage, voltage);
- EXPECT_EQ(status_fetcher_->intake_complete, status);
+ EXPECT_EQ(output_fetcher_->intake_voltage(), voltage);
+ EXPECT_EQ(status_fetcher_->intake_complete(), status);
}
void set_limit_sensor(bool value) { limit_sensor_ = value; }
@@ -71,9 +69,9 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<BasicQueue::Position> position_sender_;
- ::aos::Fetcher<BasicQueue::Status> status_fetcher_;
- ::aos::Fetcher<BasicQueue::Output> output_fetcher_;
+ ::aos::Sender<Position> position_sender_;
+ ::aos::Fetcher<Status> status_fetcher_;
+ ::aos::Fetcher<Output> output_fetcher_;
bool limit_sensor_ = false;
@@ -83,13 +81,41 @@
class BasicControlLoopTest : public ::aos::testing::ControlLoopTest {
public:
BasicControlLoopTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ "{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.JoystickState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.RobotState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Goal\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Output\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Status\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Position\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- goal_sender_(test_event_loop_->MakeSender<BasicQueue::Goal>(
- ".frc971.codelab.basic_queue.goal")),
+ goal_sender_(test_event_loop_->MakeSender<Goal>("/codelab")),
basic_event_loop_(MakeEventLoop()),
- basic_(basic_event_loop_.get(), ".frc971.codelab.basic_queue"),
+ basic_(basic_event_loop_.get(), "/codelab"),
basic_simulation_event_loop_(MakeEventLoop()),
basic_simulation_(basic_simulation_event_loop_.get(), dt()) {
@@ -97,7 +123,7 @@
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<BasicQueue::Goal> goal_sender_;
+ ::aos::Sender<Goal> goal_sender_;
::std::unique_ptr<::aos::EventLoop> basic_event_loop_;
Basic basic_;
@@ -109,9 +135,10 @@
// Tests that when the motor has finished intaking it stops.
TEST_F(BasicControlLoopTest, IntakeLimitTransitionsToTrue) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -121,9 +148,10 @@
basic_simulation_.VerifyResults(12.0, false);
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -136,9 +164,10 @@
// and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitNotSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -151,9 +180,10 @@
// even if the limit sensor is off.
TEST_F(BasicControlLoopTest, NoIntakeLimitNotSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = false;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(false);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -166,9 +196,10 @@
// is pressed and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -180,9 +211,10 @@
// Tests that the intake is off if no intake is requested,
TEST_F(BasicControlLoopTest, NoIntakeLimitSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = false;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(false);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -205,9 +237,10 @@
basic_simulation_.set_limit_sensor(true);
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(false);
diff --git a/aos/config/10-net-eth0.rules b/frc971/config/10-net-eth0.rules
similarity index 100%
rename from aos/config/10-net-eth0.rules
rename to frc971/config/10-net-eth0.rules
diff --git a/aos/config/BUILD b/frc971/config/BUILD
similarity index 100%
rename from aos/config/BUILD
rename to frc971/config/BUILD
diff --git a/aos/config/aos.conf b/frc971/config/aos.conf
similarity index 100%
rename from aos/config/aos.conf
rename to frc971/config/aos.conf
diff --git a/aos/config/robotCommand b/frc971/config/robotCommand
similarity index 100%
rename from aos/config/robotCommand
rename to frc971/config/robotCommand
diff --git a/aos/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
similarity index 100%
rename from aos/config/setup_roborio.sh
rename to frc971/config/setup_roborio.sh
diff --git a/aos/config/setup_rt_caps.sh b/frc971/config/setup_rt_caps.sh
similarity index 100%
rename from aos/config/setup_rt_caps.sh
rename to frc971/config/setup_rt_caps.sh
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 3d6c930..17613ab 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
@@ -34,7 +34,7 @@
hdrs = ["pose.h"],
deps = [
"//aos/util:math",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -53,15 +53,16 @@
"hall_effect_tracker.h",
],
deps = [
- ":queues",
+ ":control_loops_fbs",
],
)
-queue_library(
- name = "queues",
+flatbuffer_cc_library(
+ name = "control_loops_fbs",
srcs = [
- "control_loops.q",
+ "control_loops.fbs",
],
+ compatible_with = mcu_cpus,
)
cc_test(
@@ -70,8 +71,8 @@
"position_sensor_sim_test.cc",
],
deps = [
+ ":control_loops_fbs",
":position_sensor_sim",
- ":queues",
"//aos/logging",
"//aos/testing:googletest",
],
@@ -90,8 +91,8 @@
"-lm",
],
deps = [
+ ":control_loops_fbs",
":gaussian_noise",
- ":queues",
"//aos/testing:random_seed",
],
)
@@ -120,7 +121,7 @@
restricted_to = mcu_cpus,
deps = [
"//aos/controls:polytope_uc",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -137,7 +138,7 @@
],
deps = [
"//aos/controls:polytope",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -151,7 +152,7 @@
restricted_to = mcu_cpus,
deps = [
"//aos:macros",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -163,7 +164,7 @@
deps = [
"//aos:macros",
"//aos/logging",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -178,7 +179,7 @@
"//aos:macros",
"//aos/controls:control_loop",
"//aos/logging",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -189,7 +190,7 @@
],
deps = [
":state_feedback_loop",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -199,7 +200,7 @@
"runge_kutta.h",
],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -211,7 +212,7 @@
deps = [
":runge_kutta",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -233,24 +234,13 @@
],
)
-queue_library(
- name = "profiled_subsystem_queue",
+flatbuffer_cc_library(
+ name = "profiled_subsystem_fbs",
srcs = [
- "profiled_subsystem.q",
+ "profiled_subsystem.fbs",
],
- deps = [
- ":queues",
- ],
-)
-
-queue_library(
- name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
- srcs = [
- "static_zeroing_single_dof_profiled_subsystem_test.q",
- ],
- deps = [
- ":profiled_subsystem_queue",
- ":queues",
+ includes = [
+ ":control_loops_fbs_includes",
],
)
@@ -263,7 +253,8 @@
"profiled_subsystem.h",
],
deps = [
- ":profiled_subsystem_queue",
+ ":control_loops_fbs",
+ ":profiled_subsystem_fbs",
":simple_capped_state_feedback_loop",
":state_feedback_loop",
"//aos/controls:control_loop",
@@ -278,7 +269,7 @@
"jacobian.h",
],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -290,7 +281,7 @@
deps = [
":jacobian",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -315,7 +306,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/time",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -326,7 +317,7 @@
],
visibility = ["//visibility:public"],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
"@slycot_repo//:slicot",
],
)
@@ -408,6 +399,17 @@
],
)
+flatbuffer_cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_fbs",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.fbs",
+ ],
+ includes = [
+ ":control_loops_fbs_includes",
+ ":profiled_subsystem_fbs_includes",
+ ],
+)
+
cc_test(
name = "static_zeroing_single_dof_profiled_subsystem_test",
srcs = [
@@ -417,8 +419,8 @@
":capped_test_plant",
":position_sensor_sim",
":static_zeroing_single_dof_profiled_subsystem",
+ ":static_zeroing_single_dof_profiled_subsystem_test_fbs",
":static_zeroing_single_dof_profiled_subsystem_test_plants",
- ":static_zeroing_single_dof_profiled_subsystem_test_queue",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
],
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 6c1f504..6e927b0 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -76,7 +76,7 @@
} else {
Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
#ifdef __linux__
- AOS_CHECK_GT(region_vertices.outerSize(), 0);
+ CHECK_GT(reinterpret_cast<ssize_t>(region_vertices.outerSize()), 0);
#else
assert(region_vertices.outerSize() > 0);
#endif
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.fbs
similarity index 65%
rename from frc971/control_loops/control_loops.q
rename to frc971/control_loops/control_loops.fbs
index 2359bcd..4f1e888 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.fbs
@@ -1,4 +1,4 @@
-package frc971;
+namespace frc971;
// Represents all of the data for a single indexed encoder. In other words,
// just a relative encoder with an index pulse.
@@ -6,14 +6,14 @@
// All encoder values are relative to where the encoder was at some arbitrary
// point in time. All potentiometer values are relative to some arbitrary 0
// position which varies with each robot.
-struct IndexPosition {
+table IndexPosition {
// Current position read from the encoder.
- double encoder;
+ encoder:double;
// Position from the encoder latched at the last index pulse.
- double latched_encoder;
+ latched_encoder:double;
// How many index pulses we've seen since startup. Starts at 0.
- uint32_t index_pulses;
-};
+ index_pulses:uint;
+}
// Represents all of the data for a single potentiometer and indexed encoder
// pair.
@@ -21,20 +21,20 @@
// All encoder values are relative to where the encoder was at some arbitrary
// point in time. All potentiometer values are relative to some arbitrary 0
// position which varies with each robot.
-struct PotAndIndexPosition {
+table PotAndIndexPosition {
// Current position read from the encoder.
- double encoder;
+ encoder:double;
// Current position read from the potentiometer.
- double pot;
+ pot:double;
// Position from the encoder latched at the last index pulse.
- double latched_encoder;
+ latched_encoder:double;
// Position from the potentiometer latched at the last index pulse.
- double latched_pot;
+ latched_pot:double;
// How many index pulses we've seen since startup. Starts at 0.
- uint32_t index_pulses;
-};
+ index_pulses:uint;
+}
// Represents all of the data for a single potentiometer with an absolute and
// relative encoder pair.
@@ -42,182 +42,183 @@
// The relative encoder values are relative to where the encoder was at some
// arbitrary point in time. All potentiometer values are relative to some
// arbitrary 0 position which varies with each robot.
-struct PotAndAbsolutePosition {
+table PotAndAbsolutePosition {
// Current position read from each encoder.
- double encoder;
- double absolute_encoder;
+ encoder:double;
+ absolute_encoder:double;
// Current position read from the potentiometer.
- double pot;
-};
+ pot:double;
+}
// Represents all of the data for an absolute and relative encoder pair.
// The units on all of the positions are the same.
// The relative encoder values are relative to where the encoder was at some
// arbitrary point in time.
-struct AbsolutePosition {
+table AbsolutePosition {
// Current position read from each encoder.
- double encoder;
- double absolute_encoder;
-};
+ encoder:double;
+ absolute_encoder:double;
+}
// The internal state of a zeroing estimator.
-struct EstimatorState {
+table EstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated position not using the index pulse.
- double pot_position;
-};
+ pot_position:double;
+}
// The internal state of a zeroing estimator.
-struct PotAndAbsoluteEncoderEstimatorState {
+table PotAndAbsoluteEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated position not using the index pulse.
- double pot_position;
+ pot_position:double;
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
- double absolute_position;
-};
+ absolute_position:double;
+}
// The internal state of a zeroing estimator.
-struct AbsoluteEncoderEstimatorState {
+table AbsoluteEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
- double absolute_position;
-};
+ absolute_position:double;
+}
// The internal state of a zeroing estimator.
-struct IndexEstimatorState {
+table IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint. This is just the position relative to
// where we started if we're not zeroed yet.
- double position;
+ position:double;
// The positions of the extreme index pulses we've seen.
- double min_index_position;
- double max_index_position;
+ min_index_position:double;
+ max_index_position:double;
// The number of index pulses we've seen.
- int32_t index_pulses_seen;
-};
+ index_pulses_seen:int;
+}
-struct HallEffectAndPositionEstimatorState {
+table HallEffectAndPositionEstimatorState {
// If error.
- bool error;
+ error:bool;
// If we've found a positive edge while moving backwards and is zeroed.
- bool zeroed;
+ zeroed:bool;
// Encoder angle relative to where we started.
- double encoder;
+ encoder:double;
// The positions of the extreme posedges we've seen.
// If we've gotten enough samples where the hall effect is high before can be
// certain it is not a false positive.
- bool high_long_enough;
- double offset;
-};
+ high_long_enough:bool;
+ offset:double;
+}
// A left/right pair of PotAndIndexPositions.
-struct PotAndIndexPair {
- PotAndIndexPosition left;
- PotAndIndexPosition right;
-};
+table PotAndIndexPair {
+ left:PotAndIndexPosition;
+ right:PotAndIndexPosition;
+}
// Records edges captured on a single hall effect sensor.
-struct HallEffectStruct {
- bool current;
- int32_t posedge_count;
- int32_t negedge_count;
- double posedge_value;
- double negedge_value;
-};
+table HallEffectStruct {
+ current:bool;
+ posedge_count:int;
+ negedge_count:int;
+ posedge_value:double;
+ negedge_value:double;
+}
// Records the hall effect sensor and encoder values.
-struct HallEffectAndPosition {
+table HallEffectAndPosition {
// The current hall effect state.
- bool current;
+ current:bool;
// The current encoder position.
- double encoder;
+ encoder:double;
// The number of positive and negative edges we've seen on the hall effect
// sensor.
- int32_t posedge_count;
- int32_t negedge_count;
+ posedge_count:int;
+ negedge_count:int;
// The values corresponding to the last hall effect sensor reading.
- double posedge_value;
- double negedge_value;
-};
+ posedge_value:double;
+ negedge_value:double;
+}
// Records the positions for a mechanism with edge-capturing sensors on it.
-struct HallEffectPositions {
- double current;
- double posedge;
- double negedge;
-};
+table HallEventPositions {
+ current:double;
+ posedge:double;
+ negedge:double;
+}
// Records edges captured on a single hall effect sensor.
-struct PosedgeOnlyCountedHallEffectStruct {
- bool current;
- int32_t posedge_count;
- int32_t negedge_count;
- double posedge_value;
-};
+table PosedgeOnlyCountedHallEffectStruct {
+ current:bool;
+ posedge_count:int;
+ negedge_count:int;
+ posedge_value:double;
+}
// Parameters for the motion profiles.
-struct ProfileParameters {
+table ProfileParameters {
// Maximum velocity for the profile.
- float max_velocity;
+ max_velocity:float;
// Maximum acceleration for the profile.
- float max_acceleration;
-};
+ max_acceleration:float;
+}
+
+enum ConstraintType : byte {
+ CONSTRAINT_TYPE_UNDEFINED,
+ LONGITUDINAL_ACCELERATION,
+ LATERAL_ACCELERATION,
+ VOLTAGE,
+ VELOCITY,
+}
// Definition of a constraint on a trajectory
-struct Constraint {
- // Type of constraint
- // 0: Null constraint. Ignore and all following
- // 1: longitual acceleration
- // 2: lateral acceleration
- // 3: voltage
- // 4: velocity
- uint8_t constraint_type;
- float value;
+table Constraint {
+ constraint_type:ConstraintType;
+
+ value:float;
+
// start and end distance are only checked for velocity limits.
- float start_distance;
- float end_distance;
-};
+ start_distance:float;
+ end_distance:float;
+}
// Parameters for computing a trajectory using a chain of splines and
// constraints.
-struct MultiSpline {
- // index of the spline. Zero indicates the spline should not be computed.
- int32_t spline_idx;
+table MultiSpline {
// Number of splines. The spline point arrays will be expected to have
// 6 + 5 * (n - 1) points in them. The endpoints are shared between
// neighboring splines.
- uint8_t spline_count;
- float[36] spline_x;
- float[36] spline_y;
+ spline_count:byte;
+ // Maximum of 36 spline points (7 splines).
+ spline_x:[float];
+ spline_y:[float];
- // Whether to follow the spline driving forwards or backwards.
- bool drive_spline_backwards;
-
- Constraint[6] constraints;
-};
+ // Maximum of 6 constraints;
+ constraints:[Constraint];
+}
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 5f89be1..71c7278 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,30 +1,117 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:config.bzl", "aos_config")
load("//tools:environments.bzl", "mcu_cpus")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
-cc_binary(
- name = "replay_drivetrain",
- srcs = [
- "replay_drivetrain.cc",
- ],
- deps = [
- ":drivetrain_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- "//frc971/queues:gyro",
- ],
+flatbuffer_cc_library(
+ name = "drivetrain_goal_fbs",
+ srcs = ["drivetrain_goal.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
-queue_library(
- name = "drivetrain_queue",
- srcs = [
- "drivetrain.q",
+flatbuffer_cc_library(
+ name = "drivetrain_output_fbs",
+ srcs = ["drivetrain_output.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_status_fbs",
+ srcs = ["drivetrain_status.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+genrule(
+ name = "drivetrain_goal_float_fbs_generated",
+ srcs = ["drivetrain_goal.fbs"],
+ outs = ["drivetrain_goal_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_position_float_fbs_generated",
+ srcs = ["drivetrain_position.fbs"],
+ outs = ["drivetrain_position_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_output_float_fbs_generated",
+ srcs = ["drivetrain_output.fbs"],
+ outs = ["drivetrain_output_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_status_float_fbs_generated",
+ srcs = ["drivetrain_status.fbs"],
+ outs = ["drivetrain_status_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_goal_float_fbs",
+ srcs = ["drivetrain_goal_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_output_float_fbs",
+ srcs = ["drivetrain_output_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_float_fbs",
+ srcs = ["drivetrain_position_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_status_float_fbs",
+ srcs = ["drivetrain_status_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+aos_config(
+ name = "config",
+ src = "drivetrain_config.json",
+ flatbuffers = [
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
+ ":drivetrain_position_fbs",
+ ":localizer_fbs",
+ "//frc971/queues:gyro",
+ "//frc971/wpilib:imu_fbs",
],
+ visibility = ["//visibility:public"],
deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ "//aos/robot_state:config",
],
)
@@ -49,7 +136,7 @@
"//aos/util:math",
"//frc971/control_loops:c2d",
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -66,11 +153,10 @@
],
)
-queue_library(
- name = "localizer_queue",
- srcs = [
- "localizer.q",
- ],
+flatbuffer_cc_library(
+ name = "localizer_fbs",
+ srcs = ["localizer.fbs"],
+ gen_reflections = 1,
)
cc_library(
@@ -102,11 +188,14 @@
deps = [
":distance_spline",
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":spline",
":trajectory",
"//aos:init",
"//aos/util:math",
+ "//frc971/control_loops:control_loops_fbs",
],
)
@@ -120,14 +209,19 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":localizer",
"//aos:math",
"//aos/util:math",
"//frc971/control_loops:c2d",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:pose",
- "//third_party/eigen",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -158,19 +252,20 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":gear",
":localizer",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/util:log_interval",
"//aos/util:trapezoid_profile",
"//frc971:shifter_hall_effect",
"//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
],
)
@@ -185,15 +280,17 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":gear",
"//aos:math",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/util:log_interval",
"//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
],
)
@@ -213,20 +310,24 @@
cc_library(
name = "polydrivetrain_uc",
srcs = [
- "drivetrain_uc.q.cc",
"polydrivetrain.cc",
],
hdrs = [
- "drivetrain_uc.q.h",
"polydrivetrain.h",
],
+ copts = ["-Wno-type-limits"],
restricted_to = mcu_cpus,
deps = [
":drivetrain_config_uc",
+ ":drivetrain_goal_float_fbs",
+ ":drivetrain_output_float_fbs",
+ ":drivetrain_position_float_fbs",
+ ":drivetrain_status_float_fbs",
":gear",
"//aos:math",
"//aos/controls:polytope_uc",
"//frc971/control_loops:coerce_goal_uc",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop_uc",
],
)
@@ -267,21 +368,22 @@
],
deps = [
":down_estimator",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":gear",
":line_follow_drivetrain",
":localizer",
- ":localizer_queue",
+ ":localizer_fbs",
":polydrivetrain",
":splinedrivetrain",
":ssdrivetrain",
"//aos/controls:control_loop",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro",
- "//frc971/wpilib:imu_queue",
+ "//frc971/wpilib:imu_fbs",
],
)
@@ -292,10 +394,14 @@
hdrs = ["drivetrain_test_lib.h"],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":trajectory",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/testing:googletest",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro",
"//y2016:constants",
@@ -308,6 +414,7 @@
srcs = [
"drivetrain_lib_test.cc",
],
+ data = ["config.json"],
defines =
cpu_select({
"amd64": [
@@ -319,10 +426,12 @@
deps = [
":drivetrain_config",
":drivetrain_lib",
- ":localizer_queue",
- ":drivetrain_queue",
+ ":localizer_fbs",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_status_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_output_fbs",
":drivetrain_test_lib",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/queues:gyro",
@@ -399,7 +508,7 @@
hdrs = ["spline.h"],
deps = [
"//frc971/control_loops:binomial",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -413,8 +522,8 @@
":trajectory",
"//aos/logging:implementations",
"//aos/network:team_number",
- "//third_party/eigen",
"//y2019/control_loops/drivetrain:drivetrain_base",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -440,7 +549,7 @@
":spline",
"//aos/logging",
"//frc971/control_loops:fixed_quadrature",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -477,13 +586,12 @@
deps = [
":distance_spline",
":drivetrain_config",
- "//aos/logging:matrix_logging",
"//frc971/control_loops:c2d",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:hybrid_state_feedback_loop",
"//frc971/control_loops:runge_kutta",
"//frc971/control_loops:state_feedback_loop",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -497,12 +605,11 @@
":distance_spline",
":trajectory",
"//aos/logging:implementations",
- "//aos/logging:matrix_logging",
"//aos/network:team_number",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"//y2019/control_loops/drivetrain:drivetrain_base",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4685ed6..78cb378 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -7,18 +7,18 @@
#include "Eigen/Dense"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
-
#include "frc971/control_loops/drivetrain/down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/runge_kutta.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/shifter_hall_effect.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/wpilib/imu_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -31,16 +31,16 @@
::aos::EventLoop *event_loop,
LocalizerInterface *localizer,
const ::std::string &name)
- : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
- event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
dt_config_(dt_config),
- localizer_control_fetcher_(event_loop->MakeFetcher<LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ localizer_control_fetcher_(
+ event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValues>(".frc971.imu_values")),
+ event_loop->MakeFetcher<::frc971::IMUValues>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")),
+ "/drivetrain")),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -89,10 +89,9 @@
}
void DrivetrainLoop::RunIteration(
- const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
- ::frc971::control_loops::DrivetrainQueue::Output *output,
- ::frc971::control_loops::DrivetrainQueue::Status *status) {
+ const drivetrain::Goal *goal, const drivetrain::Position *position,
+ aos::Sender<drivetrain::Output>::Builder *output,
+ aos::Sender<drivetrain::Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
@@ -118,9 +117,9 @@
}
break;
case ShifterType::HALL_EFFECT_SHIFTER:
- left_gear_ = ComputeGear(position->left_shifter_position,
+ left_gear_ = ComputeGear(position->left_shifter_position(),
dt_config_.left_drive, left_high_requested_);
- right_gear_ = ComputeGear(position->right_shifter_position,
+ right_gear_ = ComputeGear(position->right_shifter_position(),
dt_config_.right_drive, right_high_requested_);
break;
case ShifterType::NO_SHIFTER:
@@ -129,35 +128,39 @@
kf_.set_index(ControllerIndexFromGears());
+ flatbuffers::Offset<GearLogging> gear_logging_offset;
// Set the gear-logging parts of the status
if (status) {
- status->gear_logging.left_state = static_cast<uint32_t>(left_gear_);
- status->gear_logging.right_state = static_cast<uint32_t>(right_gear_);
- status->gear_logging.left_loop_high = MaybeHigh(left_gear_);
- status->gear_logging.right_loop_high = MaybeHigh(right_gear_);
- status->gear_logging.controller_index = kf_.index();
+ GearLogging::Builder gear_logging_builder =
+ status->MakeBuilder<GearLogging>();
+ gear_logging_builder.add_left_state(static_cast<uint32_t>(left_gear_));
+ gear_logging_builder.add_right_state(static_cast<uint32_t>(right_gear_));
+ gear_logging_builder.add_left_loop_high(MaybeHigh(left_gear_));
+ gear_logging_builder.add_right_loop_high(MaybeHigh(right_gear_));
+ gear_logging_builder.add_controller_index(kf_.index());
+ gear_logging_offset = gear_logging_builder.Finish();
}
const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
if (is_latest_imu_values) {
- const double rate = -imu_values_fetcher_->gyro_y;
+ const double rate = -imu_values_fetcher_->gyro_y();
const double accel_squared =
- ::std::pow(imu_values_fetcher_->accelerometer_x, 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_y, 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_z, 2.0);
- const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x,
- imu_values_fetcher_->accelerometer_z) +
+ ::std::pow(imu_values_fetcher_->accelerometer_x(), 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_y(), 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_z(), 2.0);
+ const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x(),
+ imu_values_fetcher_->accelerometer_z()) +
0.008;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
- last_accel_ = -imu_values_fetcher_->accelerometer_x;
+ last_accel_ = -imu_values_fetcher_->accelerometer_x();
break;
case IMUType::IMU_FLIPPED_X:
- last_accel_ = imu_values_fetcher_->accelerometer_x;
+ last_accel_ = imu_values_fetcher_->accelerometer_x();
break;
case IMUType::IMU_Y:
- last_accel_ = -imu_values_fetcher_->accelerometer_y;
+ last_accel_ = -imu_values_fetcher_->accelerometer_y();
break;
}
@@ -185,43 +188,37 @@
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_x;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_x();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Y_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_y;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_y();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Z_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_z;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_z();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
+ last_gyro_rate_ = -imu_values_fetcher_->gyro_z();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
- last_gyro_rate_ = gyro_reading_fetcher_->velocity;
+ last_gyro_rate_ = gyro_reading_fetcher_->velocity();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
- last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
+ last_gyro_rate_ = -gyro_reading_fetcher_->velocity();
last_gyro_time_ = monotonic_now;
}
break;
@@ -236,7 +233,7 @@
{
Eigen::Matrix<double, 4, 1> Y;
- Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
+ Y << position->left_encoder(), position->right_encoder(), last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
// If we get a new message setting the absolute position, then reset the
@@ -244,33 +241,36 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ VLOG(1) << "localizer_control "
+ << aos::FlatbufferToJson(localizer_control_fetcher_.get());
localizer_->ResetPosition(
- monotonic_now, localizer_control_fetcher_->x,
- localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
- localizer_control_fetcher_->theta_uncertainty,
- !localizer_control_fetcher_->keep_current_theta);
+ monotonic_now, localizer_control_fetcher_->x(),
+ localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
+ localizer_control_fetcher_->theta_uncertainty(),
+ !localizer_control_fetcher_->keep_current_theta());
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
- monotonic_now, position->left_encoder,
- position->right_encoder, last_gyro_rate_, last_accel_);
+ monotonic_now, position->left_encoder(),
+ position->right_encoder(), last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
- int controller_type = 0;
+ ControllerType controller_type = ControllerType_POLYDRIVE;
if (goal) {
- controller_type = goal->controller_type;
+ controller_type = goal->controller_type();
- dt_closedloop_.SetGoal(*goal);
- dt_openloop_.SetGoal(*goal);
- dt_spline_.SetGoal(*goal);
- dt_line_follow_.SetGoal(monotonic_now, *goal);
+ dt_closedloop_.SetGoal(goal);
+ dt_openloop_.SetGoal(goal->wheel(), goal->throttle(), goal->quickturn(),
+ goal->highgear());
+ dt_spline_.SetGoal(goal);
+ dt_line_follow_.SetGoal(monotonic_now, goal);
}
- dt_openloop_.Update(robot_state().voltage_battery);
+ dt_openloop_.Update(robot_state().voltage_battery());
- dt_closedloop_.Update(output != NULL && controller_type == 1);
+ dt_closedloop_.Update(output != nullptr &&
+ controller_type == ControllerType_MOTION_PROFILE);
const Eigen::Matrix<double, 5, 1> trajectory_state =
(Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
@@ -278,25 +278,30 @@
localizer_->right_velocity())
.finished();
- dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+ dt_spline_.Update(
+ output != nullptr && controller_type == ControllerType_SPLINE_FOLLOWER,
+ trajectory_state);
dt_line_follow_.Update(monotonic_now, trajectory_state);
+ OutputT output_struct;
+
switch (controller_type) {
- case 0:
- dt_openloop_.SetOutput(output);
+ case ControllerType_POLYDRIVE:
+ dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 1:
- dt_closedloop_.SetOutput(output);
+ case ControllerType_MOTION_PROFILE:
+ dt_closedloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 2:
- dt_spline_.SetOutput(output);
+ case ControllerType_SPLINE_FOLLOWER:
+ dt_spline_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 3:
- if (!dt_line_follow_.SetOutput(output)) {
+ case ControllerType_LINE_FOLLOWER:
+ if (!dt_line_follow_.SetOutput(output != nullptr ? &output_struct
+ : nullptr)) {
// If the line follow drivetrain was unable to execute (generally due to
// not having a target), execute the regular teleop drivetrain.
- dt_openloop_.SetOutput(output);
+ dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
}
break;
}
@@ -305,8 +310,6 @@
// set the output status of the control loop state
if (status) {
- status->robot_speed = (kf_.X_hat(1) + kf_.X_hat(3)) / 2.0;
-
Eigen::Matrix<double, 2, 1> linear =
dt_config_.LeftRightToLinear(kf_.X_hat());
Eigen::Matrix<double, 2, 1> angular =
@@ -317,42 +320,60 @@
Eigen::Matrix<double, 4, 1> gyro_left_right =
dt_config_.AngularLinearToLeftRight(linear, angular);
- status->estimated_left_position = gyro_left_right(0, 0);
- status->estimated_right_position = gyro_left_right(2, 0);
+ const flatbuffers::Offset<CIMLogging> cim_logging_offset =
+ dt_openloop_.PopulateStatus(status->fbb());
- status->estimated_left_velocity = gyro_left_right(1, 0);
- status->estimated_right_velocity = gyro_left_right(3, 0);
- status->output_was_capped = dt_closedloop_.output_was_capped();
- status->uncapped_left_voltage = kf_.U_uncapped(0, 0);
- status->uncapped_right_voltage = kf_.U_uncapped(1, 0);
+ flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
+ dt_line_follow_.PopulateStatus(status);
+ flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
+ dt_spline_.MakeTrajectoryLogging(status);
- status->left_voltage_error = kf_.X_hat(4);
- status->right_voltage_error = kf_.X_hat(5);
- status->estimated_angular_velocity_error = kf_.X_hat(6);
- status->estimated_heading = localizer_->theta();
+ StatusBuilder builder = status->MakeBuilder<Status>();
- status->x = localizer_->x();
- status->y = localizer_->y();
- status->theta = ::aos::math::NormalizeAngle(localizer_->theta());
+ dt_closedloop_.PopulateStatus(&builder);
- status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
+ builder.add_estimated_left_position(gyro_left_right(0, 0));
+ builder.add_estimated_right_position(gyro_left_right(2, 0));
- dt_openloop_.PopulateStatus(status);
- dt_closedloop_.PopulateStatus(status);
- dt_spline_.PopulateStatus(status);
- dt_line_follow_.PopulateStatus(status);
+ builder.add_estimated_left_velocity(gyro_left_right(1, 0));
+ builder.add_estimated_right_velocity(gyro_left_right(3, 0));
+
+ if (dt_spline_.enable()) {
+ dt_spline_.PopulateStatus(&builder);
+ } else {
+ builder.add_robot_speed((kf_.X_hat(1) + kf_.X_hat(3)) / 2.0);
+ builder.add_output_was_capped(dt_closedloop_.output_was_capped());
+ builder.add_uncapped_left_voltage(kf_.U_uncapped(0, 0));
+ builder.add_uncapped_right_voltage(kf_.U_uncapped(1, 0));
+ }
+
+ builder.add_left_voltage_error(kf_.X_hat(4));
+ builder.add_right_voltage_error(kf_.X_hat(5));
+ builder.add_estimated_angular_velocity_error(kf_.X_hat(6));
+ builder.add_estimated_heading(localizer_->theta());
+
+ builder.add_x(localizer_->x());
+ builder.add_y(localizer_->y());
+ builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
+
+ builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
+ builder.add_cim_logging(cim_logging_offset);
+ builder.add_gear_logging(gear_logging_offset);
+ builder.add_line_follow_logging(line_follow_logging_offset);
+ builder.add_trajectory_logging(trajectory_logging_offset);
+ status->Send(builder.Finish());
}
double left_voltage = 0.0;
double right_voltage = 0.0;
if (output) {
- left_voltage = output->left_voltage;
- right_voltage = output->right_voltage;
- left_high_requested_ = output->left_high;
- right_high_requested_ = output->right_high;
+ left_voltage = output_struct.left_voltage;
+ right_voltage = output_struct.right_voltage;
+ left_high_requested_ = output_struct.left_high;
+ right_high_requested_ = output_struct.right_high;
}
- const double scalar = robot_state().voltage_battery / 12.0;
+ const double scalar = robot_state().voltage_battery() / 12.0;
left_voltage *= scalar;
right_voltage *= scalar;
@@ -375,14 +396,20 @@
last_state_ = kf_.X_hat();
kf_.UpdateObserver(U, dt_config_.dt);
+
+ if (output) {
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
}
-void DrivetrainLoop::Zero(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
- output->left_voltage = 0;
- output->right_voltage = 0;
- output->left_high = dt_config_.default_high_gear;
- output->right_high = dt_config_.default_high_gear;
+flatbuffers::Offset<Output> DrivetrainLoop::Zero(
+ aos::Sender<Output>::Builder *output) {
+ Output::Builder builder = output->MakeBuilder<Output>();
+ builder.add_left_voltage(0);
+ builder.add_right_voltage(0);
+ builder.add_left_high(dt_config_.default_high_gear);
+ builder.add_right_high(dt_config_.default_high_gear);
+ return builder.Finish();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 33ff770..9f6a34c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -6,43 +6,49 @@
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
#include "aos/util/log_interval.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/queues/gyro.q.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-class DrivetrainLoop : public aos::controls::ControlLoop<
- ::frc971::control_loops::DrivetrainQueue> {
+class DrivetrainLoop
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
- explicit DrivetrainLoop(
- const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
- LocalizerInterface *localizer,
- const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
+ explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
+ const ::std::string &name = "/drivetrain");
int ControllerIndexFromGears();
protected:
// Executes one cycle of the control loop.
void RunIteration(
- const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
- ::frc971::control_loops::DrivetrainQueue::Output *output,
- ::frc971::control_loops::DrivetrainQueue::Status *status) override;
+ const ::frc971::control_loops::drivetrain::Goal *goal,
+ const ::frc971::control_loops::drivetrain::Position *position,
+ aos::Sender<::frc971::control_loops::drivetrain::Output>::Builder *output,
+ aos::Sender<::frc971::control_loops::drivetrain::Status>::Builder *status)
+ override;
- void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
+ flatbuffers::Offset<drivetrain::Output> Zero(
+ aos::Sender<drivetrain::Output>::Builder *builder) override;
double last_gyro_rate_ = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
deleted file mode 100644
index c7a307e..0000000
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ /dev/null
@@ -1,224 +0,0 @@
-package frc971.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// For logging information about what the code is doing with the shifters.
-struct GearLogging {
- // Which controller is being used.
- int8_t controller_index;
-
- // Whether each loop for the drivetrain sides is the high-gear one.
- bool left_loop_high;
- bool right_loop_high;
-
- // The states of each drivetrain shifter.
- int8_t left_state;
- int8_t right_state;
-};
-
-// For logging information about the state of the shifters.
-struct CIMLogging {
- // Whether the code thinks each drivetrain side is currently in gear.
- bool left_in_gear;
- bool right_in_gear;
-
- // The angular velocities (in rad/s, positive forward) the code thinks motors
- // on each side of the drivetrain are moving at.
- double left_motor_speed;
- double right_motor_speed;
-
- // The velocity estimates for each drivetrain side of the robot (in m/s,
- // positive forward) that can be used for shifting.
- double left_velocity;
- double right_velocity;
-};
-
-// For logging information about the state of the trajectory planning.
-struct TrajectoryLogging {
- // state of planning the trajectory.
- // 0: not currently planning
- // 1: received a multispline to plan
- // 2: Built the spline and planning.
- // 3: Finished the plan and ready to excecute.
- int8_t planning_state;
-
- // State of the spline execution.
- bool is_executing;
- // Whether we have finished the spline specified by current_spline_idx.
- bool is_executed;
-
- // The handle of the goal spline. 0 means stop requested.
- int32_t goal_spline_handle;
- // Handle of the executing spline. -1 means none requested. If there was no
- // spline executing when a spline finished optimizing, it will become the
- // current spline even if we aren't ready to start yet.
- int32_t current_spline_idx;
- // Handle of the spline that is being optimized and staged.
- int32_t planning_spline_idx;
-
- // Expected position and velocity on the spline
- float x;
- float y;
- float theta;
- float left_velocity;
- float right_velocity;
- float distance_remaining;
-};
-
-// For logging state of the line follower.
-struct LineFollowLogging {
- // Whether we are currently freezing target choice.
- bool frozen;
- // Whether we currently have a target.
- bool have_target;
- // Absolute position of the current goal.
- float x;
- float y;
- float theta;
- // Current lateral offset from line pointing straight out of the target.
- float offset;
- // Current distance from the plane of the target, in meters.
- float distance_to_target;
- // Current goal heading.
- float goal_theta;
- // Current relative heading.
- float rel_theta;
-};
-
-// Published on ".frc971.control_loops.drivetrain_queue"
-queue_group DrivetrainQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Position of the steering wheel (positive = turning left when going
- // forwards).
- float wheel;
- float wheel_velocity;
- float wheel_torque;
-
- // Position of the throttle (positive forwards).
- float throttle;
- float throttle_velocity;
- float throttle_torque;
-
- // True to shift into high, false to shift into low.
- bool highgear;
-
- // True to activate quickturn.
- bool quickturn;
-
- // Type of controller in charge of the drivetrain.
- // 0: polydrive
- // 1: motion profiled position drive (statespace)
- // 2: spline follower
- // 3: line follower (for guiding into a target)
- uint8_t controller_type;
-
- // Position goals for each drivetrain side (in meters) when the
- // closed-loop controller is active.
- double left_goal;
- double right_goal;
-
- float max_ss_voltage;
-
- // Motion profile parameters.
- // The control loop will profile if these are all non-zero.
- .frc971.ProfileParameters linear;
- .frc971.ProfileParameters angular;
-
- // Parameters for a spline to follow. This just contains info on a spline to
- // compute. Each time this is sent, spline drivetrain will compute a new
- // spline.
- .frc971.MultiSpline spline;
-
- // Which spline to follow.
- int32_t spline_handle;
- };
-
- message Position {
- // Relative position of each drivetrain side (in meters).
- double left_encoder;
- double right_encoder;
-
- // The speed in m/s of each drivetrain side from the most recent encoder
- // pulse, or 0 if there was no edge within the last 5ms.
- double left_speed;
- double right_speed;
-
- // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
- // is towards low gear.
- double left_shifter_position;
- double right_shifter_position;
-
- // Raw analog voltages of each shifter hall effect for logging purposes.
- double low_left_hall;
- double high_left_hall;
- double low_right_hall;
- double high_right_hall;
- };
-
- message Output {
- // Voltage to send to motor(s) on either side of the drivetrain.
- double left_voltage;
- double right_voltage;
-
- // Whether to set each shifter piston to high gear.
- bool left_high;
- bool right_high;
- };
-
- message Status {
- // Estimated speed of the center of the robot in m/s (positive forwards).
- double robot_speed;
-
- // Estimated relative position of each drivetrain side (in meters).
- double estimated_left_position;
- double estimated_right_position;
-
- // Estimated velocity of each drivetrain side (in m/s).
- double estimated_left_velocity;
- double estimated_right_velocity;
-
- // The voltage we wanted to send to each drivetrain side last cycle.
- double uncapped_left_voltage;
- double uncapped_right_voltage;
-
- // The voltage error for the left and right sides.
- double left_voltage_error;
- double right_voltage_error;
-
- // The profiled goal states.
- double profiled_left_position_goal;
- double profiled_right_position_goal;
- double profiled_left_velocity_goal;
- double profiled_right_velocity_goal;
-
- // The KF offset
- double estimated_angular_velocity_error;
- // The KF estimated heading.
- double estimated_heading;
-
- // xytheta of the robot.
- double x;
- double y;
- double theta;
-
- // True if the output voltage was capped last cycle.
- bool output_was_capped;
-
- // The angle of the robot relative to the ground.
- double ground_angle;
-
- // Information about shifting logic and curent gear, for logging purposes
- GearLogging gear_logging;
- CIMLogging cim_logging;
- TrajectoryLogging trajectory_logging;
- LineFollowLogging line_follow_logging;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
new file mode 100644
index 0000000..dc828bb
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -0,0 +1,43 @@
+{
+ "channels":
+ [
+ {
+ "name": "/drivetrain",
+ "type": "frc971.IMUValues",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.sensors.GyroReading",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.LocalizerControl",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../../../aos/robot_state/robot_state_config.json"
+ ]
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_goal.fbs b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
new file mode 100644
index 0000000..8a67849
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
@@ -0,0 +1,65 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+enum ControllerType : byte {
+ POLYDRIVE,
+ MOTION_PROFILE,
+ SPLINE_FOLLOWER,
+ LINE_FOLLOWER,
+}
+
+table SplineGoal {
+ // index of the spline. Zero indicates the spline should not be computed.
+ spline_idx:int;
+
+ // Acutal spline.
+ spline:frc971.MultiSpline;
+
+ // Whether to follow the spline driving forwards or backwards.
+ drive_spline_backwards:bool;
+}
+
+table Goal {
+ // Position of the steering wheel (positive = turning left when going
+ // forwards).
+ wheel:float;
+ wheel_velocity:float;
+ wheel_torque:float;
+
+ // Position of the throttle (positive forwards).
+ throttle:float;
+ throttle_velocity:float;
+ throttle_torque:float;
+
+ // True to shift into high, false to shift into low.
+ highgear:bool;
+
+ // True to activate quickturn.
+ quickturn:bool;
+
+ // Type of controller in charge of the drivetrain.
+ controller_type:ControllerType;
+
+ // Position goals for each drivetrain side (in meters) when the
+ // closed-loop controller is active.
+ left_goal:double;
+ right_goal:double;
+
+ max_ss_voltage:float;
+
+ // Motion profile parameters.
+ // The control loop will profile if these are all non-zero.
+ linear:ProfileParameters;
+ angular:ProfileParameters;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ spline:SplineGoal;
+
+ // Which spline to follow.
+ spline_handle:int;
+}
+
+root_type Goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e4de9bb..a46defd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,19 +5,21 @@
#include "aos/controls/control_loop_test.h"
#include "aos/controls/polytope.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -30,27 +32,21 @@
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
DrivetrainTest()
- : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig(
+ "frc971/control_loops/drivetrain/config.json"),
+ GetTestDrivetrainConfig().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")),
drivetrain_status_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/drivetrain")),
drivetrain_output_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/drivetrain")),
localizer_control_sender_(
- test_event_loop_->MakeSender<LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTestDrivetrainConfig()),
localizer_(drivetrain_event_loop_.get(), dt_config_),
@@ -65,9 +61,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);
}
@@ -79,8 +75,10 @@
void VerifyNearSplineGoal() {
drivetrain_status_fetcher_.Fetch();
- const double expected_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double expected_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double expected_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double expected_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), expected_x, 2e-2);
EXPECT_NEAR(actual(1), expected_y, 2e-2);
@@ -92,7 +90,8 @@
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
RunFor(dt());
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+ } while (CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->planning_state() !=
(int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
}
@@ -100,17 +99,18 @@
do {
RunFor(dt());
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
+ } while (!CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->is_executed());
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
::aos::Sender<LocalizerControl> localizer_control_sender_;
@@ -128,11 +128,12 @@
TEST_F(DrivetrainTest, ConvergesCorrectly) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
@@ -143,11 +144,12 @@
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -158,11 +160,12 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
{
- 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 goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
if (i > 20 && i < 200) {
@@ -187,20 +190,21 @@
TEST_F(DrivetrainTest, DriveStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 4.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(4.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -11);
}
VerifyNearGoal();
}
@@ -210,17 +214,18 @@
TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 3.9;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(3.9);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -11);
}
VerifyNearGoal();
}
@@ -254,26 +259,39 @@
TEST_F(DrivetrainTest, ProfileStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 3.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 3.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -282,26 +300,39 @@
TEST_F(DrivetrainTest, ProfileTurn) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 3.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 3.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- -drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ -drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -310,15 +341,28 @@
TEST_F(DrivetrainTest, SaturatedTurnDrive) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 5.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 6.0;
- message->linear.max_acceleration = 4.0;
- message->angular.max_velocity = 2.0;
- message->angular.max_acceleration = 4.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(6.0);
+ linear_builder.add_max_acceleration(4.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(2.0);
+ angular_builder.add_max_acceleration(4.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(5.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
@@ -333,61 +377,77 @@
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = 1.0;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(1.0);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = -0.3;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(-0.3);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = 0.0;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 5.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 2.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 2.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(2.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(2.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(5.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
const auto end_time = monotonic_now() + chrono::seconds(4);
while (monotonic_now() < end_time) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -396,22 +456,44 @@
TEST_F(DrivetrainTest, SplineSimple) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
// Send the start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -423,22 +505,43 @@
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.drive_spline_backwards = true;
- message->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
- message->spline.spline_y = {{0.0, 0.0, -0.25, -0.75, -1.0, -1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, -0.25, -0.5, -0.5, -0.75, -1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, -0.25, -0.75, -1.0, -1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(true);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -456,7 +559,7 @@
drivetrain_status_fetcher_.Fetch();
auto actual = drivetrain_plant_.state();
const double expected_theta =
- drivetrain_status_fetcher_->trajectory_logging.theta;
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
@@ -467,14 +570,35 @@
TEST_F(DrivetrainTest, SplineSingleGoal) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -486,100 +610,179 @@
TEST_F(DrivetrainTest, SplineStop) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double goal_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double goal_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Now stop.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped.
drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+ goal_x, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
+ goal_y, 1e-9);
}
// Tests that a spline can't be restarted.
TEST_F(DrivetrainTest, SplineRestart) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double goal_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double goal_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Send a stop goal.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
// Send a restart.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped and restarted.
drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+ goal_x, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
+ goal_y, 1e-9);
}
// Tests that simple spline converges when it doesn't start where it thinks.
TEST_F(DrivetrainTest, SplineOffset) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.2, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.2, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.2, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -592,21 +795,43 @@
TEST_F(DrivetrainTest, SplineSideOffset) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.5, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.5, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -618,23 +843,45 @@
TEST_F(DrivetrainTest, MultiSpline) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 2;
- message->spline.spline_x = {
- {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {
- {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>(
+ {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>(
+ {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(2);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -646,21 +893,43 @@
TEST_F(DrivetrainTest, SequentialSplines) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -670,22 +939,44 @@
// Second spline.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
// And then start it.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -696,14 +987,35 @@
TEST_F(DrivetrainTest, SplineStopFirst) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -711,23 +1023,45 @@
// Stop goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
// Second spline goal.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -740,15 +1074,35 @@
TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- // Don't start running the splane.
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -756,23 +1110,45 @@
// Plan another spline, but don't start it yet:
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
// Now execute it.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
@@ -784,35 +1160,78 @@
TEST_F(DrivetrainTest, ParallelSplines) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
// Second spline goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->spline_handle = 1;
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
// Second start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(4000));
@@ -823,20 +1242,42 @@
TEST_F(DrivetrainTest, OnlyPlanSpline) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
for (int i = 0; i < 100; ++i) {
RunFor(dt());
drivetrain_status_fetcher_.Fetch();
- EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+ EXPECT_EQ(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->planning_state(),
3);
::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
}
@@ -847,23 +1288,45 @@
TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
// Start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
@@ -877,21 +1340,29 @@
localizer_.target_selector()->set_has_target(true);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
{
- 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 goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+ goal_builder.add_throttle(0.5);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
drivetrain_status_fetcher_.Fetch();
- EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.frozen);
- EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.have_target);
- EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.x);
- EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.y);
- EXPECT_FLOAT_EQ(M_PI_4,
- drivetrain_status_fetcher_->line_follow_logging.theta);
+ EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->frozen());
+ EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->have_target());
+ EXPECT_EQ(
+ 1.0,
+ CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->x());
+ EXPECT_EQ(
+ 1.0,
+ CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->y());
+ EXPECT_FLOAT_EQ(
+ M_PI_4, CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->theta());
// Should have run off the end of the target, running along the y=x line.
EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
@@ -906,10 +1377,11 @@
localizer_.target_selector()->set_has_target(false);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
{
- 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 goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+ goal_builder.add_throttle(0.5);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -926,11 +1398,13 @@
EXPECT_EQ(0.0, localizer_.y());
EXPECT_EQ(0.0, localizer_.theta());
{
- auto message = localizer_control_sender_.MakeMessage();
- message->x = 9.0;
- message->y = 7.0;
- message->theta = 1.0;
- ASSERT_TRUE(message.Send());
+ auto builder = localizer_control_sender_.MakeBuilder();
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(9.0);
+ localizer_control_builder.add_y(7.0);
+ localizer_control_builder.add_theta(1.0);
+ ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
}
RunFor(dt());
diff --git a/frc971/control_loops/drivetrain/drivetrain_output.fbs b/frc971/control_loops/drivetrain/drivetrain_output.fbs
new file mode 100644
index 0000000..da8c889
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_output.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+table Output {
+ // Voltage to send to motor(s) on either side of the drivetrain.
+ left_voltage:double;
+ right_voltage:double;
+
+ // Whether to set each shifter piston to high gear.
+ left_high:bool;
+ right_high:bool;
+}
+
+root_type Output;
diff --git a/frc971/control_loops/drivetrain/drivetrain_position.fbs b/frc971/control_loops/drivetrain/drivetrain_position.fbs
new file mode 100644
index 0000000..900c036
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_position.fbs
@@ -0,0 +1,25 @@
+namespace frc971.control_loops.drivetrain;
+
+table Position {
+ // Relative position of each drivetrain side (in meters).
+ left_encoder:double;
+ right_encoder:double;
+
+ // The speed in m/s of each drivetrain side from the most recent encoder
+ // pulse, or 0 if there was no edge within the last 5ms.
+ left_speed:double;
+ right_speed:double;
+
+ // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
+ // is towards low gear.
+ left_shifter_position:double;
+ right_shifter_position:double;
+
+ // Raw analog voltages of each shifter hall effect for logging purposes.
+ low_left_hall:double;
+ high_left_hall:double;
+ low_right_hall:double;
+ high_right_hall:double;
+}
+
+root_type Position;
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
new file mode 100644
index 0000000..67134f5
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -0,0 +1,140 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+// For logging information about what the code is doing with the shifters.
+table GearLogging {
+ // Which controller is being used.
+ controller_index:byte;
+
+ // Whether each loop for the drivetrain sides is the high-gear one.
+ left_loop_high:bool;
+ right_loop_high:bool;
+
+ // The states of each drivetrain shifter.
+ left_state:byte;
+ right_state:byte;
+}
+
+// For logging information about the state of the shifters.
+table CIMLogging {
+ // Whether the code thinks each drivetrain side is currently in gear.
+ left_in_gear:bool;
+ right_in_gear:bool;
+
+ // The angular velocities (in rad/s, positive forward) the code thinks motors
+ // on each side of the drivetrain are moving at.
+ left_motor_speed:double;
+ right_motor_speed:double;
+
+ // The velocity estimates for each drivetrain side of the robot (in m/s,
+ // positive forward) that can be used for shifting.
+ left_velocity:double;
+ right_velocity:double;
+}
+
+enum PlanningState : byte {
+ NO_PLAN,
+ BUILDING_TRAJECTORY,
+ PLANNING_TRAJECTORY,
+ PLANNED,
+}
+
+// For logging information about the state of the trajectory planning.
+table TrajectoryLogging {
+ // state of planning the trajectory.
+ planning_state:PlanningState;
+
+ // State of the spline execution.
+ is_executing:bool;
+ // Whether we have finished the spline specified by current_spline_idx.
+ is_executed:bool;
+
+ // The handle of the goal spline. 0 means stop requested.
+ goal_spline_handle:int;
+ // Handle of the executing spline. -1 means none requested. If there was no
+ // spline executing when a spline finished optimizing, it will become the
+ // current spline even if we aren't ready to start yet.
+ current_spline_idx:int;
+ // Handle of the spline that is being optimized and staged.
+ planning_spline_idx:int;
+
+ // Expected position and velocity on the spline
+ x:float;
+ y:float;
+ theta:float;
+ left_velocity:float;
+ right_velocity:float;
+ distance_remaining:float;
+}
+
+// For logging state of the line follower.
+table LineFollowLogging {
+ // Whether we are currently freezing target choice.
+ frozen:bool;
+ // Whether we currently have a target.
+ have_target:bool;
+ // Absolute position of the current goal.
+ x:float;
+ y:float;
+ theta:float;
+ // Current lateral offset from line pointing straight out of the target.
+ offset:float;
+ // Current distance from the plane of the target, in meters.
+ distance_to_target:float;
+ // Current goal heading.
+ goal_theta:float;
+ // Current relative heading.
+ rel_theta:float;
+}
+
+table Status {
+ // Estimated speed of the center of the robot in m/s (positive forwards).
+ robot_speed:double;
+
+ // Estimated relative position of each drivetrain side (in meters).
+ estimated_left_position:double;
+ estimated_right_position:double;
+
+ // Estimated velocity of each drivetrain side (in m/s).
+ estimated_left_velocity:double;
+ estimated_right_velocity:double;
+
+ // The voltage we wanted to send to each drivetrain side last cycle.
+ uncapped_left_voltage:double;
+ uncapped_right_voltage:double;
+
+ // The voltage error for the left and right sides.
+ left_voltage_error:double;
+ right_voltage_error:double;
+
+ // The profiled goal states.
+ profiled_left_position_goal:double;
+ profiled_right_position_goal:double;
+ profiled_left_velocity_goal:double;
+ profiled_right_velocity_goal:double;
+
+ // The KF offset
+ estimated_angular_velocity_error:double;
+ // The KF estimated heading.
+ estimated_heading:double;
+
+ // xytheta of the robot.
+ x:double;
+ y:double;
+ theta:double;
+
+ // True if the output voltage was capped last cycle.
+ output_was_capped:bool;
+
+ // The angle of the robot relative to the ground.
+ ground_angle:double;
+
+ // Information about shifting logic and curent gear, for logging purposes
+ gear_logging:GearLogging;
+ cim_logging:CIMLogging;
+ trajectory_logging:TrajectoryLogging;
+ line_follow_logging:LineFollowLogging;
+}
+
+root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 34c9907..8ebbb1b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -90,23 +90,20 @@
DrivetrainSimulation::DrivetrainSimulation(
::aos::EventLoop *event_loop, const DrivetrainConfig<double> &dt_config)
: event_loop_(event_loop),
- robot_state_fetcher_(
- event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
+ robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
drivetrain_position_sender_(
event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")),
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")),
drivetrain_output_fetcher_(
- event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
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")),
gyro_reading_sender_(
event_loop->MakeSender<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")),
+ "/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -133,9 +130,9 @@
actual_y_.push_back(actual_position(1));
trajectory_x_.push_back(
- drivetrain_status_fetcher_->trajectory_logging.x);
+ drivetrain_status_fetcher_->trajectory_logging()->x());
trajectory_y_.push_back(
- drivetrain_status_fetcher_->trajectory_logging.y);
+ drivetrain_status_fetcher_->trajectory_logging()->y());
}
}
first_ = false;
@@ -159,22 +156,27 @@
const double right_encoder = GetRightPosition();
{
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>::Message
- position = drivetrain_position_sender_.MakeMessage();
- position->left_encoder = left_encoder;
- position->right_encoder = right_encoder;
- position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
- position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
- position.Send();
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>::Builder
+ builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_left_encoder(left_encoder);
+ position_builder.add_right_encoder(right_encoder);
+ position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
+ position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
+ builder.Send(position_builder.Finish());
}
{
- auto gyro = gyro_reading_sender_.MakeMessage();
- gyro->angle =
- (right_encoder - left_encoder) / (dt_config_.robot_radius * 2.0);
- gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
- (dt_config_.robot_radius * 2.0);
- gyro.Send();
+ auto builder = gyro_reading_sender_.MakeBuilder();
+ frc971::sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<frc971::sensors::GyroReading>();
+ gyro_builder.add_angle((right_encoder - left_encoder) /
+ (dt_config_.robot_radius * 2.0));
+ gyro_builder.add_velocity(
+ (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+ (dt_config_.robot_radius * 2.0));
+ builder.Send(gyro_builder.Finish());
}
}
@@ -184,17 +186,17 @@
last_right_position_ = drivetrain_plant_.Y(1, 0);
EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage;
+ last_U_ << drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage();
{
robot_state_fetcher_.Fetch();
const double scalar = robot_state_fetcher_.get()
- ? robot_state_fetcher_->voltage_battery / 12.0
+ ? robot_state_fetcher_->voltage_battery() / 12.0
: 1.0;
last_U_ *= scalar;
}
- left_gear_high_ = drivetrain_output_fetcher_->left_high;
- right_gear_high_ = drivetrain_output_fetcher_->right_high;
+ left_gear_high_ = drivetrain_output_fetcher_->left_high();
+ right_gear_high_ = drivetrain_output_fetcher_->right_high();
if (left_gear_high_) {
if (right_gear_high_) {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 9a4f177..2284f63 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -1,11 +1,15 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_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/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -79,11 +83,11 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
@@ -95,7 +99,8 @@
::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
::std::unique_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>> velocity_drivetrain_;
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
double last_left_position_;
double last_right_position_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 8234b1c..7fe5cad 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -105,9 +105,9 @@
void LineFollowDrivetrain::SetGoal(
::aos::monotonic_clock::time_point now,
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
// TODO(james): More properly calculate goal velocity from throttle.
- goal_velocity_ = 4.0 * goal.throttle;
+ goal_velocity_ = 4.0 * goal->throttle();
// The amount of time to freeze the target choice after the driver releases
// the button. Depending on the current state, we vary how long this timeout
// is so that we can avoid button glitches causing issues.
@@ -115,7 +115,7 @@
// Freeze the target once the driver presses the button; if we haven't yet
// confirmed a target when the driver presses the button, we will not do
// anything and report not ready until we have a target.
- if (goal.controller_type == 3) {
+ if (goal->controller_type() == drivetrain::ControllerType_LINE_FOLLOWER) {
last_enable_ = now;
// If we already acquired a target, we want to keep track if it.
if (have_target_) {
@@ -130,11 +130,11 @@
freeze_target_ = now <= freeze_delay + last_enable_;
// Set an adjustment that lets the driver tweak the offset for where we place
// the target left/right.
- side_adjust_ = -goal.wheel * 0.1;
+ side_adjust_ = -goal->wheel() * 0.1;
}
bool LineFollowDrivetrain::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
// TODO(james): Account for voltage error terms, and/or provide driver with
// ability to influence steering.
if (output != nullptr && have_target_) {
@@ -257,18 +257,20 @@
}
}
-void LineFollowDrivetrain::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
- status->line_follow_logging.frozen = freeze_target_;
- status->line_follow_logging.have_target = have_target_;
- status->line_follow_logging.x = target_pose_.abs_pos().x();
- status->line_follow_logging.y = target_pose_.abs_pos().y();
- status->line_follow_logging.theta = target_pose_.abs_theta();
- status->line_follow_logging.offset = relative_pose_.rel_pos().y();
- status->line_follow_logging.distance_to_target =
- -relative_pose_.rel_pos().x();
- status->line_follow_logging.goal_theta = controls_goal_(0, 0);
- status->line_follow_logging.rel_theta = relative_pose_.rel_theta();
+flatbuffers::Offset<LineFollowLogging> LineFollowDrivetrain::PopulateStatus(
+ aos::Sender<drivetrain::Status>::Builder *builder) const {
+ LineFollowLogging::Builder line_follow_logging_builder =
+ builder->MakeBuilder<LineFollowLogging>();
+ line_follow_logging_builder.add_frozen(freeze_target_);
+ line_follow_logging_builder.add_have_target(have_target_);
+ line_follow_logging_builder.add_x(target_pose_.abs_pos().x());
+ line_follow_logging_builder.add_y(target_pose_.abs_pos().y());
+ line_follow_logging_builder.add_theta(target_pose_.abs_theta());
+ line_follow_logging_builder.add_offset(relative_pose_.rel_pos().y());
+ line_follow_logging_builder.add_distance_to_target(-relative_pose_.rel_pos().x());
+ line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
+ line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
+ return line_follow_logging_builder.Finish();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 09b2080..1ebf64d 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -2,10 +2,15 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#include "Eigen/Dense"
-#include "frc971/control_loops/pose.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace frc971 {
namespace control_loops {
@@ -29,7 +34,7 @@
// a Target; the positive X-axis in the Pose's frame represents the direction
// we want to go (we approach the pose from the left-half plane).
void SetGoal(::aos::monotonic_clock::time_point now,
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ const ::frc971::control_loops::drivetrain::Goal *goal);
// State: [x, y, theta, left_vel, right_vel]
void Update(::aos::monotonic_clock::time_point now,
const ::Eigen::Matrix<double, 5, 1> &state);
@@ -37,9 +42,11 @@
// yet have a target to track into and so some other drivetrain should take
// over.
bool SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output);
- void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ ::frc971::control_loops::drivetrain::OutputT *output);
+
+ flatbuffers::Offset<LineFollowLogging> PopulateStatus(
+ aos::Sender<drivetrain::Status>::Builder *line_follow_logging_builder)
+ const;
private:
// Nominal max voltage.
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index f4d718e..bd45e92 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -50,13 +50,21 @@
}
void Iterate() {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.throttle = driver_model_(state_);
- goal.controller_type = freeze_target_ ? 3 : 0;
- drivetrain_.SetGoal(t_, goal);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ Goal::Builder goal_builder(fbb);
+ goal_builder.add_throttle(driver_model_(state_));
+ goal_builder.add_controller_type(freeze_target_
+ ? ControllerType_LINE_FOLLOWER
+ : ControllerType_POLYDRIVE);
+ fbb.Finish(goal_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+ drivetrain_.SetGoal(t_, &goal.message());
drivetrain_.Update(t_, state_);
- ::frc971::control_loops::DrivetrainQueue::Output output;
+ ::frc971::control_loops::drivetrain::OutputT output;
EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
if (!expect_output_) {
EXPECT_EQ(0.0, output.left_voltage);
@@ -96,9 +104,15 @@
}
double GoalTheta(double x, double y, double v, double throttle) {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.throttle = throttle;
- drivetrain_.SetGoal(t_, goal);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ Goal::Builder goal_builder(fbb);
+ goal_builder.add_throttle(throttle);
+ fbb.Finish(goal_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+ drivetrain_.SetGoal(t_, &goal.message());
::Eigen::Matrix<double, 5, 1> state;
state << x, y, 0.0, v, v;
return drivetrain_.GoalTheta(state, y, throttle > 0.0 ? 1.0 : -1.0);
diff --git a/frc971/control_loops/drivetrain/localizer.fbs b/frc971/control_loops/drivetrain/localizer.fbs
new file mode 100644
index 0000000..5450c08
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localizer.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+// Allows you to reset the state of the localizer to a specific position on the
+// field.
+table LocalizerControl {
+ x:float; // X position, meters
+ y:float; // Y position, meters
+ theta:float; // heading, radians
+ theta_uncertainty:double; // Uncertainty in theta.
+ keep_current_theta:bool; // Whether to keep the current theta value.
+}
+
+root_type LocalizerControl;
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 7b2ceae..2589d4b 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,7 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/pose.h"
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
deleted file mode 100644
index 323f895..0000000
--- a/frc971/control_loops/drivetrain/localizer.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package frc971.control_loops.drivetrain;
-
-// Allows you to reset the state of the localizer to a specific position on the
-// field.
-// Published on ".frc971.control_loops.drivetrain.localizer_control"
-message LocalizerControl {
- float x; // X position, meters
- float y; // Y position, meters
- float theta; // heading, radians
- double theta_uncertainty; // Uncertainty in theta.
- bool keep_current_theta; // Whether to keep the current theta value.
-};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index c213b06..a0d374e 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,14 +1,5 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
-#include "frc971/control_loops/coerce_goal.h"
-#ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#endif // __linux__
-#include "frc971/control_loops/state_feedback_loop.h"
-
namespace frc971 {
namespace control_loops {
namespace drivetrain {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index f8f1a39..74fb2eb 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -7,13 +7,18 @@
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/gear.h"
#ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/matrix_logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#else
-#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_float_generated.h"
#endif // __linux__
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -35,10 +40,11 @@
Scalar MotorSpeed(const constants::ShifterHallEffect &hall_effect,
Scalar shifter_position, Scalar velocity, Gear gear);
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const Scalar wheel, const Scalar throttle, const bool quickturn,
+ const bool highgear);
void SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ const ::frc971::control_loops::drivetrain::Position *position,
Gear left_gear, Gear right_gear);
Scalar FilterVelocity(Scalar throttle) const;
@@ -47,9 +53,10 @@
void Update(Scalar voltage_battery);
- void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
- void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+ flatbuffers::Offset<CIMLogging> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Computes the next state of a shifter given the current state and the
// requested state.
@@ -81,8 +88,8 @@
Gear left_gear_;
Gear right_gear_;
- ::frc971::control_loops::DrivetrainQueue::Position last_position_;
- ::frc971::control_loops::DrivetrainQueue::Position position_;
+ ::frc971::control_loops::drivetrain::PositionT last_position_;
+ ::frc971::control_loops::drivetrain::PositionT position_;
int counter_;
DrivetrainConfig<Scalar> dt_config_;
@@ -123,10 +130,7 @@
left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
counter_(0),
- dt_config_(dt_config) {
- last_position_.Zero();
- position_.Zero();
-}
+ dt_config_(dt_config) {}
template <typename Scalar>
Scalar PolyDrivetrain<Scalar>::MotorSpeed(
@@ -195,13 +199,9 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- const Scalar wheel = goal.wheel;
- const Scalar throttle = goal.throttle;
- const bool quickturn = goal.quickturn;
- const bool highgear = goal.highgear;
-
+void PolyDrivetrain<Scalar>::SetGoal(const Scalar wheel, const Scalar throttle,
+ const bool quickturn,
+ const bool highgear) {
// Apply a sin function that's scaled to make it feel better.
const Scalar angular_range =
static_cast<Scalar>(M_PI_2) * dt_config_.wheel_non_linearity;
@@ -234,12 +234,12 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ const ::frc971::control_loops::drivetrain::Position *position,
Gear left_gear, Gear right_gear) {
left_gear_ = left_gear;
right_gear_ = right_gear;
last_position_ = position_;
- position_ = *position;
+ position->UnPackTo(&position_);
}
template <typename Scalar>
@@ -415,8 +415,8 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
- if (output != NULL) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
+ if (output != nullptr) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
output->left_high = MaybeHigh(left_gear_);
@@ -425,19 +425,21 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) {
+flatbuffers::Offset<CIMLogging> PolyDrivetrain<Scalar>::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ CIMLogging::Builder builder(*fbb);
- status->cim_logging.left_in_gear = IsInGear(left_gear_);
- status->cim_logging.left_motor_speed = left_motor_speed_;
- status->cim_logging.left_velocity = current_left_velocity_;
+ builder.add_left_in_gear(IsInGear(left_gear_));
+ builder.add_left_motor_speed(left_motor_speed_);
+ builder.add_left_velocity(current_left_velocity_);
- status->cim_logging.right_in_gear = IsInGear(right_gear_);
- status->cim_logging.right_motor_speed = right_motor_speed_;
- status->cim_logging.right_velocity = current_right_velocity_;
+ builder.add_right_in_gear(IsInGear(right_gear_));
+ builder.add_right_motor_speed(right_motor_speed_);
+ builder.add_right_velocity(current_right_velocity_);
+
+ return builder.Finish();
}
-
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
deleted file mode 100644
index 2f63c9e..0000000
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ /dev/null
@@ -1,38 +0,0 @@
-#include "aos/controls/replay_control_loop.h"
-#include "aos/init.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
-
-// Reads one or more log files and sends out all the queue messages (in the
-// correct order and at the correct time) to feed a "live" drivetrain process.
-
-int main(int argc, char **argv) {
- if (argc <= 1) {
- fprintf(stderr, "Need at least one file to replay!\n");
- return EXIT_FAILURE;
- }
-
- ::aos::InitNRT();
-
- ::frc971::control_loops::DrivetrainQueue drivetrain_queue(
- ".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status");
-
- {
- ::aos::controls::ControlLoopReplayer<
- ::frc971::control_loops::DrivetrainQueue>
- replayer(&drivetrain_queue, "drivetrain");
-
- replayer.AddDirectQueueSender<::frc971::sensors::GyroReading>(
- "wpilib_interface.Gyro", "sending", ".frc971.sensors.gyro_reading");
- for (int i = 1; i < argc; ++i) {
- replayer.ProcessFile(argv[i]);
- }
- }
-
- ::aos::Cleanup();
-}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index de59355..1222ae0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,10 +2,13 @@
#include "Eigen/Dense"
-#include "aos/init.h"
+#include "aos/realtime.h"
#include "aos/util/math.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
namespace frc971 {
namespace control_loops {
@@ -30,7 +33,7 @@
::aos::MutexLocker locker(&mutex_);
while (run_) {
- while (goal_.spline.spline_idx == future_spline_idx_) {
+ while (goal_.spline_idx == future_spline_idx_) {
AOS_CHECK(!new_goal_.Wait());
if (!run_) {
return;
@@ -39,10 +42,10 @@
past_distance_spline_.reset();
past_trajectory_.reset();
- plan_state_ = PlanState::kBuildingTrajectory;
- const ::frc971::MultiSpline &multispline = goal_.spline;
- future_spline_idx_ = multispline.spline_idx;
+ plan_state_ = PlanningState_BUILDING_TRAJECTORY;
+ future_spline_idx_ = goal_.spline_idx;
planning_spline_idx_ = future_spline_idx_;
+ const MultiSpline &multispline = goal_.spline;
auto x = multispline.spline_x;
auto y = multispline.spline_y;
::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -56,7 +59,7 @@
splines.emplace_back(Spline(points));
}
- future_drive_spline_backwards_ = goal_.spline.drive_spline_backwards;
+ future_drive_spline_backwards_ = goal_.drive_spline_backwards;
future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(::std::move(splines)));
@@ -65,47 +68,79 @@
new Trajectory(future_distance_spline_.get(), dt_config_));
for (size_t i = 0; i < multispline.constraints.size(); ++i) {
- const ::frc971::Constraint &constraint = multispline.constraints[i];
+ const ::frc971::ConstraintT &constraint = multispline.constraints[i];
switch (constraint.constraint_type) {
- case 0:
+ case frc971::ConstraintType_CONSTRAINT_TYPE_UNDEFINED:
break;
- case 1:
+ case frc971::ConstraintType_LONGITUDINAL_ACCELERATION:
future_trajectory_->set_longitudal_acceleration(constraint.value);
break;
- case 2:
+ case frc971::ConstraintType_LATERAL_ACCELERATION:
future_trajectory_->set_lateral_acceleration(constraint.value);
break;
- case 3:
+ case frc971::ConstraintType_VOLTAGE:
future_trajectory_->set_voltage_limit(constraint.value);
break;
- case 4:
+ case frc971::ConstraintType_VELOCITY:
future_trajectory_->LimitVelocity(constraint.start_distance,
constraint.end_distance,
constraint.value);
break;
}
}
- plan_state_ = PlanState::kPlanningTrajectory;
+ plan_state_ = PlanningState_PLANNING_TRAJECTORY;
future_trajectory_->Plan();
- plan_state_ = PlanState::kPlannedTrajectory;
+ plan_state_ = PlanningState_PLANNED;
}
}
void SplineDrivetrain::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- current_spline_handle_ = goal.spline_handle;
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
+ current_spline_handle_ = goal->spline_handle();
+
// If told to stop, set the executing spline to an invalid index and clear out
// its plan:
if (current_spline_handle_ == 0 &&
- current_spline_idx_ != goal.spline.spline_idx) {
+ (goal->spline() == nullptr ||
+ current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
current_spline_idx_ = -1;
}
::aos::Mutex::State mutex_state = mutex_.TryLock();
if (mutex_state == ::aos::Mutex::State::kLocked) {
- if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
- goal_ = goal;
+ if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
+ future_spline_idx_ != goal->spline()->spline_idx()) {
+ CHECK_NOTNULL(goal->spline());
+ goal_.spline_idx = goal->spline()->spline_idx();
+ goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
+
+ const frc971::MultiSpline *multispline = goal->spline()->spline();
+ CHECK_NOTNULL(multispline);
+
+ goal_.spline.spline_count = multispline->spline_count();
+
+ CHECK_EQ(multispline->spline_x()->size(),
+ static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+ CHECK_EQ(multispline->spline_y()->size(),
+ static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+
+ std::copy(multispline->spline_x()->begin(),
+ multispline->spline_x()->end(), goal_.spline.spline_x.begin());
+ std::copy(multispline->spline_y()->begin(),
+ multispline->spline_y()->end(), goal_.spline.spline_y.begin());
+
+ for (size_t i = 0; i < 6; ++i) {
+ if (multispline->constraints() != nullptr &&
+ i < multispline->constraints()->size()) {
+ multispline->constraints()->Get(i)->UnPackTo(
+ &goal_.spline.constraints[i]);
+ } else {
+ goal_.spline.constraints[i].constraint_type =
+ ConstraintType_CONSTRAINT_TYPE_UNDEFINED;
+ }
+ }
+
new_goal_.Broadcast();
if (current_spline_handle_ != current_spline_idx_) {
// If we aren't going to actively execute the current spline, evict it's
@@ -178,7 +213,7 @@
}
void SplineDrivetrain::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
if (!output) {
return;
}
@@ -193,42 +228,52 @@
output->right_voltage = next_U_(1);
}
+
void SplineDrivetrain::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
- if (status && enable_) {
- status->uncapped_left_voltage = uncapped_U_(0);
- status->uncapped_right_voltage = uncapped_U_(1);
- status->robot_speed = current_xva_(1);
- status->output_was_capped = output_was_capped_;
+ drivetrain::Status::Builder *builder) const {
+ if (builder && enable_) {
+ builder->add_uncapped_left_voltage(uncapped_U_(0));
+ builder->add_uncapped_right_voltage(uncapped_U_(1));
+ builder->add_robot_speed(current_xva_(1));
+ builder->add_output_was_capped(output_was_capped_);
}
+}
- if (status) {
- if (current_distance_spline_) {
- ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- status->trajectory_logging.x = goal_state(0);
- status->trajectory_logging.y = goal_state(1);
- status->trajectory_logging.theta = ::aos::math::NormalizeAngle(
- goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0));
- status->trajectory_logging.left_velocity = goal_state(3);
- status->trajectory_logging.right_velocity = goal_state(4);
- }
- status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
- status->trajectory_logging.is_executed =
- (current_spline_idx_ != -1) && IsAtEnd();
- status->trajectory_logging.goal_spline_handle = current_spline_handle_;
- status->trajectory_logging.current_spline_idx = current_spline_idx_;
- status->trajectory_logging.distance_remaining =
- current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
- : 0.0;
-
- int32_t planning_spline_idx = planning_spline_idx_;
- if (current_spline_idx_ == planning_spline_idx) {
- status->trajectory_logging.planning_spline_idx = 0;
- } else {
- status->trajectory_logging.planning_spline_idx = planning_spline_idx_;
- }
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+ flatbuffers::FlatBufferBuilder *builder) const {
+ drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
+ if (current_distance_spline_) {
+ ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+ trajectory_logging_builder.add_x(goal_state(0));
+ trajectory_logging_builder.add_y(goal_state(1));
+ trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
+ goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+ trajectory_logging_builder.add_left_velocity(goal_state(3));
+ trajectory_logging_builder.add_right_velocity(goal_state(4));
}
+ trajectory_logging_builder.add_planning_state(plan_state_.load());
+ trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
+ has_started_execution_);
+ trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
+ IsAtEnd());
+ trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
+ trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
+ trajectory_logging_builder.add_distance_remaining(
+ current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
+ : 0.0);
+
+ int32_t planning_spline_idx = planning_spline_idx_;
+ if (current_spline_idx_ == planning_spline_idx) {
+ trajectory_logging_builder.add_planning_spline_idx(0);
+ } else {
+ trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+ }
+ return trajectory_logging_builder.Finish();
+}
+
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+ aos::Sender<drivetrain::Status>::Builder *builder) const {
+ return MakeTrajectoryLogging(builder->fbb());
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 98c4e74..4f21b29 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -8,9 +8,12 @@
#include "aos/condition.h"
#include "aos/mutex/mutex.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
@@ -31,14 +34,18 @@
worker_thread_.join();
}
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
- void SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output);
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
+
+ flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+ aos::Sender<drivetrain::Status>::Builder *builder) const;
+ flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+ flatbuffers::FlatBufferBuilder *builder) const;
void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ drivetrain::Status::Builder *status) const;
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
@@ -55,6 +62,9 @@
true;
}
+ // Returns true if the splinedrivetrain is enabled.
+ bool enable() const { return enable_; }
+
enum class PlanState : int8_t {
kNoPlan = 0,
kBuildingTrajectory = 1,
@@ -85,7 +95,7 @@
bool enable_ = false;
bool output_was_capped_ = false;
- std::atomic<PlanState> plan_state_ = {PlanState::kNoPlan};
+ std::atomic<PlanningState> plan_state_ = {PlanningState_NO_PLAN};
::std::thread worker_thread_;
// mutex_ is held by the worker thread while it is doing work or by the main
@@ -95,7 +105,26 @@
::aos::Condition new_goal_;
// The following variables are guarded by mutex_.
bool run_ = true;
- ::frc971::control_loops::DrivetrainQueue::Goal goal_;
+
+ // These two structures mirror the flatbuffer Multispline.
+ // TODO(austin): copy the goal flatbuffer directly instead of recreating it
+ // like this...
+ struct MultiSpline {
+ int32_t spline_count;
+ std::array<float, 36> spline_x;
+ std::array<float, 36> spline_y;
+ std::array<ConstraintT, 6> constraints;
+ };
+
+ struct SplineGoal {
+ int32_t spline_idx = 0;
+
+ bool drive_spline_backwards;
+
+ MultiSpline spline;
+ };
+
+ SplineGoal goal_;
::std::unique_ptr<DistanceSpline> past_distance_spline_;
::std::unique_ptr<DistanceSpline> future_distance_spline_;
::std::unique_ptr<Trajectory> past_trajectory_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 5924eb1..cd29e39 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -2,11 +2,12 @@
#include "aos/commonmath.h"
#include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
@@ -60,8 +61,6 @@
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
- AOS_LOG_MATRIX(DEBUG, "U_uncapped", *U);
-
Eigen::Matrix<double, 2, 2> position_K;
position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
kf_->controller().K(1, 0), kf_->controller().K(1, 2);
@@ -75,7 +74,6 @@
const auto drive_error = T_inverse_ * position_error;
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(1, 0), error(3, 0);
- AOS_LOG_MATRIX(DEBUG, "error", error);
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
@@ -144,23 +142,31 @@
}
void DrivetrainMotorsSS::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- unprofiled_goal_ << goal.left_goal, 0.0, goal.right_goal, 0.0, 0.0, 0.0, 0.0;
- if (::std::abs(goal.max_ss_voltage) < 0.01) {
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
+ unprofiled_goal_ << goal->left_goal(), 0.0, goal->right_goal(), 0.0, 0.0, 0.0,
+ 0.0;
+ if (!goal->has_max_ss_voltage()) {
max_voltage_ = kMaxVoltage;
} else {
- max_voltage_ = goal.max_ss_voltage;
+ max_voltage_ = goal->has_max_ss_voltage();
}
- use_profile_ =
- !kf_->controller().Kff().isZero(0) &&
- (goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
- goal.angular.max_velocity != 0.0 &&
- goal.angular.max_acceleration != 0.0);
- linear_profile_.set_maximum_velocity(goal.linear.max_velocity);
- linear_profile_.set_maximum_acceleration(goal.linear.max_acceleration);
- angular_profile_.set_maximum_velocity(goal.angular.max_velocity);
- angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
+ use_profile_ = !kf_->controller().Kff().isZero(0) &&
+ (goal->has_linear() && goal->has_angular() &&
+ goal->linear()->has_max_velocity() &&
+ goal->linear()->has_max_acceleration() &&
+ goal->angular()->has_max_velocity() &&
+ goal->angular()->has_max_acceleration());
+ if (goal->has_linear()) {
+ linear_profile_.set_maximum_velocity(goal->linear()->max_velocity());
+ linear_profile_.set_maximum_acceleration(
+ goal->linear()->max_acceleration());
+ }
+ if (goal->has_angular()) {
+ angular_profile_.set_maximum_velocity(goal->angular()->max_velocity());
+ angular_profile_.set_maximum_acceleration(
+ goal->angular()->max_acceleration());
+ }
}
void DrivetrainMotorsSS::Update(bool enable_control_loop) {
@@ -255,7 +261,7 @@
}
void DrivetrainMotorsSS::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) const {
+ ::frc971::control_loops::drivetrain::OutputT *output) const {
if (output) {
output->left_voltage = kf_->U(0, 0);
output->right_voltage = kf_->U(1, 0);
@@ -265,7 +271,7 @@
}
void DrivetrainMotorsSS::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ ::frc971::control_loops::drivetrain::StatusBuilder *builder) const {
Eigen::Matrix<double, 2, 1> profiled_linear =
dt_config_.LeftRightToLinear(kf_->next_R());
Eigen::Matrix<double, 2, 1> profiled_angular =
@@ -276,10 +282,10 @@
Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
- status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
- status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
- status->profiled_right_position_goal = profiled_gyro_left_right(2, 0);
- status->profiled_right_velocity_goal = profiled_gyro_left_right(3, 0);
+ builder->add_profiled_left_position_goal(profiled_gyro_left_right(0, 0));
+ builder->add_profiled_left_velocity_goal(profiled_gyro_left_right(1, 0));
+ builder->add_profiled_right_position_goal(profiled_gyro_left_right(2, 0));
+ builder->add_profiled_right_velocity_goal(profiled_gyro_left_right(3, 0));
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 762cbc2..4d91807 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -4,14 +4,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/util/trapezoid_profile.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
@@ -23,7 +25,7 @@
StateFeedbackLoop<7, 2, 4> *kf,
LocalizerInterface *localizer);
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
// Computes the power to send out as part of the controller. Should be called
// when disabled (with enable_control_loop false) so the profiles get computed
@@ -34,10 +36,9 @@
bool output_was_capped() const { return output_was_capped_; }
- void SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) const;
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output) const;
void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ ::frc971::control_loops::drivetrain::StatusBuilder *builder) const;
private:
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 9d35a5e..ba4c0c2 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -3,7 +3,6 @@
#include <chrono>
#include "Eigen/Dense"
-#include "aos/logging/matrix_logging.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
@@ -327,9 +326,7 @@
::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
- if (info == 0) {
- AOS_LOG_MATRIX(INFO, "K", K);
- } else {
+ if (info != 0) {
AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
controls::Controllability(A, B));
// TODO(austin): Can we be more clever here? Use the last one? We should
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 0b35ee8..2a990f9 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -3,7 +3,6 @@
#include <chrono>
#include "aos/logging/implementations.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "frc971/control_loops/dlqr.h"
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index d1b6efe..084c9a7 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -3,7 +3,7 @@
#include <stdint.h>
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
namespace frc971 {
@@ -27,22 +27,22 @@
double posedge_value() const { return posedge_value_; }
double negedge_value() const { return negedge_value_; }
- void Update(const HallEffectStruct &position) {
+ void Update(const HallEffectStruct *position) {
last_value_ = value_;
- value_ = position.current;
- posedge_value_ = position.posedge_value;
- negedge_value_ = position.negedge_value;
- posedges_.update(position.posedge_count);
- negedges_.update(position.negedge_count);
+ value_ = position->current();
+ posedge_value_ = position->posedge_value();
+ negedge_value_ = position->negedge_value();
+ posedges_.update(position->posedge_count());
+ negedges_.update(position->negedge_count());
}
- void Reset(const HallEffectStruct &position) {
- posedges_.Reset(position.posedge_count);
- negedges_.Reset(position.negedge_count);
- value_ = position.current;
- last_value_ = position.current;
- posedge_value_ = position.posedge_value;
- negedge_value_ = position.negedge_value;
+ void Reset(const HallEffectStruct *position) {
+ posedges_.Reset(position->posedge_count());
+ negedges_.Reset(position->negedge_count());
+ value_ = position->current();
+ last_value_ = position->current();
+ posedge_value_ = position->posedge_value();
+ negedge_value_ = position->negedge_value();
}
private:
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index c875a0b..612801e 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -127,72 +127,95 @@
current_position_ = new_position;
}
-void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<IndexPosition>
+PositionSensorSimulator::GetSensorValues<IndexPositionBuilder>(
+ IndexPositionBuilder *builder) {
+ builder->add_encoder(current_position_ - start_position_);
- values->index_pulses = lower_index_edge_.index_count();
- if (values->index_pulses == 0) {
- values->latched_encoder = 0.0;
+ const int index_count = lower_index_edge_.index_count();
+ builder->add_index_pulses(index_count);
+ if (index_count == 0) {
+ builder->add_latched_encoder(0.0);
} else {
// Populate the latched encoder samples.
- values->latched_encoder =
- lower_index_edge_.IndexPulsePosition() - start_position_;
+ builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+ start_position_);
}
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
- values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
- current_position_);
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndIndexPosition>
+PositionSensorSimulator::GetSensorValues<PotAndIndexPositionBuilder>(
+ PotAndIndexPositionBuilder *builder) {
+ builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_));
+ builder->add_encoder(current_position_ - start_position_);
if (lower_index_edge_.index_count() == 0) {
- values->latched_pot = 0.0;
- values->latched_encoder = 0.0;
+ builder->add_latched_pot(0.0);
+ builder->add_latched_encoder(0.0);
} else {
// Populate the latched pot/encoder samples.
- values->latched_pot = lower_index_edge_.latched_pot();
- values->latched_encoder =
- lower_index_edge_.IndexPulsePosition() - start_position_;
+ builder->add_latched_pot(lower_index_edge_.latched_pot());
+ builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+ start_position_);
}
- values->index_pulses = lower_index_edge_.index_count();
+ builder->add_index_pulses(lower_index_edge_.index_count());
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
- values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
- current_position_);
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndAbsolutePosition>
+PositionSensorSimulator::GetSensorValues<PotAndAbsolutePositionBuilder>(
+ PotAndAbsolutePositionBuilder *builder) {
+ builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_));
+ builder->add_encoder(current_position_ - start_position_);
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
- values->absolute_encoder = ::std::remainder(
+ double absolute_encoder = ::std::remainder(
current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (values->absolute_encoder < 0) {
- values->absolute_encoder += distance_per_revolution_;
+ if (absolute_encoder < 0) {
+ absolute_encoder += distance_per_revolution_;
}
+ builder->add_absolute_encoder(absolute_encoder);
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(AbsolutePosition *values) {
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<AbsolutePosition>
+PositionSensorSimulator::GetSensorValues<AbsolutePositionBuilder>(
+ AbsolutePositionBuilder *builder) {
+ builder->add_encoder(current_position_ - start_position_);
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
- values->absolute_encoder = ::std::remainder(
+ double absolute_encoder = ::std::remainder(
current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (values->absolute_encoder < 0) {
- values->absolute_encoder += distance_per_revolution_;
+ if (absolute_encoder < 0) {
+ absolute_encoder += distance_per_revolution_;
}
+ builder->add_absolute_encoder(absolute_encoder);
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
- values->current = lower_index_edge_.current_index_segment() !=
- upper_index_edge_.current_index_segment();
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<HallEffectAndPosition>
+PositionSensorSimulator::GetSensorValues<HallEffectAndPositionBuilder>(
+ HallEffectAndPositionBuilder *builder) {
+ builder->add_current(lower_index_edge_.current_index_segment() !=
+ upper_index_edge_.current_index_segment());
+ builder->add_encoder(current_position_ - start_position_);
- values->posedge_count = posedge_count_;
- values->negedge_count = negedge_count_;
- values->posedge_value = posedge_value_ - start_position_;
- values->negedge_value = negedge_value_ - start_position_;
+ builder->add_posedge_count(posedge_count_);
+ builder->add_negedge_count(negedge_count_);
+ builder->add_posedge_value(posedge_value_ - start_position_);
+ builder->add_negedge_value(negedge_value_ - start_position_);
+ return builder->Finish();
}
} // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 2880429..6be6bbc 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,9 +1,11 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#include "flatbuffers/flatbuffers.h"
+
#include "aos/testing/random_seed.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
namespace frc971 {
@@ -58,11 +60,17 @@
// values: The target structure will be populated with simulated sensor
// readings. The readings will be in SI units. For example the units
// can be given in radians, meters, etc.
- void GetSensorValues(IndexPosition *values);
- void GetSensorValues(PotAndIndexPosition *values);
- void GetSensorValues(AbsolutePosition *values);
- void GetSensorValues(PotAndAbsolutePosition *values);
- void GetSensorValues(HallEffectAndPosition *values);
+ template <typename PositionBuilder>
+ flatbuffers::Offset<typename PositionBuilder::Table> GetSensorValues(
+ PositionBuilder *builder);
+ template <typename Position>
+ const Position *FillSensorValues(flatbuffers::FlatBufferBuilder *fbb) {
+ fbb->Clear();
+ typename Position::Builder values(*fbb);
+
+ fbb->Finish(GetSensorValues(&values));
+ return flatbuffers::GetRoot<Position>(fbb->GetBufferPointer());
+ }
private:
// It turns out that we can re-use a lot of the same logic to count the index
@@ -84,7 +92,7 @@
index_count_ = 0;
}
- double index_count() const { return index_count_; }
+ int index_count() const { return index_count_; }
double latched_pot() const { return latched_pot_; }
int current_index_segment() const { return current_index_segment_; }
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index a4436ce..93a648a 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -5,10 +5,12 @@
#include <random>
#include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "aos/die.h"
+#include "flatbuffers/flatbuffers.h"
+
namespace frc971 {
namespace control_loops {
@@ -23,46 +25,51 @@
// this test is to verify that no false index pulses are generated while the
// mechanism stays between two index pulses.
const double index_diff = 0.5;
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, 0);
// Make sure that we don't accidentally hit an index pulse.
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
}
@@ -72,58 +79,59 @@
// again simulate zero noise on the potentiometer to accurately verify the
// mechanism's position during the index pulses.
const double index_diff = 0.8;
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
PositionSensorSimulator sim(index_diff);
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_EQ(0u, index_position.index_pulses);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_EQ(0u, index_position->index_pulses());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(1u, index_position.index_pulses);
- ASSERT_EQ(1u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(1u, index_position->index_pulses());
+ ASSERT_EQ(1u, pot_and_index_position->index_pulses());
sim.MoveTo(4.5 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(2u, index_position.index_pulses);
- ASSERT_EQ(2u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(2u, index_position->index_pulses());
+ ASSERT_EQ(2u, pot_and_index_position->index_pulses());
sim.MoveTo(5.9 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(3u, index_position.index_pulses);
- ASSERT_EQ(3u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(3u, index_position->index_pulses());
+ ASSERT_EQ(3u, pot_and_index_position->index_pulses());
sim.MoveTo(6.1 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(4u, index_position.index_pulses);
- ASSERT_EQ(4u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(4u, index_position->index_pulses());
+ ASSERT_EQ(4u, pot_and_index_position->index_pulses());
sim.MoveTo(8.7 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(5u, index_position.index_pulses);
- ASSERT_EQ(5u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(5u, index_position->index_pulses());
+ ASSERT_EQ(5u, pot_and_index_position->index_pulses());
sim.MoveTo(7.3 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(6u, index_position.index_pulses);
- ASSERT_EQ(6u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(6u, index_position->index_pulses());
+ ASSERT_EQ(6u, pot_and_index_position->index_pulses());
}
// Tests that the simulator handles non-zero specified index pulse locations
@@ -132,65 +140,66 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(2u, index_position.index_pulses);
- EXPECT_EQ(2u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, index_position->index_pulses());
+ EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(2u, index_position.index_pulses);
- EXPECT_EQ(2u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, index_position->index_pulses());
+ EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
}
// Tests that the latched values update correctly.
@@ -198,44 +207,45 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(0, 0.25);
- PotAndIndexPosition position;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(0u, position.index_pulses);
+ const PotAndIndexPosition *position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(0u, position->index_pulses());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
- const double first_latched_pot = position.latched_pot;
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
+ const double first_latched_pot = position->latched_pot();
sim.MoveTo(1.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(first_latched_pot, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(first_latched_pot, position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
sim.MoveTo(2.05 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, position->index_pulses());
+ EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
sim.MoveTo(1.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(3u, position.index_pulses);
- EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(3u, position->index_pulses());
+ EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
sim.MoveTo(0.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(4u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_GT(::std::abs(first_latched_pot - position.latched_pot), 0.005);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(4u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_GT(::std::abs(first_latched_pot - position->latched_pot()), 0.005);
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
}
// This test makes sure that our simulation for an absolute encoder + relative
@@ -263,42 +273,43 @@
const double index_diff = 0.1;
PositionSensorSimulator sim(index_diff);
sim.Initialize(0.20, 0.05, 0.2, 0.07);
- PotAndAbsolutePosition position;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.20);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.00, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ const PotAndAbsolutePosition *position =
+ sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.00, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.30);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.10, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.10, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.40);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.20, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.20, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.34);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.14, position.encoder);
- EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.14, position->encoder());
+ EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.24);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.04, position.encoder);
- EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.04, position->encoder());
+ EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.23);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.03, position.encoder);
- EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.03, position->encoder());
+ EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.13);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(-0.07, position.encoder);
- EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(-0.07, position->encoder());
+ EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
// TODO(philipp): Test negative values.
}
@@ -309,78 +320,80 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
- HallEffectAndPosition position;
+ //HallEffectAndPosition position;
+ flatbuffers::FlatBufferBuilder fbb;
// Go over only the lower edge rising.
sim.MoveTo(0.25);
- sim.GetSensorValues(&position);
- EXPECT_TRUE(position.current);
- EXPECT_DOUBLE_EQ(0.50, position.encoder);
- EXPECT_EQ(1, position.posedge_count);
- EXPECT_EQ(0.25, position.posedge_value);
- EXPECT_EQ(0, position.negedge_count);
- EXPECT_EQ(0, position.negedge_value);
+ const HallEffectAndPosition *position =
+ sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_TRUE(position->current());
+ EXPECT_DOUBLE_EQ(0.50, position->encoder());
+ EXPECT_EQ(1, position->posedge_count());
+ EXPECT_EQ(0.25, position->posedge_value());
+ EXPECT_EQ(0, position->negedge_count());
+ EXPECT_EQ(0, position->negedge_value());
// Now, go over the upper edge, falling.
sim.MoveTo(0.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(1.0, position.encoder);
- EXPECT_EQ(1, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.posedge_value);
- EXPECT_EQ(1, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(1.0, position->encoder());
+ EXPECT_EQ(1, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->posedge_value());
+ EXPECT_EQ(1, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->negedge_value());
// Now, jump a whole cycle.
sim.MoveTo(1.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(2.0, position.encoder);
- EXPECT_EQ(2, position.posedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.posedge_value);
- EXPECT_EQ(2, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.75, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(2.0, position->encoder());
+ EXPECT_EQ(2, position->posedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->posedge_value());
+ EXPECT_EQ(2, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.75, position->negedge_value());
// Now, jump a whole cycle backwards.
sim.MoveTo(0.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(1.0, position.encoder);
- EXPECT_EQ(3, position.posedge_count);
- EXPECT_DOUBLE_EQ(1.75, position.posedge_value);
- EXPECT_EQ(3, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(1.0, position->encoder());
+ EXPECT_EQ(3, position->posedge_count());
+ EXPECT_DOUBLE_EQ(1.75, position->posedge_value());
+ EXPECT_EQ(3, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
// Now, go over the upper edge, rising.
sim.MoveTo(0.25);
- sim.GetSensorValues(&position);
- EXPECT_TRUE(position.current);
- EXPECT_DOUBLE_EQ(0.5, position.encoder);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(3, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_TRUE(position->current());
+ EXPECT_DOUBLE_EQ(0.5, position->encoder());
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(3, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
// Now, go over the lower edge, falling.
sim.MoveTo(-0.25);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(0.0, position.encoder);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(4, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(0.0, position->encoder());
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(4, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
for (int i = 0; i < 10; ++i) {
// Now, go over the lower edge, falling.
sim.MoveTo(-0.25 - i * 1.0e-6);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_NEAR(-i * 1.0e-6, position.encoder, 1e-8);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(4, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_NEAR(-i * 1.0e-6, position->encoder(), 1e-8);
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(4, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
}
}
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
new file mode 100644
index 0000000..46bc9fc
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -0,0 +1,204 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops;
+
+table ProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table HallProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.HallEffectAndPositionEstimatorState;
+}
+
+table PotAndAbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table IndexProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.IndexEstimatorState;
+}
+
+table AbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.AbsoluteEncoderEstimatorState;
+}
+
+table StaticZeroingSingleDOFProfiledSubsystemGoal {
+ unsafe_goal:double;
+
+ profile_params:frc971.ProfileParameters;
+}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 58058c7..2752dcc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -11,8 +11,8 @@
#include "aos/controls/control_loop.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/profiled_subsystem.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
@@ -106,8 +106,9 @@
}
// Returns the current internal estimator state for logging.
- typename ZeroingEstimator::State EstimatorState(int index) {
- return estimators_[index].GetEstimatorState();
+ flatbuffers::Offset<typename ZeroingEstimator::State> EstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb, int index) {
+ return estimators_[index].GetEstimatorState(fbb);
}
// Sets the maximum voltage that will be commanded by the loop.
@@ -152,13 +153,14 @@
double default_angular_acceleration);
// Updates our estimator with the latest position.
- void Correct(typename ZeroingEstimator::Position position);
+ void Correct(const typename ZeroingEstimator::Position &position);
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- template <class StatusType>
- void PopulateStatus(StatusType *status);
+ template <class StatusTypeBuilder>
+ StatusTypeBuilder BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
@@ -166,7 +168,7 @@
// this goal.
void set_unprofiled_goal(double unprofiled_goal);
// Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
@@ -248,37 +250,41 @@
}
template <class ZeroingEstimator>
-template <class StatusType>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
- StatusType *status) {
- status->zeroed = this->zeroed();
- status->state = -1;
+template <class StatusTypeBuilder>
+StatusTypeBuilder SingleDOFProfiledSubsystem<ZeroingEstimator>::BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ flatbuffers::Offset<typename ZeroingEstimator::State> estimator_state =
+ this->EstimatorState(fbb, 0);
+
+ StatusTypeBuilder builder(*fbb);
+
+ builder.add_zeroed(this->zeroed());
// We don't know, so default to the bad case.
- status->estopped = true;
- status->position = this->X_hat(0, 0);
- status->velocity = this->X_hat(1, 0);
- status->goal_position = this->goal(0, 0);
- status->goal_velocity = this->goal(1, 0);
- status->unprofiled_goal_position = this->unprofiled_goal(0, 0);
- status->unprofiled_goal_velocity = this->unprofiled_goal(1, 0);
- status->voltage_error = this->X_hat(2, 0);
- status->calculated_velocity =
+ builder.add_position(this->X_hat(0, 0));
+ builder.add_velocity(this->X_hat(1, 0));
+ builder.add_goal_position(this->goal(0, 0));
+ builder.add_goal_velocity(this->goal(1, 0));
+ builder.add_unprofiled_goal_position(this->unprofiled_goal(0, 0));
+ builder.add_unprofiled_goal_velocity(this->unprofiled_goal(1, 0));
+ builder.add_voltage_error(this->X_hat(2, 0));
+ builder.add_calculated_velocity(
(position() - last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
- status->estimator_state = this->EstimatorState(0);
+ builder.add_estimator_state(estimator_state);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power =
- this->controller().controller().K(0, 0) * error(0, 0);
- status->velocity_power =
- this->controller().controller().K(0, 1) * error(1, 0);
+ builder.add_position_power(
+ this->controller().controller().K(0, 0) * error(0, 0));
+ builder.add_velocity_power(
+ this->controller().controller().K(0, 1) * error(1, 0));
+ return builder;
}
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
- typename ZeroingEstimator::Position new_position) {
+ const typename ZeroingEstimator::Position &new_position) {
this->estimators_[0].UpdateEstimate(new_position);
if (this->estimators_[0].error()) {
@@ -299,7 +305,7 @@
}
last_position_ = position();
- this->Y_ << new_position.encoder;
+ this->Y_ << new_position.encoder();
this->Y_ += this->offset_;
this->loop_->Correct(Y_);
}
@@ -385,9 +391,11 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
- const ::frc971::ProfileParameters &profile_parameters) {
- AdjustProfile(profile_parameters.max_velocity,
- profile_parameters.max_acceleration);
+ const ::frc971::ProfileParameters *profile_parameters) {
+ AdjustProfile(
+ profile_parameters != nullptr ? profile_parameters->max_velocity() : 0.0,
+ profile_parameters != nullptr ? profile_parameters->max_acceleration()
+ : 0.0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
deleted file mode 100644
index ac23907..0000000
--- a/frc971/control_loops/profiled_subsystem.q
+++ /dev/null
@@ -1,198 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/control_loops.q";
-
-struct ProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-struct HallProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.HallEffectAndPositionEstimatorState estimator_state;
-};
-
-struct PotAndAbsoluteEncoderProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IndexProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.IndexEstimatorState estimator_state;
-};
-
-struct AbsoluteEncoderProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.AbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct StaticZeroingSingleDOFProfiledSubsystemGoal {
- double unsafe_goal;
- .frc971.ProfileParameters profile_params;
-};
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 748086f..b220581 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -6,6 +6,12 @@
namespace frc971 {
namespace control_loops {
+// TODO(austin): Use ProfileParametersT...
+struct ProfileParametersStruct {
+ float max_velocity;
+ float max_acceleration;
+};
+
template <typename ZeroingEstimator>
struct StaticZeroingSingleDOFProfiledSubsystemParams {
// Maximum voltage while the subsystem is zeroing
@@ -15,11 +21,11 @@
double operating_voltage;
// Maximum velocity (units/s) and acceleration while State::ZEROING
- ::frc971::ProfileParameters zeroing_profile_params;
+ ProfileParametersStruct zeroing_profile_params;
// Maximum velocity (units/s) and acceleration while State::RUNNING if max
// velocity or acceleration in goal profile_params is 0
- ::frc971::ProfileParameters default_profile_params;
+ ProfileParametersStruct default_profile_params;
// Maximum range of the subsystem in meters
::frc971::constants::Range range;
@@ -65,9 +71,10 @@
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
- void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position,
- double *output, ProfiledJointStatus *status);
+ flatbuffers::Offset<ProfiledJointStatus> Iterate(
+ const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ flatbuffers::FlatBufferBuilder *status_fbb);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -125,11 +132,11 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
- ProfiledJointStatus>::
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ProfiledJointStatus *status) {
+ flatbuffers::FlatBufferBuilder *status_fbb) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -159,7 +166,9 @@
// jump.
profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
// Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(params_.zeroing_profile_params);
+ profiled_subsystem_.AdjustProfile(
+ params_.zeroing_profile_params.max_velocity,
+ params_.zeroing_profile_params.max_acceleration);
// We are not ready to start doing anything yet.
disabled = true;
@@ -181,9 +190,9 @@
}
if (goal) {
- profiled_subsystem_.AdjustProfile(goal->profile_params);
+ profiled_subsystem_.AdjustProfile(goal->profile_params());
- double safe_goal = goal->unsafe_goal;
+ double safe_goal = goal->unsafe_goal();
if (safe_goal < min_position_) {
AOS_LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
safe_goal = min_position_;
@@ -217,11 +226,14 @@
*output = profiled_subsystem_.voltage();
}
- status->zeroed = profiled_subsystem_.zeroed();
+ typename ProfiledJointStatus::Builder status_builder =
+ profiled_subsystem_
+ .template BuildStatus<typename ProfiledJointStatus::Builder>(
+ status_fbb);
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ status_builder.add_estopped(state_ == State::ESTOP);
+ status_builder.add_state(static_cast<int32_t>(state_));
+ return status_builder.Finish();
}
} // namespace control_loops
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 96cb782..46e9c76 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -1,3 +1,4 @@
+#include "flatbuffers/flatbuffers.h"
#include "gtest/gtest.h"
#include "aos/controls/control_loop.h"
@@ -5,9 +6,10 @@
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
+#include "frc971/zeroing/zeroing.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -20,20 +22,32 @@
using ::aos::monotonic_clock;
using SZSDPS_PotAndAbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
using SZSDPS_AbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+using FBB = flatbuffers::FlatBufferBuilder;
+
+struct PotAndAbsoluteEncoderQueueGroup {
+ typedef PotAndAbsoluteEncoderProfiledJointStatus Status;
+ typedef PotAndAbsolutePosition Position;
+ typedef ::frc971::control_loops::zeroing::testing::SubsystemGoal Goal;
+ typedef ::frc971::control_loops::zeroing::testing::SubsystemOutput Output;
+};
+
+struct AbsoluteEncoderQueueGroup {
+ typedef AbsoluteEncoderProfiledJointStatus Status;
+ typedef AbsolutePosition Position;
+ typedef zeroing::testing::SubsystemGoal Goal;
+ typedef zeroing::testing::SubsystemOutput Output;
+};
+
typedef ::testing::Types<
- ::std::pair<
- SZSDPS_AbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
- ::std::pair<
- SZSDPS_PotAndAbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+ ::std::pair<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>,
+ ::std::pair<SZSDPS_PotAndAbsEncoder, PotAndAbsoluteEncoderQueueGroup>>
TestTypes;
constexpr ::frc971::constants::Range kRange{
@@ -54,14 +68,15 @@
};
template <>
-const zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
+const frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
TestIntakeSystemValues<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
+ frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
template <>
-const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
- TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
+const frc971::zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<
+ frc971::zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
template <typename ZeroingEstimator>
@@ -83,29 +98,21 @@
template <typename SZSDPS, typename QueueGroup>
class TestIntakeSystemSimulation {
public:
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
subsystem_position_sender_(
- event_loop_->MakeSender<PositionType>(".position")),
+ event_loop_->MakeSender<PositionType>("/loop")),
subsystem_status_fetcher_(
- event_loop_->MakeFetcher<StatusType>(".status")),
+ event_loop_->MakeFetcher<StatusType>("/loop")),
subsystem_output_fetcher_(
- event_loop_->MakeFetcher<OutputType>(".output")),
+ event_loop_->MakeFetcher<OutputType>("/loop")),
subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
subsystem_sensor_sim_(kEncoderIndexDifference) {
@@ -133,8 +140,8 @@
void InitializeSensorSim(double start_pos);
- double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
- double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
+ double subsystem_position() const { return subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
@@ -144,12 +151,13 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- typename ::aos::Sender<PositionType>::Message position =
- subsystem_position_sender_.MakeMessage();
+ typename ::aos::Sender<PositionType>::Builder position =
+ subsystem_position_sender_.MakeBuilder();
- this->subsystem_sensor_sim_.GetSensorValues(&position->position);
-
- position.Send();
+ auto position_builder = position.template MakeBuilder<PositionType>();
+ position.Send(this->subsystem_sensor_sim_
+ .template GetSensorValues<typename PositionType::Builder>(
+ &position_builder));
}
void set_peak_subsystem_acceleration(double value) {
@@ -168,15 +176,15 @@
const double voltage_check_subsystem =
(static_cast<typename SZSDPS::State>(
- subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+ subsystem_status_fetcher_->state()) == SZSDPS::State::RUNNING)
? kOperatingVoltage
: kZeroingVoltage;
- EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+ EXPECT_LE(::std::abs(subsystem_output_fetcher_->output()),
voltage_check_subsystem);
::Eigen::Matrix<double, 1, 1> subsystem_U;
- subsystem_U << subsystem_output_fetcher_->output +
+ subsystem_U << subsystem_output_fetcher_->output() +
subsystem_plant_->voltage_offset();
subsystem_plant_->Update(subsystem_U);
@@ -211,13 +219,14 @@
double peak_subsystem_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_subsystem_velocity_ = 1e10;
+
+ flatbuffers::FlatBufferBuilder fbb;
};
template <>
void TestIntakeSystemSimulation<
SZSDPS_PotAndAbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
- InitializeSensorSim(double start_pos) {
+ PotAndAbsoluteEncoderQueueGroup>::InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
TestIntakeSystemValues<
@@ -226,9 +235,7 @@
}
template <>
-void TestIntakeSystemSimulation<
- SZSDPS_AbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>::
InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
@@ -240,46 +247,73 @@
// Class to represent a module using a subsystem. This lets us use event loops
// to wrap it.
template <typename QueueGroup, typename SZSDPS>
-class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+class Subsystem
+ : public ::aos::controls::ControlLoop<
+ typename QueueGroup::Goal, typename QueueGroup::Position,
+ typename QueueGroup::Status, typename QueueGroup::Output> {
public:
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+ : aos::controls::ControlLoop<
+ typename QueueGroup::Goal, typename QueueGroup::Position,
+ typename QueueGroup::Status, typename QueueGroup::Output>(
+ event_loop, name),
subsystem_(TestIntakeSystemValues<
typename SZSDPS::ZeroingEstimator>::make_params()) {}
void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
- OutputType *output, StatusType *status) {
+ typename ::aos::Sender<OutputType>::Builder *output,
+ typename ::aos::Sender<StatusType>::Builder *status) {
if (this->WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
subsystem_.Reset();
}
// Convert one goal type to another...
- StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+ // TODO(austin): This mallocs...
+ FBB fbb;
+ ProfileParametersBuilder params_builder(fbb);
if (unsafe_goal != nullptr ) {
- goal.unsafe_goal = unsafe_goal->unsafe_goal;
- goal.profile_params.max_velocity =
- unsafe_goal->profile_params.max_velocity;
- goal.profile_params.max_acceleration =
- unsafe_goal->profile_params.max_acceleration;
+ if (unsafe_goal->profile_params() != nullptr) {
+ params_builder.add_max_velocity(
+ unsafe_goal->profile_params()->max_velocity());
+ params_builder.add_max_acceleration(
+ unsafe_goal->profile_params()->max_acceleration());
+ }
+
+ const auto params_builder_offset = params_builder.Finish();
+ StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+ goal_builder.add_unsafe_goal(unsafe_goal->unsafe_goal());
+ goal_builder.add_profile_params(params_builder_offset);
+ fbb.Finish(goal_builder.Finish());
+ } else {
+ params_builder.add_max_velocity(0.0);
+ params_builder.add_max_acceleration(0.0);
+ const auto params_builder_offset = params_builder.Finish();
+ StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+ goal_builder.add_profile_params(params_builder_offset);
+ fbb.Finish(goal_builder.Finish());
}
- subsystem_.Iterate(
- unsafe_goal == nullptr ? nullptr : &goal, &position->position,
- output == nullptr ? nullptr : &output->output, &status->status);
+ double output_voltage;
+
+ flatbuffers::Offset<StatusType> status_offset = subsystem_.Iterate(
+ unsafe_goal == nullptr
+ ? nullptr
+ : flatbuffers::GetRoot<StaticZeroingSingleDOFProfiledSubsystemGoal>(
+ fbb.GetBufferPointer()),
+ position, output == nullptr ? nullptr : &output_voltage, status->fbb());
+ status->Send(status_offset);
+ if (output != nullptr) {
+ typename OutputType::Builder output_builder =
+ output->template MakeBuilder<OutputType>();
+ output_builder.add_output(output_voltage);
+ output->Send(output_builder.Finish());
+ }
}
SZSDPS *subsystem() { return &subsystem_; }
@@ -296,29 +330,58 @@
using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
IntakeSystemTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ "{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.JoystickState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.RobotState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.zeroing.testing.SubsystemGoal\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.zeroing.testing.SubsystemOutput\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.AbsolutePosition\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.PotAndAbsolutePosition\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.AbsoluteEncoderProfiledJointStatus\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+ subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>("/loop")),
subsystem_goal_fetcher_(
- test_event_loop_->MakeFetcher<GoalType>(".goal")),
+ test_event_loop_->MakeFetcher<GoalType>("/loop")),
subsystem_status_fetcher_(
- test_event_loop_->MakeFetcher<StatusType>(".status")),
+ test_event_loop_->MakeFetcher<StatusType>("/loop")),
subsystem_event_loop_(MakeEventLoop()),
- subsystem_(subsystem_event_loop_.get(), ""),
+ subsystem_(subsystem_event_loop_.get(), "/loop"),
subsystem_plant_event_loop_(MakeEventLoop()),
subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
@@ -327,20 +390,20 @@
EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
- subsystem_status_fetcher_->status.position, 0.001);
- EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
+ subsystem_status_fetcher_->position(), 0.001);
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
subsystem_plant_.subsystem_position(), 0.001);
- EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+ EXPECT_NEAR(subsystem_status_fetcher_->velocity(), 0, 0.001);
}
SZSDPS *subsystem() { return subsystem_.subsystem(); }
void set_peak_subsystem_acceleration(double value) {
- set_peak_subsystem_acceleration(value);
+ subsystem_plant_.set_peak_subsystem_acceleration(value);
}
void set_peak_subsystem_velocity(double value) {
- set_peak_subsystem_velocity(value);
+ subsystem_plant_.set_peak_subsystem_velocity(value);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -363,9 +426,9 @@
this->SetEnabled(true);
// Intake system uses 0.05 to test for 0.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 0.05;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.05)));
}
this->RunFor(chrono::seconds(5));
@@ -377,11 +440,13 @@
this->SetEnabled(true);
// Set a reasonable goal.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 0.1;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish())));
}
// Give it a lot of time to get there.
@@ -396,9 +461,9 @@
this->SetEnabled(true);
// Zero it before we move.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
@@ -406,11 +471,12 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower;
- message->profile_params.max_velocity = 20.0;
- message->profile_params.max_acceleration = 0.1;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(20.0);
+ profile_builder.add_max_acceleration(0.1);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower, profile_builder.Finish())));
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
@@ -420,11 +486,13 @@
// Now do a high acceleration move with a low velocity limit.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- message->profile_params.max_velocity = 0.1;
- message->profile_params.max_acceleration = 100;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(0.1);
+ profile_builder.add_max_acceleration(100.0);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())));
}
this->set_peak_subsystem_velocity(0.2);
@@ -441,34 +509,34 @@
// Set some ridiculous goals to test upper limits.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 100.0;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 100.0, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
- 0.001);
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = -100.0;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), -100.0, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
- 0.001);
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(), 0.001);
}
// Tests that the subsystem loop zeroes when run for a while.
@@ -476,11 +544,12 @@
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
@@ -500,9 +569,9 @@
this->SetEnabled(true);
this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(10));
@@ -515,9 +584,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(10));
@@ -530,9 +599,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper - 0.1;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper - 0.1)));
}
this->RunFor(chrono::seconds(10));
@@ -554,9 +623,9 @@
// Tests that the internal goals don't change while disabled.
TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower + 0.03;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower + 0.03)));
}
// Checks that the subsystem has not moved from its starting position at 0
@@ -572,9 +641,9 @@
// Tests that zeroing while disabled works.
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower)));
}
// Run disabled for 2 seconds
@@ -591,16 +660,16 @@
TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower_hard;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower_hard)));
}
this->RunFor(chrono::seconds(2));
// Check that kRange.lower is used as the default min position
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
0.001);
// Set min position and check that the subsystem increases to that position
@@ -608,7 +677,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->position(),
0.001);
// Clear min position and check that the subsystem returns to kRange.lower
@@ -616,7 +685,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
0.001);
}
@@ -625,16 +694,16 @@
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper_hard;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper_hard)));
}
this->RunFor(chrono::seconds(2));
// Check that kRange.upper is used as the default max position
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
0.001);
// Set max position and check that the subsystem lowers to that position
@@ -642,7 +711,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->position(),
0.001);
// Clear max position and check that the subsystem returns to kRange.upper
@@ -650,7 +719,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
0.001);
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
new file mode 100644
index 0000000..e39272a
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
@@ -0,0 +1,13 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.zeroing.testing;
+
+table SubsystemGoal {
+ unsafe_goal:double;
+ profile_params:frc971.ProfileParameters;
+}
+
+table SubsystemOutput {
+ output:double;
+}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
deleted file mode 100644
index 3b35837..0000000
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
+++ /dev/null
@@ -1,46 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/profiled_subsystem.q";
-
-message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
- double unsafe_goal;
- .frc971.ProfileParameters profile_params;
-};
-
-message StaticZeroingSingleDOFProfiledSubsystemOutput {
- double output;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
- implements aos.control_loops.ControlLoop;
-
- message Status {
- PotAndAbsoluteEncoderProfiledJointStatus status;
- };
-
- message Position {
- PotAndAbsolutePosition position;
- };
-
- queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
- queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
- queue Status status;
- queue Position position;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
- implements aos.control_loops.ControlLoop;
-
- message Status {
- AbsoluteEncoderProfiledJointStatus status;
- };
-
- message Position {
- AbsolutePosition position;
- };
-
- queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
- queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
- queue Status status;
- queue Position position;
-};
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
index 9ef4036..73898f9 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap_test.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -4,7 +4,6 @@
#include "gtest/gtest.h"
-#include "aos/queue.h"
#include "aos/testing/test_shm.h"
namespace frc971 {
diff --git a/frc971/downloader.bzl b/frc971/downloader.bzl
index 2967127..842398f58 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -1,37 +1,37 @@
-load('//aos/downloader:downloader.bzl', 'aos_downloader')
-load('//tools/build_rules:label.bzl', 'expand_label')
+load("//frc971/downloader:downloader.bzl", "aos_downloader")
+load("//tools/build_rules:label.bzl", "expand_label")
-def robot_downloader(start_binaries, binaries=[], dirs=None, default_target=None):
- '''Sets up the standard robot download targets.
+def robot_downloader(start_binaries, binaries = [], dirs = None, default_target = None):
+ """Sets up the standard robot download targets.
- Attrs:
- start_binaries: A list of cc_binary targets to start on the robot.
- dirs: Passed through to aos_downloader.
- default_target: Passed through to aos_downloader.
- '''
+ Attrs:
+ start_binaries: A list of cc_binary targets to start on the robot.
+ dirs: Passed through to aos_downloader.
+ default_target: Passed through to aos_downloader.
+ """
- aos_downloader(
- name = 'download',
- start_srcs = [
- '//aos:prime_start_binaries',
- ] + start_binaries,
- srcs = [
- '//aos:prime_binaries',
- ] + binaries,
- dirs = dirs,
- default_target = default_target,
- restricted_to = ['//tools:roborio'],
- )
+ aos_downloader(
+ name = "download",
+ start_srcs = [
+ "//aos:prime_start_binaries",
+ ] + start_binaries,
+ srcs = [
+ "//aos:prime_binaries",
+ ] + binaries,
+ dirs = dirs,
+ default_target = default_target,
+ restricted_to = ["//tools:roborio"],
+ )
- aos_downloader(
- name = 'download_stripped',
- start_srcs = [
- '//aos:prime_start_binaries_stripped',
- ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
- srcs = [
- '//aos:prime_binaries_stripped',
- ] + [expand_label(binary) + ".stripped" for binary in binaries],
- dirs = dirs,
- default_target = default_target,
- restricted_to = ['//tools:roborio'],
- )
+ aos_downloader(
+ name = "download_stripped",
+ start_srcs = [
+ "//aos:prime_start_binaries_stripped",
+ ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
+ srcs = [
+ "//aos:prime_binaries_stripped",
+ ] + [expand_label(binary) + ".stripped" for binary in binaries],
+ dirs = dirs,
+ default_target = default_target,
+ restricted_to = ["//tools:roborio"],
+ )
diff --git a/aos/downloader/BUILD b/frc971/downloader/BUILD
similarity index 100%
rename from aos/downloader/BUILD
rename to frc971/downloader/BUILD
diff --git a/frc971/downloader/downloader.bzl b/frc971/downloader/downloader.bzl
new file mode 100644
index 0000000..a5d1bc1
--- /dev/null
+++ b/frc971/downloader/downloader.bzl
@@ -0,0 +1,119 @@
+def _aos_downloader_impl(ctx):
+ all_files = ctx.files.srcs + ctx.files.start_srcs + [ctx.outputs._startlist]
+ ctx.file_action(
+ output = ctx.outputs.executable,
+ executable = True,
+ content = "\n".join([
+ "#!/bin/bash",
+ "set -e",
+ 'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
+ ] + ['%s %s --dirs %s -- %s "$@"' % (
+ ctx.executable._downloader.short_path,
+ " ".join([src.short_path for src in d.downloader_srcs]),
+ d.downloader_dir,
+ ctx.attr.default_target,
+ ) for d in ctx.attr.dirs] + [
+ 'exec %s %s -- %s "$@"' % (
+ ctx.executable._downloader.short_path,
+ " ".join([src.short_path for src in all_files]),
+ ctx.attr.default_target,
+ ),
+ ]),
+ )
+
+ ctx.file_action(
+ output = ctx.outputs._startlist,
+ content = "\n".join([f.basename for f in ctx.files.start_srcs]) + "\n",
+ )
+
+ to_download = [ctx.outputs._startlist]
+ to_download += all_files
+ for d in ctx.attr.dirs:
+ to_download += d.downloader_srcs
+
+ return struct(
+ runfiles = ctx.runfiles(
+ files = to_download + ctx.files._downloader,
+ transitive_files = ctx.attr._downloader.default_runfiles.files,
+ collect_data = True,
+ collect_default = True,
+ ),
+ files = depset([ctx.outputs.executable]),
+ )
+
+def _aos_downloader_dir_impl(ctx):
+ return struct(
+ downloader_dir = ctx.attr.dir,
+ downloader_srcs = ctx.files.srcs,
+ )
+
+"""Creates a binary which downloads code to a robot.
+
+Running this with `bazel run` will actually download everything.
+
+This also generates a start_list.txt file with the names of binaries to start.
+
+Attrs:
+ srcs: The files to download. They currently all get shoved into one folder.
+ dirs: A list of aos_downloader_dirs to download too.
+ start_srcs: Like srcs, except they also get put into start_list.txt.
+ default_target: The default host to download to. If not specified, defaults to
+ roboRIO-971.local.
+"""
+
+aos_downloader = rule(
+ attrs = {
+ "_downloader": attr.label(
+ executable = True,
+ cfg = "host",
+ default = Label("//frc971/downloader"),
+ ),
+ "start_srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "dirs": attr.label_list(
+ mandatory = False,
+ providers = [
+ "downloader_dir",
+ "downloader_srcs",
+ ],
+ ),
+ "default_target": attr.string(
+ default = "roboRIO-971-frc.local",
+ ),
+ },
+ executable = True,
+ outputs = {
+ "_startlist": "%{name}.start_list.dir/start_list.txt",
+ },
+ implementation = _aos_downloader_impl,
+)
+
+"""Downloads files to a specific directory.
+
+This rule does nothing by itself. Use it by adding to the dirs attribute of an
+aos_downloader rule.
+
+Attrs:
+ srcs: The files to download. They all go in the same directory.
+ dir: The directory (relative to the standard download directory) to put all
+ the files in.
+"""
+
+aos_downloader_dir = rule(
+ attrs = {
+ "srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "dir": attr.string(
+ mandatory = True,
+ ),
+ },
+ implementation = _aos_downloader_dir_impl,
+)
diff --git a/aos/downloader/downloader.py b/frc971/downloader/downloader.py
similarity index 100%
rename from aos/downloader/downloader.py
rename to frc971/downloader/downloader.py
diff --git a/frc971/queues/BUILD b/frc971/queues/BUILD
index 83ee252..e440638 100644
--- a/frc971/queues/BUILD
+++ b/frc971/queues/BUILD
@@ -1,10 +1,11 @@
-package(default_visibility = ['//visibility:public'])
+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 = 'gyro',
- srcs = [
- 'gyro.q',
- ],
+flatbuffer_cc_library(
+ name = "gyro",
+ srcs = [
+ "gyro.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/frc971/queues/gyro.fbs b/frc971/queues/gyro.fbs
new file mode 100644
index 0000000..8a76678
--- /dev/null
+++ b/frc971/queues/gyro.fbs
@@ -0,0 +1,18 @@
+namespace frc971.sensors;
+
+// Published on "/drivetrain"
+table GyroReading {
+ // Positive is counter-clockwise (Austin says "it's Positive").
+ // Right-hand coordinate system around the Z-axis going up.
+ // The angle is measured in radians.
+ angle:double;
+ // The angular velocity in radians/sec
+ velocity:double;
+}
+
+// Published on "/drivetrain"
+table Uid {
+ uid:uint;
+}
+
+root_type GyroReading;
diff --git a/frc971/queues/gyro.q b/frc971/queues/gyro.q
deleted file mode 100644
index 0885738..0000000
--- a/frc971/queues/gyro.q
+++ /dev/null
@@ -1,16 +0,0 @@
-package frc971.sensors;
-
-// Published on ".frc971.sensors.gyro_reading"
-message GyroReading {
- // Positive is counter-clockwise (Austin says "it's Positive").
- // Right-hand coordinate system around the Z-axis going up.
- // The angle is measured in radians.
- double angle;
- // The angular velocity in radians/sec
- double velocity;
-};
-
-// Published on ".frc971.sensors.gyro_part_id"
-message Uid {
- uint32_t uid;
-};
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 4fc2c7b..f364d56 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -8,10 +8,9 @@
#include <chrono>
#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
@@ -121,10 +120,9 @@
ADIS16448::ADIS16448(::aos::EventLoop *event_loop, frc::SPI::Port port,
frc::DigitalInput *dio1)
: event_loop_(event_loop),
- joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
- ".aos.joystick_state")),
+ robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
imu_values_sender_(
- event_loop_->MakeSender<::frc971::IMUValues>(".frc971.imu_values")),
+ event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
spi_(new frc::SPI(port)),
dio1_(dio1) {
// 1MHz is the maximum supported for burst reads, but we
@@ -247,31 +245,35 @@
}
}
- auto message = imu_values_sender_.MakeMessage();
- message->fpga_timestamp = ::aos::time::DurationInSeconds(
- dio1_->ReadRisingTimestamp().time_since_epoch());
- message->monotonic_timestamp_ns =
- chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
- .count();
+ auto builder = imu_values_sender_.MakeBuilder();
- message->gyro_x =
+ IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+
+ imu_builder.add_fpga_timestamp(::aos::time::DurationInSeconds(
+ dio1_->ReadRisingTimestamp().time_since_epoch()));
+ imu_builder.add_monotonic_timestamp_ns(
+ chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
+ .count());
+
+ float gyro_x =
ConvertValue(&to_receive[4], kGyroLsbDegreeSecond * M_PI / 180.0);
- message->gyro_y =
+ float gyro_y =
ConvertValue(&to_receive[6], kGyroLsbDegreeSecond * M_PI / 180.0);
- message->gyro_z =
+ float gyro_z =
ConvertValue(&to_receive[8], kGyroLsbDegreeSecond * M_PI / 180.0);
// The first few seconds of samples are averaged and subtracted from
// subsequent samples for zeroing purposes.
if (!gyros_are_zeroed_) {
- average_gyro_x.AddData(message->gyro_x);
- average_gyro_y.AddData(message->gyro_y);
- average_gyro_z.AddData(message->gyro_z);
+ average_gyro_x.AddData(gyro_x);
+ average_gyro_y.AddData(gyro_y);
+ average_gyro_z.AddData(gyro_z);
if (average_gyro_x.full() && average_gyro_y.full() &&
average_gyro_z.full()) {
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
+ robot_state_fetcher_.Fetch();
+ if (robot_state_fetcher_.get() &&
+ robot_state_fetcher_->outputs_enabled()) {
gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
@@ -282,33 +284,36 @@
}
}
}
+ gyro_x += gyro_x_zeroed_offset_;
+ gyro_y += gyro_y_zeroed_offset_;
+ gyro_z += gyro_z_zeroed_offset_;
- message->gyro_x += gyro_x_zeroed_offset_;
- message->gyro_y += gyro_y_zeroed_offset_;
- message->gyro_z += gyro_z_zeroed_offset_;
+ imu_builder.add_gyro_x(gyro_x);
+ imu_builder.add_gyro_y(gyro_y);
+ imu_builder.add_gyro_z(gyro_z);
- message->accelerometer_x =
- ConvertValue(&to_receive[10], kAccelerometerLsbG);
- message->accelerometer_y =
- ConvertValue(&to_receive[12], kAccelerometerLsbG);
- message->accelerometer_z =
- ConvertValue(&to_receive[14], kAccelerometerLsbG);
+ imu_builder.add_accelerometer_x(
+ ConvertValue(&to_receive[10], kAccelerometerLsbG));
+ imu_builder.add_accelerometer_y(
+ ConvertValue(&to_receive[12], kAccelerometerLsbG));
+ imu_builder.add_accelerometer_z(
+ ConvertValue(&to_receive[14], kAccelerometerLsbG));
- message->magnetometer_x =
- ConvertValue(&to_receive[16], kMagnetometerLsbGauss);
- message->magnetometer_y =
- ConvertValue(&to_receive[18], kMagnetometerLsbGauss);
- message->magnetometer_z =
- ConvertValue(&to_receive[20], kMagnetometerLsbGauss);
+ imu_builder.add_magnetometer_x(
+ ConvertValue(&to_receive[16], kMagnetometerLsbGauss));
+ imu_builder.add_magnetometer_y(
+ ConvertValue(&to_receive[18], kMagnetometerLsbGauss));
+ imu_builder.add_magnetometer_z(
+ ConvertValue(&to_receive[20], kMagnetometerLsbGauss));
- message->barometer =
- ConvertValue(&to_receive[22], kBarometerLsbPascal, false);
+ imu_builder.add_barometer(
+ ConvertValue(&to_receive[22], kBarometerLsbPascal, false));
- message->temperature =
- ConvertValue(&to_receive[24], kTemperatureLsbDegree) + kTemperatureZero;
+ imu_builder.add_temperature(
+ ConvertValue(&to_receive[24], kTemperatureLsbDegree) +
+ kTemperatureZero);
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- if (!message.Send()) {
+ if (!builder.Send(imu_builder.Finish())) {
AOS_LOG(WARNING, "sending queue message failed\n");
}
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 6aeac9d..ce950df 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -11,10 +11,10 @@
#include "frc971/wpilib/ahal/SPI.h"
#undef ERROR
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state.q.h"
-#include "frc971/wpilib/imu.q.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/wpilib/imu_generated.h"
#include "frc971/wpilib/spi_rx_clearer.h"
namespace frc971 {
@@ -90,7 +90,7 @@
bool Initialize();
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
::aos::Sender<::frc971::IMUValues> imu_values_sender_;
// TODO(Brian): This object has no business owning these ones.
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 870e655..b7fe837 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -1,11 +1,11 @@
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 = "logging_queue",
+flatbuffer_cc_library(
+ name = "logging_fbs",
srcs = [
- "logging.q",
+ "logging.fbs",
],
)
@@ -107,10 +107,9 @@
deps = [
":gyro_interface",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//aos/util:phased_loop",
"//frc971/queues:gyro",
@@ -118,10 +117,10 @@
],
)
-queue_library(
- name = "loop_output_handler_test_queue",
+flatbuffer_cc_library(
+ name = "loop_output_handler_test_fbs",
srcs = [
- "loop_output_handler_test.q",
+ "loop_output_handler_test.fbs",
],
)
@@ -135,8 +134,8 @@
],
deps = [
"//aos:init",
- "//aos/events:event-loop",
- "//aos/robot_state",
+ "//aos/events:event_loop",
+ "//aos/robot_state:robot_state_fbs",
"//aos/scoped:scoped_fd",
"//aos/time",
"//aos/util:log_interval",
@@ -150,9 +149,8 @@
],
deps = [
":loop_output_handler",
- ":loop_output_handler_test_queue",
+ ":loop_output_handler_test_fbs",
"//aos/events:simulated_event_loop",
- "//aos/logging:queue_logging",
"//aos/testing:googletest",
"//aos/testing:test_logging",
],
@@ -169,10 +167,10 @@
restricted_to = ["//tools:roborio"],
deps = [
"//aos:init",
- "//aos/events:shm-event-loop",
- "//aos/logging:queue_logging",
+ "//aos/events:shm_event_loop",
+ "//aos/input:driver_station_data",
"//aos/network:team_number",
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
"//third_party:wpilib",
],
)
@@ -187,16 +185,16 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/events:event_loop",
+ "//aos/robot_state:robot_state_fbs",
"//third_party:wpilib",
],
)
-queue_library(
+flatbuffer_cc_library(
name = "pdp_values",
srcs = [
- "pdp_values.q",
+ "pdp_values.fbs",
],
)
@@ -212,8 +210,7 @@
deps = [
":pdp_values",
"//aos:init",
- "//aos/events:event-loop",
- "//aos/logging:queue_logging",
+ "//aos/events:event_loop",
"//aos/util:phased_loop",
"//third_party:wpilib",
],
@@ -230,11 +227,12 @@
],
)
-queue_library(
- name = "imu_queue",
+flatbuffer_cc_library(
+ name = "imu_fbs",
srcs = [
- "imu.q",
+ "imu.fbs",
],
+ gen_reflections = 1,
)
cc_library(
@@ -247,13 +245,12 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- ":imu_queue",
+ ":imu_fbs",
":spi_rx_clearer",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//frc971/zeroing:averager",
"//third_party:wpilib",
@@ -302,11 +299,11 @@
":encoder_and_potentiometer",
":wpilib_interface",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:phased_loop",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
"//third_party:wpilib",
],
)
@@ -324,8 +321,7 @@
":loop_output_handler",
"//aos:math",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//third_party:wpilib",
],
)
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
index 5c6feb8..0afbe20 100644
--- a/frc971/wpilib/drivetrain_writer.cc
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -2,8 +2,7 @@
#include "aos/commonmath.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -11,19 +10,19 @@
namespace wpilib {
void DrivetrainWriter::Write(
- const ::frc971::control_loops::DrivetrainQueue::Output &output) {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, output.left_voltage));
+ const ::frc971::control_loops::drivetrain::Output &output) {
+ left_controller0_->SetSpeed(
+ SafeSpeed(reversed_left0_, output.left_voltage()));
right_controller0_->SetSpeed(
- SafeSpeed(reversed_right0_, output.right_voltage));
+ SafeSpeed(reversed_right0_, output.right_voltage()));
if (left_controller1_) {
left_controller1_->SetSpeed(
- SafeSpeed(reversed_left1_, output.left_voltage));
+ SafeSpeed(reversed_left1_, output.left_voltage()));
}
if (right_controller1_) {
right_controller1_->SetSpeed(
- SafeSpeed(reversed_right1_, output.right_voltage));
+ SafeSpeed(reversed_right1_, output.right_voltage()));
}
}
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index 9397230..9cf9a67 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -3,7 +3,7 @@
#include "aos/commonmath.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -11,12 +11,12 @@
namespace wpilib {
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::DrivetrainQueue::Output> {
+ ::frc971::control_loops::drivetrain::Output> {
public:
DrivetrainWriter(::aos::EventLoop *event_loop)
: ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::DrivetrainQueue::Output>(
- event_loop, ".frc971.control_loops.drivetrain_queue.output") {}
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {}
void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
left_controller0_ = ::std::move(t);
@@ -40,12 +40,11 @@
private:
void Write(
- const ::frc971::control_loops::DrivetrainQueue::Output &output) override;
+ const ::frc971::control_loops::drivetrain::Output &output) override;
void Stop() override;
double SafeSpeed(bool reversed, double voltage) {
- return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) /
- 12.0);
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) / 12.0);
}
::std::unique_ptr<::frc::PWM> left_controller0_, right_controller0_,
diff --git a/frc971/wpilib/encoder_and_potentiometer.cc b/frc971/wpilib/encoder_and_potentiometer.cc
index ac1a4a0..8173d20 100644
--- a/frc971/wpilib/encoder_and_potentiometer.cc
+++ b/frc971/wpilib/encoder_and_potentiometer.cc
@@ -1,7 +1,7 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "aos/init.h"
#include "aos/logging/logging.h"
+#include "aos/realtime.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 3a9f220..f5297c0 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -7,14 +7,13 @@
#include <chrono>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
@@ -25,13 +24,12 @@
GyroSender::GyroSender(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
- ".aos.joystick_state")),
- uid_sender_(event_loop_->MakeSender<::frc971::sensors::Uid>(
- ".frc971.sensors.gyro_part_id")),
+ joystick_state_fetcher_(
+ event_loop_->MakeFetcher<aos::RobotState>("/aos")),
+ uid_sender_(event_loop_->MakeSender<frc971::sensors::Uid>("/drivetrain")),
gyro_reading_sender_(
- event_loop_->MakeSender<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")) {
+ event_loop_->MakeSender<frc971::sensors::GyroReading>(
+ "/drivetrain")) {
AOS_PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
@@ -55,12 +53,9 @@
state_ = State::RUNNING;
AOS_LOG(INFO, "gyro initialized successfully\n");
- {
- auto message = uid_sender_.MakeMessage();
- message->uid = gyro_.ReadPartID();
- AOS_LOG_STRUCT(INFO, "gyro ID", *message);
- message.Send();
- }
+ auto builder = uid_sender_.MakeBuilder();
+ builder.Send(
+ frc971::sensors::CreateUid(*builder.fbb(), gyro_.ReadPartID()));
}
last_initialize_time_ = monotonic_now;
}
@@ -115,30 +110,31 @@
const double angle_rate = gyro_.ExtractAngle(result);
const double new_angle = angle_rate / static_cast<double>(kReadingRate);
- auto message = gyro_reading_sender_.MakeMessage();
+ auto builder = gyro_reading_sender_.MakeBuilder();
if (zeroed_) {
angle_ += (new_angle + zero_offset_) * iterations;
- message->angle = angle_;
- message->velocity = angle_rate + zero_offset_ * kReadingRate;
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- message.Send();
+ sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ gyro_builder.add_angle(angle_);
+ gyro_builder.add_velocity(angle_rate + zero_offset_ * kReadingRate);
+ builder.Send(gyro_builder.Finish());
} else {
// TODO(brian): Don't break without 6 seconds of standing still before
// enabling. Ideas:
// Don't allow driving until we have at least some data?
// Some kind of indicator light?
{
- message->angle = new_angle;
- message->velocity = angle_rate;
- AOS_LOG_STRUCT(DEBUG, "collected while zeroing", *message);
- message->angle = 0.0;
- message->velocity = 0.0;
- message.Send();
+ sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ gyro_builder.add_angle(0.0);
+ gyro_builder.add_velocity(0.0);
+ builder.Send(gyro_builder.Finish());
}
zeroing_data_.AddData(new_angle);
joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
+ if (joystick_state_fetcher_.get() &&
+ joystick_state_fetcher_->outputs_enabled() &&
zeroing_data_.full()) {
zero_offset_ = -zeroing_data_.GetAverage();
AOS_LOG(INFO, "total zero offset %f\n", zero_offset_);
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 3a43391..ec39264 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -5,9 +5,9 @@
#include <atomic>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/gyro_interface.h"
#include "frc971/zeroing/averager.h"
@@ -30,7 +30,7 @@
void Loop(const int iterations);
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> joystick_state_fetcher_;
::aos::Sender<::frc971::sensors::Uid> uid_sender_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
diff --git a/frc971/wpilib/imu.q b/frc971/wpilib/imu.fbs
similarity index 69%
rename from frc971/wpilib/imu.q
rename to frc971/wpilib/imu.fbs
index 4d9dec7..f48f31f 100644
--- a/frc971/wpilib/imu.q
+++ b/frc971/wpilib/imu.fbs
@@ -1,40 +1,42 @@
-package frc971;
+namespace frc971;
// Values returned from an IMU.
// Published on ".frc971.imu_values"
-message IMUValues {
+table IMUValues {
// Gyro readings in radians/second.
// Positive is clockwise looking at the connector.
- float gyro_x;
+ gyro_x:float;
// Positive is clockwise looking at the right side (from the connector).
- float gyro_y;
+ gyro_y:float;
// Positive is counterclockwise looking at the top.
- float gyro_z;
+ gyro_z:float;
// Accelerometer readings in Gs.
// Positive is up.
- float accelerometer_x;
+ accelerometer_x:float;
// Positive is away from the right side (from the connector).
- float accelerometer_y;
+ accelerometer_y:float;
// Positive is away from the connector.
- float accelerometer_z;
+ accelerometer_z:float;
// Magnetometer readings in gauss.
// Positive is up.
- float magnetometer_x;
+ magnetometer_x:float;
// Positive is away from the right side (from the connector).
- float magnetometer_y;
+ magnetometer_y:float;
// Positive is away from the connector.
- float magnetometer_z;
+ magnetometer_z:float;
// Barometer readings in pascals.
- float barometer;
+ barometer:float;
// Temperature readings in degrees Celsius.
- float temperature;
+ temperature:float;
// FPGA timestamp when the values were captured.
- double fpga_timestamp;
+ fpga_timestamp:double;
// CLOCK_MONOTONIC time in nanoseconds when the values were captured.
- int64_t monotonic_timestamp_ns;
-};
+ monotonic_timestamp_ns:long;
+}
+
+root_type IMUValues;
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
index 546e64b..9b63b84 100644
--- a/frc971/wpilib/interrupt_edge_counting.cc
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -2,8 +2,8 @@
#include <chrono>
+#include "aos/realtime.h"
#include "aos/time/time.h"
-#include "aos/init.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 31ecc6c..2fec111 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -1,9 +1,10 @@
#include "frc971/wpilib/joystick_sender.h"
-#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "hal/HAL.h"
@@ -11,12 +12,13 @@
namespace frc971 {
namespace wpilib {
+using aos::Joystick;
+
JoystickSender::JoystickSender(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
joystick_state_sender_(
- event_loop_->MakeSender<::aos::JoystickState>(".aos.joystick_state")),
+ event_loop_->MakeSender<::aos::JoystickState>("/aos")),
team_id_(::aos::network::GetTeamNumber()) {
-
event_loop_->SetRuntimeRealtimePriority(29);
event_loop_->OnRun([this]() {
@@ -27,36 +29,65 @@
// variable / mutex needs to get exposed all the way out or something).
while (event_loop_->is_running()) {
ds->RunIteration([&]() {
- auto new_state = joystick_state_sender_.MakeMessage();
+ auto builder = joystick_state_sender_.MakeBuilder();
HAL_MatchInfo match_info;
auto status = HAL_GetMatchInfo(&match_info);
- if (status == 0) {
- new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l';
- new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l';
- }
- new_state->test_mode = ds->IsTestMode();
- new_state->fms_attached = ds->IsFmsAttached();
- new_state->enabled = ds->IsEnabled();
- new_state->autonomous = ds->IsAutonomous();
- new_state->team_id = team_id_;
- new_state->fake = false;
+ std::array<flatbuffers::Offset<Joystick>,
+ aos::input::driver_station::JoystickFeature::kJoysticks>
+ joysticks;
- for (uint8_t i = 0;
- i < sizeof(new_state->joysticks) / sizeof(::aos::Joystick); ++i) {
- new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 6; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ for (size_t i = 0;
+ i < aos::input::driver_station::JoystickFeature::kJoysticks; ++i) {
+ std::array<double, aos::input::driver_station::JoystickAxis::kAxes>
+ axis;
+ for (int j = 0; j < aos::input::driver_station::JoystickAxis::kAxes;
+ ++j) {
+ axis[j] = ds->GetStickAxis(i, j);
}
+
+ flatbuffers::Offset<flatbuffers::Vector<double>> axis_offset =
+ builder.fbb()->CreateVector(axis.begin(), axis.size());
+
+ Joystick::Builder joystick_builder = builder.MakeBuilder<Joystick>();
+
+ joystick_builder.add_buttons(ds->GetStickButtons(i));
+
if (ds->GetStickPOVCount(i) > 0) {
- new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
+ joystick_builder.add_pov(ds->GetStickPOV(i, 0));
}
- AOS_LOG_STRUCT(DEBUG, "joystick_state", *new_state);
+
+ joystick_builder.add_axis(axis_offset);
+
+ joysticks[i] = joystick_builder.Finish();
}
- if (!new_state.Send()) {
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Joystick>>>
+ joysticks_offset = builder.fbb()->CreateVector(joysticks.begin(),
+ joysticks.size());
+
+ aos::JoystickState::Builder joystick_state_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_state_builder.add_joysticks(joysticks_offset);
+
+ if (status == 0) {
+ joystick_state_builder.add_switch_left(
+ match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l');
+ joystick_state_builder.add_scale_left(
+ match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l');
+ }
+
+ joystick_state_builder.add_test_mode(ds->IsTestMode());
+ joystick_state_builder.add_fms_attached(ds->IsFmsAttached());
+ joystick_state_builder.add_enabled(ds->IsEnabled());
+ joystick_state_builder.add_autonomous(ds->IsAutonomous());
+ joystick_state_builder.add_team_id(team_id_);
+
+ if (!builder.Send(joystick_state_builder.Finish())) {
AOS_LOG(WARNING, "sending joystick_state failed\n");
}
});
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index 34c6bf4..e2609e8 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -3,8 +3,8 @@
#include <atomic>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/logging.fbs b/frc971/wpilib/logging.fbs
new file mode 100644
index 0000000..473526f
--- /dev/null
+++ b/frc971/wpilib/logging.fbs
@@ -0,0 +1,7 @@
+namespace frc971.wpilib;
+
+// Information about the current state of the pneumatics system to log.
+table PneumaticsToLog {
+ compressor_on:bool;
+ read_solenoids:ubyte;
+}
diff --git a/frc971/wpilib/logging.q b/frc971/wpilib/logging.q
deleted file mode 100644
index a2b3799..0000000
--- a/frc971/wpilib/logging.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.wpilib;
-
-// Information about the current state of the pneumatics system to log.
-struct PneumaticsToLog {
- bool compressor_on;
- uint8_t read_solenoids;
-};
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 81baa68..a697339 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -4,8 +4,7 @@
#include <atomic>
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
#include "aos/scoped/scoped_fd.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
diff --git a/frc971/wpilib/loop_output_handler_test.cc b/frc971/wpilib/loop_output_handler_test.cc
index 12c3f96..d7ef8cd 100644
--- a/frc971/wpilib/loop_output_handler_test.cc
+++ b/frc971/wpilib/loop_output_handler_test.cc
@@ -4,12 +4,11 @@
#include "gtest/gtest.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/events/simulated_event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
-#include "frc971/wpilib/loop_output_handler_test.q.h"
+#include "frc971/wpilib/loop_output_handler_test_generated.h"
namespace frc971 {
namespace wpilib {
@@ -23,11 +22,26 @@
public:
LoopOutputHandlerTest()
: ::testing::Test(),
+ configuration_(aos::configuration::MergeConfiguration(
+ aos::FlatbufferDetachedBuffer<aos::Configuration>(
+ aos::JsonToFlatbuffer(
+ "{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/test\",\n"
+ " \"type\": "
+ "\"frc971.wpilib.LoopOutputHandlerTestOutput\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ aos::Configuration::MiniReflectTypeTable())))),
+ event_loop_factory_(&configuration_.message()),
loop_output_hander_event_loop_(event_loop_factory_.MakeEventLoop()),
test_event_loop_(event_loop_factory_.MakeEventLoop()) {
::aos::testing::EnableTestLogging();
}
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
::aos::SimulatedEventLoopFactory event_loop_factory_;
::std::unique_ptr<::aos::EventLoop> loop_output_hander_event_loop_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -49,14 +63,14 @@
protected:
void Write(const LoopOutputHandlerTestOutput &output) override {
- AOS_LOG_STRUCT(DEBUG, "output", output);
+ LOG(INFO) << "output " << aos::FlatbufferToJson(&output);
++count_;
last_time_ = event_loop()->monotonic_now();
}
void Stop() override {
stop_time_ = event_loop()->monotonic_now();
- AOS_LOG(DEBUG, "Stopping\n");
+ LOG(INFO) << "Stopping";
}
private:
@@ -71,10 +85,10 @@
// Test that the watchdog calls Stop at the right time.
TEST_F(LoopOutputHandlerTest, WatchdogTest) {
TestLoopOutputHandler loop_output(loop_output_hander_event_loop_.get(),
- ".test");
+ "/test");
::aos::Sender<LoopOutputHandlerTestOutput> output_sender =
- test_event_loop_->MakeSender<LoopOutputHandlerTestOutput>(".test");
+ test_event_loop_->MakeSender<LoopOutputHandlerTestOutput>("/test");
const monotonic_clock::time_point start_time =
test_event_loop_->monotonic_now();
@@ -86,13 +100,15 @@
EXPECT_EQ(count, loop_output.count());
if (test_event_loop_->monotonic_now() <
start_time + chrono::seconds(1)) {
- auto output = output_sender.MakeMessage();
- output->voltage = 5.0;
- EXPECT_TRUE(output.Send());
+ auto builder = output_sender.MakeBuilder();
+ LoopOutputHandlerTestOutput::Builder output_builder =
+ builder.MakeBuilder<LoopOutputHandlerTestOutput>();
+ output_builder.add_voltage(5.0);
+ EXPECT_TRUE(builder.Send(output_builder.Finish()));
++count;
}
- AOS_LOG(INFO, "Ping\n");
+ LOG(INFO) << "Ping";
});
// Kick off the ping timer handler.
diff --git a/frc971/wpilib/loop_output_handler_test.fbs b/frc971/wpilib/loop_output_handler_test.fbs
new file mode 100644
index 0000000..6cb2cf1
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler_test.fbs
@@ -0,0 +1,6 @@
+namespace frc971.wpilib;
+
+// Test output message.
+table LoopOutputHandlerTestOutput {
+ voltage:double;
+}
diff --git a/frc971/wpilib/loop_output_handler_test.q b/frc971/wpilib/loop_output_handler_test.q
deleted file mode 100644
index 81336ef..0000000
--- a/frc971/wpilib/loop_output_handler_test.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package frc971.wpilib;
-
-// Test output message.
-message LoopOutputHandlerTestOutput {
- double voltage;
-};
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index aa85184..cd17d89 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -2,11 +2,10 @@
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/events/event_loop.h"
+#include "aos/logging/logging.h"
#include "frc971/wpilib/ahal/PowerDistributionPanel.h"
-#include "frc971/wpilib/pdp_values.q.h"
+#include "frc971/wpilib/pdp_values_generated.h"
namespace frc971 {
namespace wpilib {
@@ -15,8 +14,7 @@
PDPFetcher::PDPFetcher(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- pdp_values_sender_(
- event_loop_->MakeSender<::frc971::PDPValues>(".frc971.pdp_values")),
+ pdp_values_sender_(event_loop_->MakeSender<::frc971::PDPValues>("/aos")),
pdp_(new frc::PowerDistributionPanel()) {
event_loop_->set_name("PDPFetcher");
@@ -31,15 +29,22 @@
if (iterations != 1) {
AOS_LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
- auto message = pdp_values_sender_.MakeMessage();
- message->voltage = pdp_->GetVoltage();
- message->temperature = pdp_->GetTemperature();
- message->power = pdp_->GetTotalPower();
- for (int i = 0; i < 16; ++i) {
- message->currents[i] = pdp_->GetCurrent(i);
+ std::array<double, 16> currents;
+ for (size_t i = 0; i < currents.size(); ++i) {
+ currents[i] = pdp_->GetCurrent(i);
}
- AOS_LOG_STRUCT(DEBUG, "got", *message);
- if (!message.Send()) {
+
+ auto builder = pdp_values_sender_.MakeBuilder();
+ flatbuffers::Offset<flatbuffers::Vector<double>> currents_offset =
+ builder.fbb()->CreateVector(currents.begin(), currents.size());
+
+ PDPValues::Builder pdp_builder = builder.MakeBuilder<PDPValues>();
+ pdp_builder.add_voltage(pdp_->GetVoltage());
+ pdp_builder.add_temperature(pdp_->GetTemperature());
+ pdp_builder.add_power(pdp_->GetTotalPower());
+ pdp_builder.add_currents(currents_offset);
+
+ if (!builder.Send(pdp_builder.Finish())) {
AOS_LOG(WARNING, "sending pdp values failed\n");
}
}
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
index fd05d67..d034473 100644
--- a/frc971/wpilib/pdp_fetcher.h
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -4,8 +4,8 @@
#include <atomic>
#include <memory>
-#include "aos/events/event-loop.h"
-#include "frc971/wpilib/pdp_values.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/wpilib/pdp_values_generated.h"
namespace frc {
class PowerDistributionPanel;
diff --git a/frc971/wpilib/pdp_values.fbs b/frc971/wpilib/pdp_values.fbs
new file mode 100644
index 0000000..4db2ade
--- /dev/null
+++ b/frc971/wpilib/pdp_values.fbs
@@ -0,0 +1,13 @@
+namespace frc971;
+
+// Values retrieved from the PDP.
+// Published on ".frc971.pdp_values"
+table PDPValues {
+ voltage:double;
+ temperature:double;
+ power:double;
+ // Array of 16 currents.
+ currents:[double];
+}
+
+root_type PDPValues;
diff --git a/frc971/wpilib/pdp_values.q b/frc971/wpilib/pdp_values.q
deleted file mode 100644
index 4c326ab..0000000
--- a/frc971/wpilib/pdp_values.q
+++ /dev/null
@@ -1,10 +0,0 @@
-package frc971;
-
-// Values retrieved from the PDP.
-// Published on ".frc971.pdp_values"
-message PDPValues {
- double voltage;
- double temperature;
- double power;
- double[16] currents;
-};
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 63d3992..8610105 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -4,7 +4,6 @@
#include <unistd.h>
#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
@@ -124,10 +123,8 @@
event_loop_->monotonic_now();
{
- auto new_state = robot_state_sender_.MakeMessage();
- ::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid_);
- AOS_LOG_STRUCT(DEBUG, "robot_state", *new_state);
- new_state.Send();
+ auto builder = robot_state_sender_.MakeBuilder();
+ builder.Send(::frc971::wpilib::PopulateRobotState(&builder, my_pid_));
}
RunIteration();
if (dma_synchronizer_) {
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 7a63444..ae1d580 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -4,11 +4,11 @@
#include <atomic>
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DriverStation.h"
@@ -63,7 +63,7 @@
// Copies a DMAEncoder to a IndexPosition with the correct unit and direction
// changes.
void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
- ::frc971::IndexPosition *position,
+ ::frc971::IndexPositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -82,7 +82,7 @@
// the correct unit and direction changes.
void CopyPosition(
const ::frc971::wpilib::AbsoluteEncoderAndPotentiometer &encoder,
- ::frc971::PotAndAbsolutePosition *position,
+ ::frc971::PotAndAbsolutePositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
::std::function<double(double)> potentiometer_translate, bool reverse,
double pot_offset) {
@@ -104,7 +104,7 @@
// Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
// and direction changes.
void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
- ::frc971::HallEffectAndPosition *position,
+ ::frc971::HallEffectAndPositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -129,7 +129,7 @@
// and direction changes.
void CopyPosition(
const ::frc971::wpilib::AbsoluteEncoder &encoder,
- ::frc971::AbsolutePosition *position,
+ ::frc971::AbsolutePositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -146,7 +146,7 @@
void CopyPosition(
const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPosition *position,
+ ::frc971::PotAndIndexPositionT *position,
::std::function<double(int32_t)> encoder_translate,
::std::function<double(double)> potentiometer_translate, bool reverse,
double pot_offset) {
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 4216e3b..8772af1 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -1,30 +1,38 @@
#include "frc971/wpilib/wpilib_interface.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/logging/logging.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "hal/HAL.h"
namespace frc971 {
namespace wpilib {
-void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid) {
+flatbuffers::Offset<aos::RobotState> PopulateRobotState(
+ aos::Sender<::aos::RobotState>::Builder *builder, int32_t my_pid) {
int32_t status = 0;
- robot_state->reader_pid = my_pid;
- robot_state->outputs_enabled = HAL_GetSystemActive(&status);
- robot_state->browned_out = HAL_GetBrownedOut(&status);
+ aos::RobotState::Builder robot_state_builder =
+ builder->MakeBuilder<aos::RobotState>();
- robot_state->is_3v3_active = HAL_GetUserActive3V3(&status);
- robot_state->is_5v_active = HAL_GetUserActive5V(&status);
- robot_state->voltage_3v3 = HAL_GetUserVoltage3V3(&status);
- robot_state->voltage_5v = HAL_GetUserVoltage5V(&status);
+ robot_state_builder.add_reader_pid(my_pid);
+ robot_state_builder.add_outputs_enabled(HAL_GetSystemActive(&status));
+ robot_state_builder.add_browned_out(HAL_GetBrownedOut(&status));
- robot_state->voltage_roborio_in = HAL_GetVinVoltage(&status);
- robot_state->voltage_battery = HAL_GetVinVoltage(&status);
+ robot_state_builder.add_is_3v3_active(HAL_GetUserActive3V3(&status));
+ robot_state_builder.add_is_5v_active(HAL_GetUserActive5V(&status));
+ robot_state_builder.add_voltage_3v3(HAL_GetUserVoltage3V3(&status));
+ robot_state_builder.add_voltage_5v(HAL_GetUserVoltage5V(&status));
+
+ robot_state_builder.add_voltage_roborio_in(HAL_GetVinVoltage(&status));
+ robot_state_builder.add_voltage_battery(HAL_GetVinVoltage(&status));
if (status != 0) {
AOS_LOG(FATAL, "Failed to get robot state: %d\n", status);
}
+
+ return robot_state_builder.Finish();
}
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 5db852a..104ab84 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -3,13 +3,15 @@
#include <stdint.h>
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
namespace frc971 {
namespace wpilib {
// Sends out a message on ::aos::robot_state.
-void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid);
+flatbuffers::Offset<aos::RobotState> PopulateRobotState(
+ aos::Sender<::aos::RobotState>::Builder *builder, int32_t my_pid);
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index 86672c4..978c48e 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -1,8 +1,9 @@
#ifndef FRC971_WPILIB_NEWROBOTBASE_H_
#define FRC971_WPILIB_NEWROBOTBASE_H_
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "aos/logging/logging.h"
#include "frc971/wpilib/ahal/RobotBase.h"
namespace frc971 {
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index db5507b..fe95bdc 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -1,6 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
@@ -31,8 +30,10 @@
],
deps = [
":wrap",
+ "//aos/logging",
"//frc971:constants",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
+ "@com_github_google_glog//:glog",
],
)
@@ -44,11 +45,10 @@
deps = [
":zeroing",
"//aos:die",
- "//aos/util:thread",
"//aos/testing:googletest",
"//aos/testing:test_shm",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:position_sensor_sim",
- "//frc971/control_loops:queues",
],
)
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 1399efd..f0aab23 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -8,6 +8,9 @@
#include "frc971/zeroing/wrap.h"
+#include "flatbuffers/flatbuffers.h"
+#include "glog/logging.h"
+
namespace frc971 {
namespace zeroing {
@@ -30,7 +33,7 @@
void PotAndIndexPulseZeroingEstimator::TriggerError() {
if (!error_) {
- AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
+ VLOG(1) << "Manually triggered zeroing error.";
error_ = true;
}
}
@@ -57,14 +60,14 @@
// reset and wait for that count to change before we consider ourselves
// zeroed.
if (wait_for_index_pulse_) {
- last_used_index_pulse_count_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses();
wait_for_index_pulse_ = false;
}
if (start_pos_samples_.size() < constants_.average_filter_size) {
- start_pos_samples_.push_back(info.pot - info.encoder);
+ start_pos_samples_.push_back(info.pot() - info.encoder());
} else {
- start_pos_samples_[samples_idx_] = info.pot - info.encoder;
+ start_pos_samples_[samples_idx_] = info.pot() - info.encoder();
}
// Drop the oldest sample when we run this function the next time around.
@@ -83,14 +86,14 @@
// have a well-filtered starting position then we use the filtered value as
// our best guess.
if (!zeroed_ &&
- (info.index_pulses == last_used_index_pulse_count_ || !offset_ready())) {
+ (info.index_pulses() == last_used_index_pulse_count_ || !offset_ready())) {
offset_ = start_average;
- } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses) {
+ } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses()) {
// Note the accurate start position and the current index pulse count so
// that we only run this logic once per index pulse. That should be more
// resilient to corrupted intermediate data.
- offset_ = CalculateStartPosition(start_average, info.latched_encoder);
- last_used_index_pulse_count_ = info.index_pulses;
+ offset_ = CalculateStartPosition(start_average, info.latched_encoder());
+ last_used_index_pulse_count_ = info.index_pulses();
// TODO(austin): Reject encoder positions which have x% error rather than
// rounding to the closest index pulse.
@@ -98,7 +101,7 @@
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
+ VLOG(2) << "latching start position" << first_start_pos_;
}
// Now that we have an accurate starting position we can consider ourselves
@@ -109,29 +112,30 @@
if (::std::abs(first_start_pos_ - offset_) >
constants_.allowable_encoder_error * constants_.index_difference) {
if (!error_) {
- AOS_LOG(
- ERROR,
- "Encoder ticks out of range since last index pulse. first start "
- "position: %f recent starting position: %f, allowable error: %f\n",
- first_start_pos_, offset_,
- constants_.allowable_encoder_error * constants_.index_difference);
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. first start "
+ "position: "
+ << first_start_pos_ << " recent starting position: " << offset_
+ << ", allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
error_ = true;
}
}
}
- position_ = offset_ + info.encoder;
- filtered_position_ = start_average + info.encoder;
+ position_ = offset_ + info.encoder();
+ filtered_position_ = start_average + info.encoder();
}
-PotAndIndexPulseZeroingEstimator::State
-PotAndIndexPulseZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.pot_position = filtered_position_;
- return r;
+flatbuffers::Offset<PotAndIndexPulseZeroingEstimator::State>
+PotAndIndexPulseZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ return builder.Finish();
}
HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
@@ -157,7 +161,7 @@
void HallEffectAndPositionZeroingEstimator::TriggerError() {
if (!error_) {
- AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
+ VLOG(1) << "Manually triggered zeroing error.\n";
error_ = true;
}
}
@@ -165,15 +169,15 @@
void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
const HallEffectAndPosition &info) {
// If we have a new posedge.
- if (!info.current) {
+ if (!info.current()) {
if (last_hall_) {
- min_low_position_ = max_low_position_ = info.encoder;
+ min_low_position_ = max_low_position_ = info.encoder();
} else {
- min_low_position_ = ::std::min(min_low_position_, info.encoder);
- max_low_position_ = ::std::max(max_low_position_, info.encoder);
+ min_low_position_ = ::std::min(min_low_position_, info.encoder());
+ max_low_position_ = ::std::max(max_low_position_, info.encoder());
}
}
- last_hall_ = info.current;
+ last_hall_ = info.current();
}
void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
@@ -183,49 +187,49 @@
// that count to change and for the hall effect to stay high before we
// consider ourselves zeroed.
if (!initialized_) {
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
initialized_ = true;
- last_hall_ = info.current;
+ last_hall_ = info.current();
}
StoreEncoderMaxAndMin(info);
- if (info.current) {
+ if (info.current()) {
cycles_high_++;
} else {
cycles_high_ = 0;
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
}
high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
bool moving_backward = false;
if (constants_.zeroing_move_direction) {
- moving_backward = info.encoder > min_low_position_;
+ moving_backward = info.encoder() > min_low_position_;
} else {
- moving_backward = info.encoder < max_low_position_;
+ moving_backward = info.encoder() < max_low_position_;
}
// If there are no posedges to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (last_used_posedge_count_ != info.posedge_count && high_long_enough_ &&
+ if (last_used_posedge_count_ != info.posedge_count() && high_long_enough_ &&
moving_backward) {
// Note the offset and the current posedge count so that we only run this
// logic once per posedge. That should be more resilient to corrupted
// intermediate data.
- offset_ = -info.posedge_value;
+ offset_ = -info.posedge_value();
if (constants_.zeroing_move_direction) {
offset_ += constants_.lower_hall_position;
} else {
offset_ += constants_.upper_hall_position;
}
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
+ VLOG(2) << "latching start position" << first_start_pos_;
}
// Now that we have an accurate starting position we can consider ourselves
@@ -233,18 +237,19 @@
zeroed_ = true;
}
- position_ = info.encoder - offset_;
+ position_ = info.encoder() - offset_;
}
-HallEffectAndPositionZeroingEstimator::State
-HallEffectAndPositionZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.encoder = position_;
- r.high_long_enough = high_long_enough_;
- r.offset = offset_;
- return r;
+flatbuffers::Offset<HallEffectAndPositionZeroingEstimator::State>
+HallEffectAndPositionZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_encoder(position_);
+ builder.add_high_long_enough(high_long_enough_);
+ builder.add_offset(offset_);
+ return builder.Finish();
}
PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
@@ -291,23 +296,22 @@
const PotAndAbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
- if (::std::isnan(info.absolute_encoder)) {
+ if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
- AOS_LOG(ERROR, "NAN on absolute encoder\n");
+ VLOG(1) << "NAN on absolute encoder.";
error_ = true;
} else {
++nan_samples_;
- AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ VLOG(1) << "NAN on absolute encoder while zeroing" << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
}
}
// Throw some dummy values in for now.
- filtered_absolute_encoder_ = info.absolute_encoder;
- filtered_position_ = pot_relative_encoder_offset_ + info.encoder;
- position_ = offset_ + info.encoder;
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ filtered_position_ = pot_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
return;
}
@@ -315,7 +319,7 @@
constants_.zeroing_threshold);
if (!moving) {
- const PotAndAbsolutePosition &sample = move_detector_.GetSample();
+ const PositionStruct &sample = move_detector_.GetSample();
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
@@ -402,13 +406,10 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- AOS_LOG(
- ERROR,
- "Offset moved too far. Initial: %f, current %f, allowable change: "
- "%f\n",
- first_offset_, offset_,
- constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
error_ = true;
}
@@ -417,19 +418,20 @@
}
// Update the position.
- filtered_position_ = pot_relative_encoder_offset_ + info.encoder;
- position_ = offset_ + info.encoder;
+ filtered_position_ = pot_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
}
-PotAndAbsoluteEncoderZeroingEstimator::State
-PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.pot_position = filtered_position_;
- r.absolute_position = filtered_absolute_encoder_;
- return r;
+flatbuffers::Offset<PotAndAbsoluteEncoderZeroingEstimator::State>
+PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ return builder.Finish();
}
void PulseIndexZeroingEstimator::Reset() {
@@ -444,16 +446,16 @@
void PulseIndexZeroingEstimator::StoreIndexPulseMaxAndMin(
const IndexPosition &info) {
// If we have a new index pulse.
- if (last_used_index_pulse_count_ != info.index_pulses) {
+ if (last_used_index_pulse_count_ != info.index_pulses()) {
// If the latest pulses's position is outside the range we've currently
// seen, record it appropriately.
- if (info.latched_encoder > max_index_position_) {
- max_index_position_ = info.latched_encoder;
+ if (info.latched_encoder() > max_index_position_) {
+ max_index_position_ = info.latched_encoder();
}
- if (info.latched_encoder < min_index_position_) {
- min_index_position_ = info.latched_encoder;
+ if (info.latched_encoder() < min_index_position_) {
+ min_index_position_ = info.latched_encoder();
}
- last_used_index_pulse_count_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses();
}
}
@@ -474,9 +476,9 @@
const int index_pulse_count = IndexPulseCount();
if (index_pulse_count > constants_.index_pulse_count) {
if (!error_) {
- AOS_LOG(ERROR,
- "Got more index pulses than expected. Got %d expected %d.\n",
- index_pulse_count, constants_.index_pulse_count);
+ VLOG(1) << "Got more index pulses than expected. Got "
+ << index_pulse_count << " expected "
+ << constants_.index_pulse_count;
error_ = true;
}
}
@@ -493,7 +495,7 @@
// Detect whether the index pulse is somewhere other than where we expect
// it to be. First we compute the position of the most recent index pulse.
double index_pulse_distance =
- info.latched_encoder + offset_ - constants_.measured_index_position;
+ info.latched_encoder() + offset_ - constants_.measured_index_position;
// Second we compute the position of the index pulse in terms of
// the index difference. I.e. if this index pulse is two pulses away from
// the index pulse that we know about then this number should be positive
@@ -506,32 +508,34 @@
// This lets us check if the index pulse is within an acceptable error
// margin of where we expected it to be.
if (::std::abs(error) > constants_.allowable_encoder_error) {
- AOS_LOG(ERROR,
- "Encoder ticks out of range since last index pulse. known index "
- "pulse: %f, expected index pulse: %f, actual index pulse: %f, "
- "allowable error: %f\n",
- constants_.measured_index_position,
- round(relative_distance) * constants_.index_difference +
- constants_.measured_index_position,
- info.latched_encoder + offset_,
- constants_.allowable_encoder_error * constants_.index_difference);
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. known index "
+ "pulse: "
+ << constants_.measured_index_position << ", expected index pulse: "
+ << round(relative_distance) * constants_.index_difference +
+ constants_.measured_index_position
+ << ", actual index pulse: " << info.latched_encoder() + offset_
+ << ", "
+ "allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
error_ = true;
}
}
- position_ = info.encoder + offset_;
+ position_ = info.encoder() + offset_;
}
-PulseIndexZeroingEstimator::State
-PulseIndexZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.min_index_position = min_index_position_;
- r.max_index_position = max_index_position_;
- r.index_pulses_seen = IndexPulseCount();
- return r;
+flatbuffers::Offset<PulseIndexZeroingEstimator::State>
+PulseIndexZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_min_index_position(min_index_position_);
+ builder.add_max_index_position(max_index_position_);
+ builder.add_index_pulses_seen(IndexPulseCount());
+ return builder.Finish();
}
AbsoluteEncoderZeroingEstimator::AbsoluteEncoderZeroingEstimator(
@@ -570,22 +574,21 @@
const AbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
- if (::std::isnan(info.absolute_encoder)) {
+ if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
- AOS_LOG(ERROR, "NAN on absolute encoder\n");
+ VLOG(1) << "NAN on absolute encoder.";
error_ = true;
} else {
++nan_samples_;
- AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
}
}
// Throw some dummy values in for now.
- filtered_absolute_encoder_ = info.absolute_encoder;
- position_ = offset_ + info.encoder;
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ position_ = offset_ + info.encoder();
return;
}
@@ -593,7 +596,7 @@
constants_.zeroing_threshold);
if (!moving) {
- const AbsolutePosition &sample = move_detector_.GetSample();
+ const PositionStruct &sample = move_detector_.GetSample();
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
@@ -673,13 +676,10 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- AOS_LOG(
- ERROR,
- "Offset moved too far. Initial: %f, current %f, allowable change: "
- "%f\n",
- first_offset_, offset_,
- constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
error_ = true;
}
@@ -688,17 +688,18 @@
}
// Update the position.
- position_ = offset_ + info.encoder;
+ position_ = offset_ + info.encoder();
}
-AbsoluteEncoderZeroingEstimator::State
- AbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.absolute_position = filtered_absolute_encoder_;
- return r;
+flatbuffers::Offset<AbsoluteEncoderZeroingEstimator::State>
+AbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ return builder.Finish();
}
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index e0c6169..1c6bb77 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -6,9 +6,11 @@
#include <cstdint>
#include <vector>
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/constants.h"
+#include "flatbuffers/flatbuffers.h"
+
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
//
@@ -53,7 +55,8 @@
virtual void UpdateEstimate(const Position &) = 0;
// Returns the state of the estimator
- virtual State GetEstimatorState() const = 0;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const = 0;
};
// Estimates the position with an incremental encoder with an index pulse and a
@@ -98,7 +101,8 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
// This function calculates the start position given the internal state and
@@ -168,7 +172,8 @@
bool offset_ready() const override { return zeroed_; }
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
// Sets the minimum and maximum posedge position values.
@@ -217,7 +222,7 @@
// Class to encapsulate the logic to decide when we are moving and which samples
// are safe to use.
-template <typename Position>
+template <typename Position, typename PositionBuffer>
class MoveDetector {
public:
MoveDetector(size_t filter_size) {
@@ -235,9 +240,10 @@
// buffer_size is the number of samples in the moving buffer, and
// zeroing_threshold is the max amount we can move within the period specified
// by buffer_size.
- bool Update(const Position &position, size_t buffer_size,
+ bool Update(const PositionBuffer &position_buffer, size_t buffer_size,
double zeroing_threshold) {
bool moving = true;
+ Position position(position_buffer);
if (buffered_samples_.size() < buffer_size) {
// Not enough samples to start determining if the robot is moving or not,
// don't use the samples yet.
@@ -312,9 +318,20 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
+ struct PositionStruct {
+ PositionStruct(const PotAndAbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()),
+ pot(position_buffer.pot()) {}
+ double absolute_encoder;
+ double encoder;
+ double pot;
+ };
+
// The zeroing constants used to describe the configuration of the system.
const constants::PotAndAbsoluteEncoderZeroingConstants constants_;
@@ -335,7 +352,7 @@
// Offset between the Pot and Relative encoder position.
::std::vector<double> offset_samples_;
- MoveDetector<PotAndAbsolutePosition> move_detector_;
+ MoveDetector<PositionStruct, PotAndAbsolutePosition> move_detector_;
// Estimated offset between the pot and relative encoder.
double pot_relative_encoder_offset_ = 0;
@@ -380,7 +397,8 @@
void UpdateEstimate(const IndexPosition &info) override;
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
void TriggerError() override { error_ = true; }
@@ -453,9 +471,18 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
+ struct PositionStruct {
+ PositionStruct(const AbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double absolute_encoder;
+ double encoder;
+ };
+
// The zeroing constants used to describe the configuration of the system.
const constants::AbsoluteEncoderZeroingConstants constants_;
@@ -474,7 +501,7 @@
// absolute encoder.
::std::vector<double> relative_to_absolute_offset_samples_;
- MoveDetector<AbsolutePosition> move_detector_;
+ MoveDetector<PositionStruct, AbsolutePosition> move_detector_;
// Estimated start position of the mechanism
double offset_ = 0;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index cca0585..61d2d0c 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -5,21 +5,20 @@
#include <random>
#include "gtest/gtest.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/zeroing/zeroing.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "aos/testing/test_shm.h"
-#include "aos/util/thread.h"
#include "aos/die.h"
#include "frc971/control_loops/position_sensor_sim.h"
namespace frc971 {
namespace zeroing {
-using control_loops::PositionSensorSimulator;
using constants::AbsoluteEncoderZeroingConstants;
using constants::EncoderPlusIndexZeroingConstants;
using constants::PotAndAbsoluteEncoderZeroingConstants;
using constants::PotAndIndexPulseZeroingConstants;
+using control_loops::PositionSensorSimulator;
+using FBB = flatbuffers::FlatBufferBuilder;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -28,53 +27,58 @@
class ZeroingTest : public ::testing::Test {
protected:
- void SetUp() override { aos::SetDieTestMode(true); }
+ void SetUp() override {}
void MoveTo(PositionSensorSimulator *simulator,
PotAndIndexPulseZeroingEstimator *estimator,
double new_position) {
- PotAndIndexPosition sensor_values;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values);
- estimator->UpdateEstimate(sensor_values);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndIndexPosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
- AbsoluteEncoderZeroingEstimator *estimator,
- double new_position) {
- AbsolutePosition sensor_values_;
+ AbsoluteEncoderZeroingEstimator *estimator, double new_position) {
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
PotAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
- PotAndAbsolutePosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndAbsolutePosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
PulseIndexZeroingEstimator *estimator, double new_position) {
- IndexPosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<IndexPosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
HallEffectAndPositionZeroingEstimator *estimator,
double new_position) {
- HallEffectAndPosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
}
- ::aos::testing::TestSharedMemory my_shm_;
+ template <typename T>
+ double GetEstimatorPosition(T *estimator) {
+ FBB fbb;
+ fbb.Finish(estimator->GetEstimatorState(&fbb));
+ return flatbuffers::GetRoot<typename T::State>(fbb.GetBufferPointer())
+ ->position();
+ }
};
TEST_F(ZeroingTest, TestMovingAverageFilter) {
@@ -90,13 +94,13 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.3 * index_diff);
}
- ASSERT_NEAR(3.3 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.3 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.9 * index_diff);
}
- ASSERT_NEAR(3.9 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.9 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
}
@@ -133,25 +137,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.6);
}
- ASSERT_NEAR(3.6, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.6, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
}
TEST_F(ZeroingTest, TestDifferentIndexDiffs) {
@@ -167,25 +171,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.5 * index_diff);
}
- ASSERT_NEAR(3.5 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.5 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
}
TEST_F(ZeroingTest, TestPercentage) {
@@ -278,12 +282,12 @@
MoveTo(&sim, &estimator, 3.7 * index_diff);
ASSERT_TRUE(estimator.zeroed());
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(3.7 * index_diff, estimator.GetEstimatorState().position);
+ ASSERT_DOUBLE_EQ(3.7 * index_diff, GetEstimatorPosition(&estimator));
// Trigger one more index pulse and check the offset.
MoveTo(&sim, &estimator, 4.7 * index_diff);
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.GetEstimatorState().position);
+ ASSERT_DOUBLE_EQ(4.7 * index_diff, GetEstimatorPosition(&estimator));
}
TEST_F(ZeroingTest, BasicErrorAPITest) {
@@ -379,12 +383,12 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- PotAndAbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- sensor_values.pot = 0.0;
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(
+ *flatbuffers::GetRoot<PotAndAbsolutePosition>(fbb.GetBufferPointer()));
}
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
@@ -431,17 +435,17 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
- PotAndAbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- sensor_values.pot = 0.0;
- // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ const auto sensor_values =
+ flatbuffers::GetRoot<PotAndAbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
ASSERT_FALSE(estimator.error());
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
}
@@ -493,11 +497,11 @@
ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
ASSERT_DOUBLE_EQ(3.5 * constants.index_difference,
- estimator.GetEstimatorState().position);
+ GetEstimatorPosition(&estimator));
MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
ASSERT_DOUBLE_EQ(0.5 * constants.index_difference,
- estimator.GetEstimatorState().position);
+ GetEstimatorPosition(&estimator));
}
// Tests that we can detect when an index pulse occurs where we didn't expect
@@ -716,11 +720,13 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- AbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
+ FBB fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
@@ -769,16 +775,17 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
- AbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
ASSERT_FALSE(estimator.error());
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
}
diff --git a/motors/BUILD b/motors/BUILD
index d7dc456..8fe5ff6 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -131,6 +131,7 @@
srcs = [
"simple_receiver.cc",
],
+ copts = ["-Wno-type-limits"],
restricted_to = mcu_cpus,
deps = [
":util",
@@ -153,6 +154,7 @@
srcs = [
"simpler_receiver.cc",
],
+ copts = ["-Wno-type-limits"],
restricted_to = mcu_cpus,
deps = [
":util",
diff --git a/motors/big/BUILD b/motors/big/BUILD
index 8ce81f1..062828f 100644
--- a/motors/big/BUILD
+++ b/motors/big/BUILD
@@ -36,6 +36,6 @@
"//motors:math",
"//motors:motor",
"//motors/peripheral:configuration",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index 673e1a7..5d25c41 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -76,7 +76,7 @@
"//motors:math",
"//motors:motor",
"//motors/peripheral:configuration",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/motors/pistol_grip/BUILD b/motors/pistol_grip/BUILD
index 1946c55..d00404e 100644
--- a/motors/pistol_grip/BUILD
+++ b/motors/pistol_grip/BUILD
@@ -102,6 +102,6 @@
"//motors:math",
"//motors:motor",
"//motors/peripheral:configuration",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 0470c31..5f3af0a 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -22,11 +22,10 @@
namespace motors {
namespace {
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-using ::frc971::control_loops::drivetrain::PolyDrivetrain;
using ::frc971::constants::ShifterHallEffect;
-using ::frc971::control_loops::DrivetrainQueue_Goal;
-using ::frc971::control_loops::DrivetrainQueue_Output;
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::OutputT;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
using ::motors::seems_reasonable::Spring;
namespace chrono = ::std::chrono;
@@ -547,20 +546,20 @@
(::std::abs(convert_input_width(4)) < 0.5f);
if (polydrivetrain != nullptr && spring != nullptr) {
- DrivetrainQueue_Goal goal;
- goal.control_loop_driving = false;
+ float throttle;
+ float wheel;
if (lost_drive_channel) {
- goal.throttle = 0.0f;
- goal.wheel = 0.0f;
+ throttle = 0.0f;
+ wheel = 0.0f;
} else {
- goal.throttle = convert_input_width(1);
- goal.wheel = -convert_input_width(0);
+ throttle = convert_input_width(1);
+ wheel = -convert_input_width(0);
}
- goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+ const bool quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
- DrivetrainQueue_Output output;
+ OutputT output;
- polydrivetrain->SetGoal(goal);
+ polydrivetrain->SetGoal(wheel, throttle, quickturn, false);
polydrivetrain->Update(12.0f);
polydrivetrain->SetOutput(&output);
diff --git a/motors/simpler_receiver.cc b/motors/simpler_receiver.cc
index 71d7e73..c3fcec9 100644
--- a/motors/simpler_receiver.cc
+++ b/motors/simpler_receiver.cc
@@ -20,11 +20,10 @@
namespace motors {
namespace {
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-using ::frc971::control_loops::drivetrain::PolyDrivetrain;
using ::frc971::constants::ShifterHallEffect;
-using ::frc971::control_loops::DrivetrainQueue_Goal;
-using ::frc971::control_loops::DrivetrainQueue_Output;
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::OutputT;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
namespace chrono = ::std::chrono;
@@ -286,29 +285,29 @@
}
if (polydrivetrain != nullptr) {
- DrivetrainQueue_Goal goal;
- goal.control_loop_driving = false;
+ float throttle;
+ float wheel;
if (lost_drive_channel) {
- goal.throttle = 0.0f;
- goal.wheel = 0.0f;
+ throttle = 0.0f;
+ wheel = 0.0f;
} else {
- goal.throttle = convert_input_width(1);
- goal.wheel = -convert_input_width(3);
+ throttle = convert_input_width(1);
+ wheel = -convert_input_width(3);
}
- goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+ const bool quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
if (false) {
static int count = 0;
if (++count == 50) {
count = 0;
- printf("throttle: %d wheel: %d\n", (int)(goal.throttle * 100),
- (int)(goal.wheel * 100));
+ printf("throttle: %d wheel: %d\n", (int)(throttle * 100),
+ (int)(wheel * 100));
}
}
- DrivetrainQueue_Output output;
+ OutputT output;
- polydrivetrain->SetGoal(goal);
+ polydrivetrain->SetGoal(wheel, throttle, quickturn, false);
polydrivetrain->Update(12.0f);
polydrivetrain->SetOutput(&output);
diff --git a/third_party/ceres/BUILD b/third_party/ceres/BUILD
index 8bc1dde..2a4028a 100644
--- a/third_party/ceres/BUILD
+++ b/third_party/ceres/BUILD
@@ -179,7 +179,7 @@
TEST_DEPS = [
"//:ceres",
"//:test_util",
- "@//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
"@com_github_gflags_gflags//:gflags",
]
diff --git a/third_party/ceres/bazel/ceres.bzl b/third_party/ceres/bazel/ceres.bzl
index b9bedf5..d99046a 100644
--- a/third_party/ceres/bazel/ceres.bzl
+++ b/third_party/ceres/bazel/ceres.bzl
@@ -209,7 +209,7 @@
],
visibility = ["//visibility:public"],
deps = [
- "@//third_party/eigen",
"@com_github_google_glog//:glog",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/third_party/ceres/examples/BUILD b/third_party/ceres/examples/BUILD
index 1986932..80d9a67 100644
--- a/third_party/ceres/examples/BUILD
+++ b/third_party/ceres/examples/BUILD
@@ -37,7 +37,7 @@
EXAMPLE_DEPS = [
"//:ceres",
- "@//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
"@com_github_gflags_gflags//:gflags",
]
diff --git a/third_party/ct/BUILD b/third_party/ct/BUILD
index 97e8111..ad7ce4b 100644
--- a/third_party/ct/BUILD
+++ b/third_party/ct/BUILD
@@ -275,7 +275,7 @@
"//third_party/boostorg/config",
"//third_party/boostorg/odeint",
"//third_party/boostorg/property_tree",
- "//third_party/eigen",
"//third_party/hpipm",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/third_party/eigen/BUILD b/third_party/eigen/BUILD
index e66382c..54c5cd1 100644
--- a/third_party/eigen/BUILD
+++ b/third_party/eigen/BUILD
@@ -1,6 +1,6 @@
licenses(["notice"])
-load("//tools:environments.bzl", "mcu_cpus")
+load("@//tools:environments.bzl", "mcu_cpus")
cc_library(
name = "eigen",
diff --git a/third_party/eigen/WORKSPACE b/third_party/eigen/WORKSPACE
new file mode 100644
index 0000000..6f155bf
--- /dev/null
+++ b/third_party/eigen/WORKSPACE
@@ -0,0 +1 @@
+workspace(name = "org_tuxfamily_eigen")
diff --git a/third_party/flatbuffers/BUILD b/third_party/flatbuffers/BUILD
index 17f5b8c..67a3a14 100644
--- a/third_party/flatbuffers/BUILD
+++ b/third_party/flatbuffers/BUILD
@@ -1,5 +1,7 @@
licenses(["notice"])
+load("@//tools:environments.bzl", "mcu_cpus")
+
package(
default_visibility = ["//visibility:public"],
features = [
@@ -100,6 +102,9 @@
"src/idl_gen_text.cpp",
"src/util.cpp",
],
+ data = [
+ "reflection/reflection.fbs",
+ ],
includes = [
"grpc/",
"include/",
@@ -118,6 +123,7 @@
"include/flatbuffers/stl_emulation.h",
"include/flatbuffers/util.h",
],
+ compatible_with = mcu_cpus,
includes = ["include/"],
linkstatic = 1,
)
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index a92f0ed..743c666 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -11,15 +11,18 @@
"./",
"$(GENDIR)",
"$(BINDIR)",
+ "$(execpath @com_github_google_flatbuffers//:flatc).runfiles/com_github_google_flatbuffers",
]
DEFAULT_FLATC_ARGS = [
"--gen-object-api",
"--gen-compare",
- "--no-includes",
+ "--keep-prefix",
"--gen-mutable",
"--reflect-names",
"--cpp-ptr-type flatbuffers::unique_ptr",
+ "--force-empty",
+ "--gen-name-strings",
]
def flatbuffer_library_public(
@@ -33,6 +36,7 @@
flatc_args = DEFAULT_FLATC_ARGS,
reflection_name = "",
reflection_visibility = None,
+ compatible_with = None,
output_to_bindir = False):
"""Generates code files for reading/writing the given flatbuffers in the requested language using the public compiler.
@@ -83,6 +87,7 @@
tools = [flatc_path],
cmd = genrule_cmd,
message = "Generating flatbuffer files for %s:" % (name),
+ compatible_with = compatible_with,
)
if reflection_name:
reflection_genrule_cmd = " ".join([
@@ -109,20 +114,20 @@
tools = [flatc_path],
cmd = reflection_genrule_cmd,
message = "Generating flatbuffer reflection binary for %s:" % (name),
+ compatible_with = compatible_with,
)
- native.Fileset(
- name = reflection_name,
- out = "%s_out" % reflection_name,
- entries = [
- native.FilesetEntry(files = reflection_outs),
- ],
+ native.filegroup(
+ name = "%s_out" % reflection_name,
+ srcs = reflection_outs,
visibility = reflection_visibility,
+ compatible_with = compatible_with,
)
def flatbuffer_cc_library(
name,
srcs,
srcs_filegroup_name = "",
+ compatible_with = None,
out_prefix = "",
includes = [],
include_paths = DEFAULT_INCLUDE_PATHS,
@@ -208,6 +213,7 @@
flatc_args = flatc_args,
reflection_name = reflection_name,
reflection_visibility = visibility,
+ compatible_with = compatible_with,
)
native.cc_library(
name = name,
@@ -226,6 +232,7 @@
includes = [],
linkstatic = 1,
visibility = visibility,
+ compatible_with = compatible_with,
)
# A filegroup for the `srcs`. That is, all the schema files for this
@@ -234,4 +241,5 @@
name = srcs_filegroup_name if srcs_filegroup_name else "%s_includes" % (name),
srcs = srcs,
visibility = srcs_filegroup_visibility if srcs_filegroup_visibility != None else visibility,
+ compatible_with = compatible_with,
)
diff --git a/third_party/flatbuffers/include/flatbuffers/reflection_generated.h b/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
index 0e73a0f..e80b0c9 100644
--- a/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
+++ b/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
@@ -9,22 +9,58 @@
namespace reflection {
struct Type;
+struct TypeBuilder;
+struct TypeT;
struct KeyValue;
+struct KeyValueBuilder;
+struct KeyValueT;
struct EnumVal;
+struct EnumValBuilder;
+struct EnumValT;
struct Enum;
+struct EnumBuilder;
+struct EnumT;
struct Field;
+struct FieldBuilder;
+struct FieldT;
struct Object;
+struct ObjectBuilder;
+struct ObjectT;
struct RPCCall;
+struct RPCCallBuilder;
+struct RPCCallT;
struct Service;
+struct ServiceBuilder;
+struct ServiceT;
struct Schema;
+struct SchemaBuilder;
+struct SchemaT;
+
+inline const flatbuffers::TypeTable *TypeTypeTable();
+
+inline const flatbuffers::TypeTable *KeyValueTypeTable();
+
+inline const flatbuffers::TypeTable *EnumValTypeTable();
+
+inline const flatbuffers::TypeTable *EnumTypeTable();
+
+inline const flatbuffers::TypeTable *FieldTypeTable();
+
+inline const flatbuffers::TypeTable *ObjectTypeTable();
+
+inline const flatbuffers::TypeTable *RPCCallTypeTable();
+
+inline const flatbuffers::TypeTable *ServiceTypeTable();
+
+inline const flatbuffers::TypeTable *SchemaTypeTable();
enum BaseType {
None = 0,
@@ -102,7 +138,26 @@
return EnumNamesBaseType()[index];
}
+struct TypeT : public flatbuffers::NativeTable {
+ typedef Type TableType;
+ reflection::BaseType base_type;
+ reflection::BaseType element;
+ int32_t index;
+ uint16_t fixed_length;
+ TypeT()
+ : base_type(reflection::None),
+ element(reflection::None),
+ index(-1),
+ fixed_length(0) {
+ }
+};
+
struct Type FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef TypeT NativeTableType;
+ typedef TypeBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return TypeTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_BASE_TYPE = 4,
VT_ELEMENT = 6,
@@ -129,9 +184,13 @@
VerifyField<uint16_t>(verifier, VT_FIXED_LENGTH) &&
verifier.EndTable();
}
+ TypeT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(TypeT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Type> Pack(flatbuffers::FlatBufferBuilder &_fbb, const TypeT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct TypeBuilder {
+ typedef Type Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_base_type(reflection::BaseType base_type) {
@@ -172,7 +231,22 @@
return builder_.Finish();
}
+flatbuffers::Offset<Type> CreateType(flatbuffers::FlatBufferBuilder &_fbb, const TypeT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct KeyValueT : public flatbuffers::NativeTable {
+ typedef KeyValue TableType;
+ std::string key;
+ std::string value;
+ KeyValueT() {
+ }
+};
+
struct KeyValue FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef KeyValueT NativeTableType;
+ typedef KeyValueBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return KeyValueTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_KEY = 4,
VT_VALUE = 6
@@ -197,9 +271,13 @@
verifier.VerifyString(value()) &&
verifier.EndTable();
}
+ KeyValueT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(KeyValueT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<KeyValue> Pack(flatbuffers::FlatBufferBuilder &_fbb, const KeyValueT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct KeyValueBuilder {
+ typedef KeyValue Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_key(flatbuffers::Offset<flatbuffers::String> key) {
@@ -243,7 +321,26 @@
value__);
}
+flatbuffers::Offset<KeyValue> CreateKeyValue(flatbuffers::FlatBufferBuilder &_fbb, const KeyValueT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct EnumValT : public flatbuffers::NativeTable {
+ typedef EnumVal TableType;
+ std::string name;
+ int64_t value;
+ std::unique_ptr<reflection::ObjectT> object;
+ std::unique_ptr<reflection::TypeT> union_type;
+ std::vector<std::string> documentation;
+ EnumValT()
+ : value(0) {
+ }
+};
+
struct EnumVal FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef EnumValT NativeTableType;
+ typedef EnumValBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return EnumValTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_VALUE = 6,
@@ -286,9 +383,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ EnumValT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(EnumValT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<EnumVal> Pack(flatbuffers::FlatBufferBuilder &_fbb, const EnumValT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct EnumValBuilder {
+ typedef EnumVal Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -353,7 +454,27 @@
documentation__);
}
+flatbuffers::Offset<EnumVal> CreateEnumVal(flatbuffers::FlatBufferBuilder &_fbb, const EnumValT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct EnumT : public flatbuffers::NativeTable {
+ typedef Enum TableType;
+ std::string name;
+ std::vector<std::unique_ptr<reflection::EnumValT>> values;
+ bool is_union;
+ std::unique_ptr<reflection::TypeT> underlying_type;
+ std::vector<std::unique_ptr<reflection::KeyValueT>> attributes;
+ std::vector<std::string> documentation;
+ EnumT()
+ : is_union(false) {
+ }
+};
+
struct Enum FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef EnumT NativeTableType;
+ typedef EnumBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return EnumTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_VALUES = 6,
@@ -404,9 +525,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ EnumT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(EnumT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Enum> Pack(flatbuffers::FlatBufferBuilder &_fbb, const EnumT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct EnumBuilder {
+ typedef Enum Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -482,7 +607,38 @@
documentation__);
}
+flatbuffers::Offset<Enum> CreateEnum(flatbuffers::FlatBufferBuilder &_fbb, const EnumT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct FieldT : public flatbuffers::NativeTable {
+ typedef Field TableType;
+ std::string name;
+ std::unique_ptr<reflection::TypeT> type;
+ uint16_t id;
+ uint16_t offset;
+ int64_t default_integer;
+ double default_real;
+ bool deprecated;
+ bool required;
+ bool key;
+ std::vector<std::unique_ptr<reflection::KeyValueT>> attributes;
+ std::vector<std::string> documentation;
+ FieldT()
+ : id(0),
+ offset(0),
+ default_integer(0),
+ default_real(0.0),
+ deprecated(false),
+ required(false),
+ key(false) {
+ }
+};
+
struct Field FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef FieldT NativeTableType;
+ typedef FieldBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return FieldTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_TYPE = 6,
@@ -556,9 +712,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ FieldT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(FieldT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Field> Pack(flatbuffers::FlatBufferBuilder &_fbb, const FieldT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct FieldBuilder {
+ typedef Field Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -667,7 +827,30 @@
documentation__);
}
+flatbuffers::Offset<Field> CreateField(flatbuffers::FlatBufferBuilder &_fbb, const FieldT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct ObjectT : public flatbuffers::NativeTable {
+ typedef Object TableType;
+ std::string name;
+ std::vector<std::unique_ptr<reflection::FieldT>> fields;
+ bool is_struct;
+ int32_t minalign;
+ int32_t bytesize;
+ std::vector<std::unique_ptr<reflection::KeyValueT>> attributes;
+ std::vector<std::string> documentation;
+ ObjectT()
+ : is_struct(false),
+ minalign(0),
+ bytesize(0) {
+ }
+};
+
struct Object FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef ObjectT NativeTableType;
+ typedef ObjectBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return ObjectTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_FIELDS = 6,
@@ -722,9 +905,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ ObjectT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(ObjectT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Object> Pack(flatbuffers::FlatBufferBuilder &_fbb, const ObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct ObjectBuilder {
+ typedef Object Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -806,7 +993,25 @@
documentation__);
}
+flatbuffers::Offset<Object> CreateObject(flatbuffers::FlatBufferBuilder &_fbb, const ObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct RPCCallT : public flatbuffers::NativeTable {
+ typedef RPCCall TableType;
+ std::string name;
+ std::unique_ptr<reflection::ObjectT> request;
+ std::unique_ptr<reflection::ObjectT> response;
+ std::vector<std::unique_ptr<reflection::KeyValueT>> attributes;
+ std::vector<std::string> documentation;
+ RPCCallT() {
+ }
+};
+
struct RPCCall FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef RPCCallT NativeTableType;
+ typedef RPCCallBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return RPCCallTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_REQUEST = 6,
@@ -851,9 +1056,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ RPCCallT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(RPCCallT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<RPCCall> Pack(flatbuffers::FlatBufferBuilder &_fbb, const RPCCallT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct RPCCallBuilder {
+ typedef RPCCall Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -921,7 +1130,24 @@
documentation__);
}
+flatbuffers::Offset<RPCCall> CreateRPCCall(flatbuffers::FlatBufferBuilder &_fbb, const RPCCallT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct ServiceT : public flatbuffers::NativeTable {
+ typedef Service TableType;
+ std::string name;
+ std::vector<std::unique_ptr<reflection::RPCCallT>> calls;
+ std::vector<std::unique_ptr<reflection::KeyValueT>> attributes;
+ std::vector<std::string> documentation;
+ ServiceT() {
+ }
+};
+
struct Service FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef ServiceT NativeTableType;
+ typedef ServiceBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return ServiceTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NAME = 4,
VT_CALLS = 6,
@@ -961,9 +1187,13 @@
verifier.VerifyVectorOfStrings(documentation()) &&
verifier.EndTable();
}
+ ServiceT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(ServiceT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Service> Pack(flatbuffers::FlatBufferBuilder &_fbb, const ServiceT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct ServiceBuilder {
+ typedef Service Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_name(flatbuffers::Offset<flatbuffers::String> name) {
@@ -1023,7 +1253,26 @@
documentation__);
}
+flatbuffers::Offset<Service> CreateService(flatbuffers::FlatBufferBuilder &_fbb, const ServiceT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+struct SchemaT : public flatbuffers::NativeTable {
+ typedef Schema TableType;
+ std::vector<std::unique_ptr<reflection::ObjectT>> objects;
+ std::vector<std::unique_ptr<reflection::EnumT>> enums;
+ std::string file_ident;
+ std::string file_ext;
+ std::unique_ptr<reflection::ObjectT> root_table;
+ std::vector<std::unique_ptr<reflection::ServiceT>> services;
+ SchemaT() {
+ }
+};
+
struct Schema FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+ typedef SchemaT NativeTableType;
+ typedef SchemaBuilder Builder;
+ static const flatbuffers::TypeTable *MiniReflectTypeTable() {
+ return SchemaTypeTable();
+ }
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_OBJECTS = 4,
VT_ENUMS = 6,
@@ -1069,9 +1318,13 @@
verifier.VerifyVectorOfTables(services()) &&
verifier.EndTable();
}
+ SchemaT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ void UnPackTo(SchemaT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const;
+ static flatbuffers::Offset<Schema> Pack(flatbuffers::FlatBufferBuilder &_fbb, const SchemaT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
};
struct SchemaBuilder {
+ typedef Schema Table;
flatbuffers::FlatBufferBuilder &fbb_;
flatbuffers::uoffset_t start_;
void add_objects(flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<reflection::Object>>> objects) {
@@ -1147,6 +1400,647 @@
services__);
}
+flatbuffers::Offset<Schema> CreateSchema(flatbuffers::FlatBufferBuilder &_fbb, const SchemaT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr);
+
+inline TypeT *Type::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new TypeT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Type::UnPackTo(TypeT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = base_type(); _o->base_type = _e; };
+ { auto _e = element(); _o->element = _e; };
+ { auto _e = index(); _o->index = _e; };
+ { auto _e = fixed_length(); _o->fixed_length = _e; };
+}
+
+inline flatbuffers::Offset<Type> Type::Pack(flatbuffers::FlatBufferBuilder &_fbb, const TypeT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateType(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Type> CreateType(flatbuffers::FlatBufferBuilder &_fbb, const TypeT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const TypeT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _base_type = _o->base_type;
+ auto _element = _o->element;
+ auto _index = _o->index;
+ auto _fixed_length = _o->fixed_length;
+ return reflection::CreateType(
+ _fbb,
+ _base_type,
+ _element,
+ _index,
+ _fixed_length);
+}
+
+inline KeyValueT *KeyValue::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new KeyValueT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void KeyValue::UnPackTo(KeyValueT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = key(); if (_e) _o->key = _e->str(); };
+ { auto _e = value(); if (_e) _o->value = _e->str(); };
+}
+
+inline flatbuffers::Offset<KeyValue> KeyValue::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KeyValueT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateKeyValue(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<KeyValue> CreateKeyValue(flatbuffers::FlatBufferBuilder &_fbb, const KeyValueT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KeyValueT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _key = _fbb.CreateString(_o->key);
+ auto _value = _o->value.empty() ? 0 : _fbb.CreateString(_o->value);
+ return reflection::CreateKeyValue(
+ _fbb,
+ _key,
+ _value);
+}
+
+inline EnumValT *EnumVal::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new EnumValT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void EnumVal::UnPackTo(EnumValT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = value(); _o->value = _e; };
+ { auto _e = object(); if (_e) _o->object = std::unique_ptr<reflection::ObjectT>(_e->UnPack(_resolver)); };
+ { auto _e = union_type(); if (_e) _o->union_type = std::unique_ptr<reflection::TypeT>(_e->UnPack(_resolver)); };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<EnumVal> EnumVal::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EnumValT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateEnumVal(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<EnumVal> CreateEnumVal(flatbuffers::FlatBufferBuilder &_fbb, const EnumValT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EnumValT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _value = _o->value;
+ auto _object = _o->object ? CreateObject(_fbb, _o->object.get(), _rehasher) : 0;
+ auto _union_type = _o->union_type ? CreateType(_fbb, _o->union_type.get(), _rehasher) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateEnumVal(
+ _fbb,
+ _name,
+ _value,
+ _object,
+ _union_type,
+ _documentation);
+}
+
+inline EnumT *Enum::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new EnumT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Enum::UnPackTo(EnumT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = values(); if (_e) { _o->values.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->values[_i] = std::unique_ptr<reflection::EnumValT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = is_union(); _o->is_union = _e; };
+ { auto _e = underlying_type(); if (_e) _o->underlying_type = std::unique_ptr<reflection::TypeT>(_e->UnPack(_resolver)); };
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->attributes[_i] = std::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<Enum> Enum::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EnumT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateEnum(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Enum> CreateEnum(flatbuffers::FlatBufferBuilder &_fbb, const EnumT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EnumT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _values = _fbb.CreateVector<flatbuffers::Offset<reflection::EnumVal>> (_o->values.size(), [](size_t i, _VectorArgs *__va) { return CreateEnumVal(*__va->__fbb, __va->__o->values[i].get(), __va->__rehasher); }, &_va );
+ auto _is_union = _o->is_union;
+ auto _underlying_type = _o->underlying_type ? CreateType(_fbb, _o->underlying_type.get(), _rehasher) : 0;
+ auto _attributes = _o->attributes.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateEnum(
+ _fbb,
+ _name,
+ _values,
+ _is_union,
+ _underlying_type,
+ _attributes,
+ _documentation);
+}
+
+inline FieldT *Field::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new FieldT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Field::UnPackTo(FieldT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = type(); if (_e) _o->type = std::unique_ptr<reflection::TypeT>(_e->UnPack(_resolver)); };
+ { auto _e = id(); _o->id = _e; };
+ { auto _e = offset(); _o->offset = _e; };
+ { auto _e = default_integer(); _o->default_integer = _e; };
+ { auto _e = default_real(); _o->default_real = _e; };
+ { auto _e = deprecated(); _o->deprecated = _e; };
+ { auto _e = required(); _o->required = _e; };
+ { auto _e = key(); _o->key = _e; };
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->attributes[_i] = std::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<Field> Field::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FieldT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateField(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Field> CreateField(flatbuffers::FlatBufferBuilder &_fbb, const FieldT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FieldT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _type = _o->type ? CreateType(_fbb, _o->type.get(), _rehasher) : 0;
+ auto _id = _o->id;
+ auto _offset = _o->offset;
+ auto _default_integer = _o->default_integer;
+ auto _default_real = _o->default_real;
+ auto _deprecated = _o->deprecated;
+ auto _required = _o->required;
+ auto _key = _o->key;
+ auto _attributes = _o->attributes.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateField(
+ _fbb,
+ _name,
+ _type,
+ _id,
+ _offset,
+ _default_integer,
+ _default_real,
+ _deprecated,
+ _required,
+ _key,
+ _attributes,
+ _documentation);
+}
+
+inline ObjectT *Object::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new ObjectT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Object::UnPackTo(ObjectT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = fields(); if (_e) { _o->fields.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->fields[_i] = std::unique_ptr<reflection::FieldT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = is_struct(); _o->is_struct = _e; };
+ { auto _e = minalign(); _o->minalign = _e; };
+ { auto _e = bytesize(); _o->bytesize = _e; };
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->attributes[_i] = std::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<Object> Object::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateObject(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Object> CreateObject(flatbuffers::FlatBufferBuilder &_fbb, const ObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ObjectT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _fields = _fbb.CreateVector<flatbuffers::Offset<reflection::Field>> (_o->fields.size(), [](size_t i, _VectorArgs *__va) { return CreateField(*__va->__fbb, __va->__o->fields[i].get(), __va->__rehasher); }, &_va );
+ auto _is_struct = _o->is_struct;
+ auto _minalign = _o->minalign;
+ auto _bytesize = _o->bytesize;
+ auto _attributes = _o->attributes.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateObject(
+ _fbb,
+ _name,
+ _fields,
+ _is_struct,
+ _minalign,
+ _bytesize,
+ _attributes,
+ _documentation);
+}
+
+inline RPCCallT *RPCCall::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new RPCCallT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void RPCCall::UnPackTo(RPCCallT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = request(); if (_e) _o->request = std::unique_ptr<reflection::ObjectT>(_e->UnPack(_resolver)); };
+ { auto _e = response(); if (_e) _o->response = std::unique_ptr<reflection::ObjectT>(_e->UnPack(_resolver)); };
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->attributes[_i] = std::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<RPCCall> RPCCall::Pack(flatbuffers::FlatBufferBuilder &_fbb, const RPCCallT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateRPCCall(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<RPCCall> CreateRPCCall(flatbuffers::FlatBufferBuilder &_fbb, const RPCCallT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const RPCCallT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _request = _o->request ? CreateObject(_fbb, _o->request.get(), _rehasher) : 0;
+ auto _response = _o->response ? CreateObject(_fbb, _o->response.get(), _rehasher) : 0;
+ auto _attributes = _o->attributes.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateRPCCall(
+ _fbb,
+ _name,
+ _request,
+ _response,
+ _attributes,
+ _documentation);
+}
+
+inline ServiceT *Service::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new ServiceT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Service::UnPackTo(ServiceT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = name(); if (_e) _o->name = _e->str(); };
+ { auto _e = calls(); if (_e) { _o->calls.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->calls[_i] = std::unique_ptr<reflection::RPCCallT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->attributes[_i] = std::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } };
+}
+
+inline flatbuffers::Offset<Service> Service::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ServiceT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateService(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Service> CreateService(flatbuffers::FlatBufferBuilder &_fbb, const ServiceT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ServiceT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _name = _fbb.CreateString(_o->name);
+ auto _calls = _o->calls.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::RPCCall>> (_o->calls.size(), [](size_t i, _VectorArgs *__va) { return CreateRPCCall(*__va->__fbb, __va->__o->calls[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _attributes = _o->attributes.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va ) : 0;
+ auto _documentation = _o->documentation.size() ? _fbb.CreateVectorOfStrings(_o->documentation) : 0;
+ return reflection::CreateService(
+ _fbb,
+ _name,
+ _calls,
+ _attributes,
+ _documentation);
+}
+
+inline SchemaT *Schema::UnPack(const flatbuffers::resolver_function_t *_resolver) const {
+ auto _o = new SchemaT();
+ UnPackTo(_o, _resolver);
+ return _o;
+}
+
+inline void Schema::UnPackTo(SchemaT *_o, const flatbuffers::resolver_function_t *_resolver) const {
+ (void)_o;
+ (void)_resolver;
+ { auto _e = objects(); if (_e) { _o->objects.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->objects[_i] = std::unique_ptr<reflection::ObjectT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = enums(); if (_e) { _o->enums.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->enums[_i] = std::unique_ptr<reflection::EnumT>(_e->Get(_i)->UnPack(_resolver)); } } };
+ { auto _e = file_ident(); if (_e) _o->file_ident = _e->str(); };
+ { auto _e = file_ext(); if (_e) _o->file_ext = _e->str(); };
+ { auto _e = root_table(); if (_e) _o->root_table = std::unique_ptr<reflection::ObjectT>(_e->UnPack(_resolver)); };
+ { auto _e = services(); if (_e) { _o->services.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->services[_i] = std::unique_ptr<reflection::ServiceT>(_e->Get(_i)->UnPack(_resolver)); } } };
+}
+
+inline flatbuffers::Offset<Schema> Schema::Pack(flatbuffers::FlatBufferBuilder &_fbb, const SchemaT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
+ return CreateSchema(_fbb, _o, _rehasher);
+}
+
+inline flatbuffers::Offset<Schema> CreateSchema(flatbuffers::FlatBufferBuilder &_fbb, const SchemaT *_o, const flatbuffers::rehasher_function_t *_rehasher) {
+ (void)_rehasher;
+ (void)_o;
+ struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const SchemaT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va;
+ auto _objects = _fbb.CreateVector<flatbuffers::Offset<reflection::Object>> (_o->objects.size(), [](size_t i, _VectorArgs *__va) { return CreateObject(*__va->__fbb, __va->__o->objects[i].get(), __va->__rehasher); }, &_va );
+ auto _enums = _fbb.CreateVector<flatbuffers::Offset<reflection::Enum>> (_o->enums.size(), [](size_t i, _VectorArgs *__va) { return CreateEnum(*__va->__fbb, __va->__o->enums[i].get(), __va->__rehasher); }, &_va );
+ auto _file_ident = _o->file_ident.empty() ? 0 : _fbb.CreateString(_o->file_ident);
+ auto _file_ext = _o->file_ext.empty() ? 0 : _fbb.CreateString(_o->file_ext);
+ auto _root_table = _o->root_table ? CreateObject(_fbb, _o->root_table.get(), _rehasher) : 0;
+ auto _services = _o->services.size() ? _fbb.CreateVector<flatbuffers::Offset<reflection::Service>> (_o->services.size(), [](size_t i, _VectorArgs *__va) { return CreateService(*__va->__fbb, __va->__o->services[i].get(), __va->__rehasher); }, &_va ) : 0;
+ return reflection::CreateSchema(
+ _fbb,
+ _objects,
+ _enums,
+ _file_ident,
+ _file_ext,
+ _root_table,
+ _services);
+}
+
+inline const flatbuffers::TypeTable *BaseTypeTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::BaseTypeTypeTable
+ };
+ static const char * const names[] = {
+ "None",
+ "UType",
+ "Bool",
+ "Byte",
+ "UByte",
+ "Short",
+ "UShort",
+ "Int",
+ "UInt",
+ "Long",
+ "ULong",
+ "Float",
+ "Double",
+ "String",
+ "Vector",
+ "Obj",
+ "Union",
+ "Array"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_ENUM, 18, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *TypeTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_CHAR, 0, 0 },
+ { flatbuffers::ET_INT, 0, -1 },
+ { flatbuffers::ET_USHORT, 0, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::BaseTypeTypeTable
+ };
+ static const char * const names[] = {
+ "base_type",
+ "element",
+ "index",
+ "fixed_length"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 4, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *KeyValueTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_STRING, 0, -1 }
+ };
+ static const char * const names[] = {
+ "key",
+ "value"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 2, type_codes, nullptr, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *EnumValTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_LONG, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 0, 0 },
+ { flatbuffers::ET_SEQUENCE, 0, 1 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::ObjectTypeTable,
+ reflection::TypeTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "value",
+ "object",
+ "union_type",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 5, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *EnumTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 0 },
+ { flatbuffers::ET_BOOL, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 0, 1 },
+ { flatbuffers::ET_SEQUENCE, 1, 2 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::EnumValTypeTable,
+ reflection::TypeTypeTable,
+ reflection::KeyValueTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "values",
+ "is_union",
+ "underlying_type",
+ "attributes",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 6, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *FieldTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 0, 0 },
+ { flatbuffers::ET_USHORT, 0, -1 },
+ { flatbuffers::ET_USHORT, 0, -1 },
+ { flatbuffers::ET_LONG, 0, -1 },
+ { flatbuffers::ET_DOUBLE, 0, -1 },
+ { flatbuffers::ET_BOOL, 0, -1 },
+ { flatbuffers::ET_BOOL, 0, -1 },
+ { flatbuffers::ET_BOOL, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 1 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::TypeTypeTable,
+ reflection::KeyValueTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "type",
+ "id",
+ "offset",
+ "default_integer",
+ "default_real",
+ "deprecated",
+ "required",
+ "key",
+ "attributes",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 11, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *ObjectTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 0 },
+ { flatbuffers::ET_BOOL, 0, -1 },
+ { flatbuffers::ET_INT, 0, -1 },
+ { flatbuffers::ET_INT, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 1 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::FieldTypeTable,
+ reflection::KeyValueTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "fields",
+ "is_struct",
+ "minalign",
+ "bytesize",
+ "attributes",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 7, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *RPCCallTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 0, 0 },
+ { flatbuffers::ET_SEQUENCE, 0, 0 },
+ { flatbuffers::ET_SEQUENCE, 1, 1 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::ObjectTypeTable,
+ reflection::KeyValueTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "request",
+ "response",
+ "attributes",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 5, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *ServiceTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 0 },
+ { flatbuffers::ET_SEQUENCE, 1, 1 },
+ { flatbuffers::ET_STRING, 1, -1 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::RPCCallTypeTable,
+ reflection::KeyValueTypeTable
+ };
+ static const char * const names[] = {
+ "name",
+ "calls",
+ "attributes",
+ "documentation"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 4, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
+inline const flatbuffers::TypeTable *SchemaTypeTable() {
+ static const flatbuffers::TypeCode type_codes[] = {
+ { flatbuffers::ET_SEQUENCE, 1, 0 },
+ { flatbuffers::ET_SEQUENCE, 1, 1 },
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_STRING, 0, -1 },
+ { flatbuffers::ET_SEQUENCE, 0, 0 },
+ { flatbuffers::ET_SEQUENCE, 1, 2 }
+ };
+ static const flatbuffers::TypeFunction type_refs[] = {
+ reflection::ObjectTypeTable,
+ reflection::EnumTypeTable,
+ reflection::ServiceTypeTable
+ };
+ static const char * const names[] = {
+ "objects",
+ "enums",
+ "file_ident",
+ "file_ext",
+ "root_table",
+ "services"
+ };
+ static const flatbuffers::TypeTable tt = {
+ flatbuffers::ST_TABLE, 6, type_codes, type_refs, nullptr, names
+ };
+ return &tt;
+}
+
inline const reflection::Schema *GetSchema(const void *buf) {
return flatbuffers::GetRoot<reflection::Schema>(buf);
}
@@ -1190,6 +2084,18 @@
fbb.FinishSizePrefixed(root, SchemaIdentifier());
}
+inline std::unique_ptr<reflection::SchemaT> UnPackSchema(
+ const void *buf,
+ const flatbuffers::resolver_function_t *res = nullptr) {
+ return std::unique_ptr<reflection::SchemaT>(GetSchema(buf)->UnPack(res));
+}
+
+inline std::unique_ptr<reflection::SchemaT> UnPackSizePrefixedSchema(
+ const void *buf,
+ const flatbuffers::resolver_function_t *res = nullptr) {
+ return std::unique_ptr<reflection::SchemaT>(GetSizePrefixedSchema(buf)->UnPack(res));
+}
+
} // namespace reflection
#endif // FLATBUFFERS_GENERATED_REFLECTION_REFLECTION_H_
diff --git a/third_party/flatbuffers/reflection/generate_code.sh b/third_party/flatbuffers/reflection/generate_code.sh
index f186b73..6e12c3d 100755
--- a/third_party/flatbuffers/reflection/generate_code.sh
+++ b/third_party/flatbuffers/reflection/generate_code.sh
@@ -15,4 +15,4 @@
# limitations under the License.
set -e
-../flatc -c --no-prefix -o ../include/flatbuffers reflection.fbs
+../../../bazel-out/host/bin/external/com_github_google_flatbuffers/flatc -c --gen-object-api --reflect-names --no-prefix -o ../include/flatbuffers reflection.fbs
diff --git a/third_party/flatbuffers/src/idl_gen_cpp.cpp b/third_party/flatbuffers/src/idl_gen_cpp.cpp
index 0f9950d..a646d91 100644
--- a/third_party/flatbuffers/src/idl_gen_cpp.cpp
+++ b/third_party/flatbuffers/src/idl_gen_cpp.cpp
@@ -196,9 +196,19 @@
auto noext = flatbuffers::StripExtension(it->second);
auto basename = flatbuffers::StripPath(noext);
- code_ += "#include \"" + parser_.opts.include_prefix +
- (parser_.opts.keep_include_path ? noext : basename) +
- "_generated.h\"";
+ // TODO(austin): Clean this up.
+ // The reflection_generated.h header is not in the reflection folder like
+ // it's include path and namespace suggests. Detect this special case and
+ // rewrite it.
+ std::string include =
+ parser_.opts.include_prefix +
+ (parser_.opts.keep_include_path ? noext : basename) + "_generated.h";
+
+ if (include == "reflection/reflection_generated.h") {
+ include = "flatbuffers/reflection_generated.h";
+ }
+
+ code_ += "#include \"" + include + "\"";
num_includes++;
}
if (num_includes) code_ += "";
@@ -251,12 +261,14 @@
// Generate forward declarations for all structs/tables, since they may
// have circular references.
+ // LOCAL_MOD: Add builders to forward declares
for (auto it = parser_.structs_.vec.begin();
it != parser_.structs_.vec.end(); ++it) {
const auto &struct_def = **it;
if (!struct_def.generated) {
SetNameSpace(struct_def.defined_namespace);
code_ += "struct " + Name(struct_def) + ";";
+ code_ += "struct " + Name(struct_def) + "Builder;";
if (parser_.opts.generate_object_based_api) {
auto nativeName =
NativeName(Name(struct_def), &struct_def, parser_.opts);
@@ -1803,6 +1815,8 @@
if (parser_.opts.generate_object_based_api) {
code_ += " typedef {{NATIVE_NAME}} NativeTableType;";
}
+ // LOCAL_MOD: Add builder typedef.
+ code_ += " typedef {{STRUCT_NAME}}Builder Builder;";
if (parser_.opts.mini_reflect != IDLOptions::kNone) {
code_ +=
" static const flatbuffers::TypeTable *MiniReflectTypeTable() {";
@@ -2065,6 +2079,7 @@
// Generate a builder struct:
code_ += "struct {{STRUCT_NAME}}Builder {";
+ code_ += " typedef {{STRUCT_NAME}} Table;";
code_ += " flatbuffers::FlatBufferBuilder &fbb_;";
code_ += " flatbuffers::uoffset_t start_;";
diff --git a/third_party/gperftools/BUILD b/third_party/gperftools/BUILD
index 3798a29..f9cf185 100644
--- a/third_party/gperftools/BUILD
+++ b/third_party/gperftools/BUILD
@@ -4,7 +4,7 @@
load("//tools/build_rules:empty_main.bzl", "empty_main_if_asan")
common_copts = [
- # Stuff from their Makefile.
+#Stuff from their Makefile.
"-Wno-cast-align",
"-Wno-sign-compare",
"-fno-builtin-malloc",
@@ -20,7 +20,7 @@
"-fno-omit-frame-pointer",
"-DNDEBUG",
- # Stuff to make it work for us.
+#Stuff to make it work for us.
"-Ithird_party/gperftools/src/",
"-Ithird_party/empty_config_h",
"-Wno-unused-parameter",
@@ -32,7 +32,7 @@
"-Wno-error=cast-align",
"-Wno-error=cast-qual",
- # Stuff pulled out of config.h.
+#Stuff pulled out of config.h.
"-DHAVE_BUILTIN_EXPECT=1",
"-DHAVE_DECL_CFREE=1",
"-DHAVE_DECL_MEMALIGN=1",
@@ -145,6 +145,7 @@
deps = [
"//third_party/empty_config_h",
],
+ includes = ["src"],
alwayslink = True,
)
@@ -209,8 +210,8 @@
]),
copts = common_copts + [
"-fno-builtin",
- # Add this back in when we upgrade clang.
- #'-Wno-mismatched-new-delete',
+#Add this back in when we upgrade clang.
+#'-Wno-mismatched-new-delete',
],
deps = [
":tcmalloc",
diff --git a/tools/build_rules/ruby.bzl b/tools/build_rules/ruby.bzl
deleted file mode 100644
index cfab2e9..0000000
--- a/tools/build_rules/ruby.bzl
+++ /dev/null
@@ -1,94 +0,0 @@
-ZIP_PATH = '/usr/bin/zip'
-
-ruby_file_types = FileType(['.rb'])
-
-def _collect_transitive_sources(ctx):
- source_files = depset(order='postorder')
- for dep in ctx.attr.deps:
- source_files += dep.transitive_ruby_files
-
- source_files += ruby_file_types.filter(ctx.files.srcs)
- return source_files
-
-def _ruby_library_impl(ctx):
- transitive_sources = _collect_transitive_sources(ctx)
- return struct(
- transitive_ruby_files = transitive_sources,
- )
-
-def _ruby_binary_impl(ctx):
- main_file = ctx.files.srcs[0]
- transitive_sources = _collect_transitive_sources(ctx)
- executable = ctx.outputs.executable
- manifest_file = ctx.outputs.manifest
-
- path_map = '\n'.join([('%s|%s' % (item.short_path, item.path))
- for item in transitive_sources])
- ctx.file_action(output=manifest_file, content=path_map)
-
- ctx.action(
- inputs = list(transitive_sources) + [manifest_file],
- outputs = [ executable ],
- arguments = [ manifest_file.path, executable.path, main_file.path ],
- executable = ctx.executable._ruby_linker,
- )
-
- return struct(
- files = depset([executable]),
- runfiles = ctx.runfiles(collect_data = True),
- )
-
-
-_ruby_attrs = {
- 'srcs': attr.label_list(
- allow_files = ruby_file_types,
- mandatory = True,
- non_empty = True,
- ),
- 'deps': attr.label_list(
- providers = ['transitive_ruby_files'],
- allow_files = False,
- ),
- 'data': attr.label_list(
- allow_files = True,
- cfg = 'data',
- ),
-}
-
-'''Packages ruby code into a library.
-
-The files can use require with paths from the base of the workspace and/or
-require_relative with other files that are part of the library.
-require also works from the filesystem as usual.
-
-Attrs:
- srcs: Ruby source files to include.
- deps: Other ruby_library rules to include.
-'''
-ruby_library = rule(
- implementation = _ruby_library_impl,
- attrs = _ruby_attrs,
-)
-
-'''Packages ruby code into a binary which can be run.
-
-See ruby_library for details on how require works.
-
-Attrs:
- srcs: Ruby source files to include. The first one is loaded to at startup.
- deps: ruby_library rules to include.
-'''
-ruby_binary = rule(
- implementation = _ruby_binary_impl,
- executable = True,
- attrs = _ruby_attrs + {
- '_ruby_linker': attr.label(
- executable = True,
- cfg = 'host',
- default = Label('//tools/ruby:standalone_ruby'),
- )
- },
- outputs = {
- 'manifest': '%{name}.tar_manifest',
- },
-)
diff --git a/y2012/BUILD b/y2012/BUILD
index 401beac..ff6d3f2 100644
--- a/y2012/BUILD
+++ b/y2012/BUILD
@@ -12,9 +12,10 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//y2012/control_loops/accessories:accessories_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//y2012/control_loops/accessories:accessories_fbs",
],
)
@@ -37,17 +38,18 @@
"//aos:init",
"//aos:make_unique",
"//aos/controls:control_loop",
- "//aos/events:shm-event-loop",
+ "//aos/controls:control_loop_fbs",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
@@ -55,13 +57,13 @@
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
- "//y2012/control_loops/accessories:accessories_queue",
+ "//y2012/control_loops/accessories:accessories_fbs",
],
)
diff --git a/y2012/control_loops/accessories/BUILD b/y2012/control_loops/accessories/BUILD
index 63f02dd..65a6d1d 100644
--- a/y2012/control_loops/accessories/BUILD
+++ b/y2012/control_loops/accessories/BUILD
@@ -1,25 +1,23 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_binary(
- name = 'accessories',
- srcs = [
- 'accessories.cc',
- ],
- deps = [
- ':accessories_queue',
- '//aos:init',
- '//aos/controls:control_loop',
- ],
+ name = "accessories",
+ srcs = [
+ "accessories.cc",
+ ],
+ deps = [
+ ":accessories_fbs",
+ "//aos:init",
+ "//aos/controls:control_loop",
+ "//aos/controls:control_loop_fbs",
+ ],
)
-queue_library(
- name = 'accessories_queue',
- srcs = [
- 'accessories.q',
- ],
- deps = [
- '//aos/controls:control_loop_queues',
- ],
+flatbuffer_cc_library(
+ name = "accessories_fbs",
+ srcs = [
+ "accessories.fbs",
+ ],
)
diff --git a/y2012/control_loops/accessories/accessories.cc b/y2012/control_loops/accessories/accessories.cc
index 9d96840..726aede 100644
--- a/y2012/control_loops/accessories/accessories.cc
+++ b/y2012/control_loops/accessories/accessories.cc
@@ -1,28 +1,43 @@
-#include "y2012/control_loops/accessories/accessories.q.h"
+#include "y2012/control_loops/accessories/accessories_generated.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/controls/control_loop.h"
+#include "aos/controls/control_loops_generated.h"
namespace y2012 {
namespace control_loops {
namespace accessories {
class AccessoriesLoop : public ::aos::controls::ControlLoop<
- ::y2012::control_loops::AccessoriesQueue> {
+ Message, ::aos::control_loops::Position,
+ ::aos::control_loops::Status, Message> {
public:
explicit AccessoriesLoop(
::aos::EventLoop *event_loop,
const ::std::string &name = ".y2012.control_loops.accessories_queue")
- : ::aos::controls::ControlLoop<::y2012::control_loops::AccessoriesQueue>(
+ : ::aos::controls::ControlLoop<Message, ::aos::control_loops::Position,
+ ::aos::control_loops::Status, Message>(
event_loop, name) {}
void RunIteration(
- const ::y2012::control_loops::AccessoriesQueue::Message *goal,
+ const Message *goal,
const ::aos::control_loops::Position * /*position*/,
- ::y2012::control_loops::AccessoriesQueue::Message *output,
- ::aos::control_loops::Status * /*status*/) override {
+ ::aos::Sender<Message>::Builder *output,
+ ::aos::Sender<::aos::control_loops::Status>::Builder * /*status*/) override {
if (output) {
- *output = *goal;
+ //*output = *goal;
+ Message::Builder output_builder = output->MakeBuilder<Message>();
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> solenoid_offset =
+ output->fbb()->template CreateVector<uint8_t>(
+ goal->solenoids()->data(), 3);
+ output_builder.add_solenoids(solenoid_offset);
+ flatbuffers::Offset<flatbuffers::Vector<double>> stick_offset =
+ output->fbb()->template CreateVector<double>(
+ goal->sticks()->data(), 2);
+ output_builder.add_sticks(stick_offset);
+
+ output_builder.Finish();
}
}
};
@@ -34,7 +49,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());
::y2012::control_loops::accessories::AccessoriesLoop accessories(&event_loop);
event_loop.Run();
diff --git a/y2012/control_loops/accessories/accessories.fbs b/y2012/control_loops/accessories/accessories.fbs
new file mode 100644
index 0000000..1174b55
--- /dev/null
+++ b/y2012/control_loops/accessories/accessories.fbs
@@ -0,0 +1,8 @@
+namespace y2012.control_loops.accessories;
+
+table Message {
+ solenoids:[bool]; // Exactly 3 values
+ sticks:[double]; // Exactly 2 values
+}
+
+root_type Message;
diff --git a/y2012/control_loops/accessories/accessories.q b/y2012/control_loops/accessories/accessories.q
deleted file mode 100644
index 6427d49..0000000
--- a/y2012/control_loops/accessories/accessories.q
+++ /dev/null
@@ -1,17 +0,0 @@
-package y2012.control_loops;
-
-import "aos/controls/control_loops.q";
-
-// Published on ".y2012.control_loops.accessories_queue"
-queue_group AccessoriesQueue {
- implements aos.control_loops.ControlLoop;
- message Message {
- bool[3] solenoids;
- double[2] sticks;
- };
-
- queue Message goal;
- queue .aos.control_loops.Position position;
- queue Message output;
- queue .aos.control_loops.Status status;
-};
diff --git a/y2012/control_loops/drivetrain/BUILD b/y2012/control_loops/drivetrain/BUILD
index 3be1e9a..35d596a 100644
--- a/y2012/control_loops/drivetrain/BUILD
+++ b/y2012/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -78,7 +76,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
index 75153cd..735f13f 100644
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2012/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 "y2012/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 460d850..48e6eb5 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -4,15 +4,15 @@
#include <math.h>
#include "aos/actions/actions.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/input/driver_station_data.h"
#include "aos/input/joystick_input.h"
#include "aos/logging/logging.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2012/control_loops/accessories/accessories.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "y2012/control_loops/accessories/accessories_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -81,13 +81,12 @@
Reader(::aos::EventLoop *event_loop)
: ::aos::input::JoystickInput(event_loop),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")),
accessories_goal_sender_(
event_loop
- ->MakeSender<::y2012::control_loops::AccessoriesQueue::Message>(
- ".y2012.control_loops.accessories_queue.goal")),
+ ->MakeSender<::y2012::control_loops::accessories::Message>(
+ "/accessories")),
is_high_gear_(false) {}
void RunIteration(const ::aos::input::driver_station::Data &data) override {
@@ -106,33 +105,43 @@
if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = true;
}
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->wheel = wheel;
- drivetrain_message->throttle = throttle;
- drivetrain_message->highgear = is_high_gear_;
- drivetrain_message->quickturn = data.IsPressed(kQuickTurn);
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Goal>();
+ goal_builder.add_wheel(wheel);
+ goal_builder.add_throttle(throttle);
+ goal_builder.add_highgear(is_high_gear_);
+ goal_builder.add_quickturn(data.IsPressed(kQuickTurn));
- if (!drivetrain_message.Send()) {
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending stick values failed\n");
}
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- auto accessories_message = accessories_goal_sender_.MakeMessage();
- accessories_message->solenoids[0] = data.IsPressed(kLongShot);
- accessories_message->solenoids[1] = data.IsPressed(kCloseShot);
- accessories_message->solenoids[2] = data.IsPressed(kFenderShot);
- accessories_message->sticks[0] = data.GetAxis(kAdjustClawGoal);
- accessories_message->sticks[1] = data.GetAxis(kAdjustClawSeparation);
- if (!accessories_message.Send()) {
+ auto builder = accessories_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> solenoids_offset =
+ builder.fbb()->CreateVector<uint8_t>({data.IsPressed(kLongShot),
+ data.IsPressed(kCloseShot),
+ data.IsPressed(kFenderShot)});
+
+ flatbuffers::Offset<flatbuffers::Vector<double>> sticks_offset =
+ builder.fbb()->CreateVector<double>({data.GetAxis(kAdjustClawGoal),
+ data.GetAxis(kAdjustClawSeparation)});
+
+ y2012::control_loops::accessories::Message::Builder message_builder =
+ builder.MakeBuilder<y2012::control_loops::accessories::Message>();
+ message_builder.add_solenoids(solenoids_offset);
+ message_builder.add_sticks(sticks_offset);
+ if (!builder.Send(message_builder.Finish())) {
AOS_LOG(WARNING, "sending accessories goal failed\n");
}
}
private:
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
- ::aos::Sender<::y2012::control_loops::AccessoriesQueue::Message>
+ ::aos::Sender<::y2012::control_loops::accessories::Message>
accessories_goal_sender_;
bool is_high_gear_;
@@ -145,7 +154,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());
::y2012::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index a85f096..b28c34c 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -19,18 +19,19 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
-#include "aos/events/shm-event-loop.h"
+#include "aos/controls/control_loops_generated.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
@@ -39,16 +40,12 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/sensor_reader.h"
-#include "y2012/control_loops/accessories/accessories.q.h"
+#include "y2012/control_loops/accessories/accessories_generated.h"
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::y2012::control_loops::AccessoriesQueue;
+namespace accessories = ::y2012::control_loops::accessories;
using namespace frc;
using aos::make_unique;
@@ -74,33 +71,40 @@
: ::frc971::wpilib::SensorReader(event_loop),
accessories_position_sender_(
event_loop->MakeSender<::aos::control_loops::Position>(
- ".y2012.control_loops.accessories_queue.position")),
+ "/accessories")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {}
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {}
void RunIteration() {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
- drivetrain_message.Send();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_right_encoder(
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
+
+ builder.Send(position_builder.Finish());
}
- accessories_position_sender_.MakeMessage().Send();
+ {
+ auto builder = accessories_position_sender_.MakeBuilder();
+ builder.Send(
+ builder.MakeBuilder<::aos::control_loops::Position>().Finish());
+ }
}
private:
::aos::Sender<::aos::control_loops::Position> accessories_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
};
@@ -112,11 +116,14 @@
pcm_(pcm),
drivetrain_fetcher_(
event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".y2012.control_loops.drivetrain_queue.output")),
- accessories_fetcher_(event_loop_->MakeFetcher<
- ::y2012::control_loops::AccessoriesQueue::Message>(
- ".y2012.control_loops.accessories_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ accessories_fetcher_(
+ event_loop_
+ ->MakeFetcher<::y2012::control_loops::accessories::Message>(
+ "/accessories")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
event_loop_->set_name("Solenoids");
event_loop_->SetRuntimeRealtimePriority(27);
@@ -155,29 +162,30 @@
{
accessories_fetcher_.Fetch();
if (accessories_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *accessories_fetcher_);
- s1_->Set(accessories_fetcher_->solenoids[0]);
- s2_->Set(accessories_fetcher_->solenoids[1]);
- s3_->Set(accessories_fetcher_->solenoids[2]);
+ s1_->Set(accessories_fetcher_->solenoids()->Get(0));
+ s2_->Set(accessories_fetcher_->solenoids()->Get(1));
+ s3_->Set(accessories_fetcher_->solenoids()->Get(2));
}
}
{
drivetrain_fetcher_.Fetch();
if (drivetrain_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
- const bool high =
- drivetrain_fetcher_->left_high || drivetrain_fetcher_->right_high;
+ const bool high = drivetrain_fetcher_->left_high() ||
+ drivetrain_fetcher_->right_high();
drivetrain_high_->Set(high);
drivetrain_low_->Set(!high);
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
}
@@ -192,28 +200,30 @@
::std::unique_ptr<Compressor> compressor_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_fetcher_;
- ::aos::Fetcher<::y2012::control_loops::AccessoriesQueue::Message>
+ ::aos::Fetcher<::y2012::control_loops::accessories::Message>
accessories_fetcher_;
+
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
};
-class AccessoriesWriter
- : public ::frc971::wpilib::LoopOutputHandler<AccessoriesQueue::Message> {
+class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler<
+ control_loops::accessories::Message> {
public:
AccessoriesWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<AccessoriesQueue::Message>(
- event_loop, ".y2012.control_loops.accessories_queue.output") {}
+ : ::frc971::wpilib::LoopOutputHandler<
+ control_loops::accessories::Message>(event_loop, "/accessories") {}
void set_talon1(::std::unique_ptr<Talon> t) { talon1_ = ::std::move(t); }
void set_talon2(::std::unique_ptr<Talon> t) { talon2_ = ::std::move(t); }
private:
- virtual void Write(const AccessoriesQueue::Message &output) override {
- talon1_->SetSpeed(output.sticks[0]);
- talon2_->SetSpeed(output.sticks[1]);
- AOS_LOG_STRUCT(DEBUG, "will output", output);
+ virtual void Write(
+ const control_loops::accessories::Message &output) override {
+ talon1_->SetSpeed(output.sticks()->Get(0));
+ talon2_->SetSpeed(output.sticks()->Get(1));
}
virtual void Stop() override {
@@ -233,21 +243,24 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
AddLoop(&sensor_reader_event_loop);
// Thread 3.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(3)), true);
@@ -260,7 +273,7 @@
AddLoop(&output_event_loop);
// Thread 4.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
diff --git a/y2014/BUILD b/y2014/BUILD
index b11f170..1e1384b 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
cc_library(
name = "constants",
@@ -33,13 +34,14 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/queues:gyro",
"//y2014/actors:shoot_action_lib",
- "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
"//y2014/control_loops/drivetrain:drivetrain_base",
- "//y2014/control_loops/shooter:shooter_queue",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
],
)
@@ -61,12 +63,32 @@
"hot_goal_reader.cc",
],
deps = [
+ "//aos:byteorder",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
- "//y2014/queues:hot_goal",
+ "//y2014/queues:hot_goal_fbs",
+ ],
+)
+
+aos_config(
+ name = "config",
+ src = "y2014.json",
+ flatbuffers = [
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/control_loops/shooter:shooter_output_fbs",
+ "//y2014/control_loops/shooter:shooter_position_fbs",
+ "//y2014/control_loops/shooter:shooter_status_fbs",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_output_fbs",
+ "//y2014/control_loops/claw:claw_position_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
],
)
@@ -82,15 +104,15 @@
"//aos:make_unique",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
@@ -99,16 +121,18 @@
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
- "//y2014/control_loops/claw:claw_queue",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:auto_mode",
+ "//y2014/control_loops/claw:claw_output_fbs",
+ "//y2014/control_loops/claw:claw_position_fbs",
+ "//y2014/control_loops/shooter:shooter_output_fbs",
+ "//y2014/control_loops/shooter:shooter_position_fbs",
+ "//y2014/queues:auto_mode_fbs",
],
)
diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 8612d0b..82255ab 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
filegroup(
name = "binaries",
srcs = [
@@ -18,16 +16,6 @@
visibility = ["//visibility:public"],
)
-queue_library(
- name = "shoot_action_queue",
- srcs = [
- "shoot_action.q",
- ],
- deps = [
- "//aos/actions:action_queue",
- ],
-)
-
cc_library(
name = "shoot_action_lib",
srcs = [
@@ -38,14 +26,13 @@
],
visibility = ["//visibility:public"],
deps = [
- ":shoot_action_queue",
"//aos/actions:action_lib",
"//aos/logging",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2014:constants",
- "//y2014/control_loops/claw:claw_queue",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:profile_params",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/control_loops/shooter:shooter_status_fbs",
],
)
@@ -56,9 +43,8 @@
],
deps = [
":shoot_action_lib",
- ":shoot_action_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -72,19 +58,18 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2014/actors:shoot_action_lib",
- "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
"//y2014/control_loops/drivetrain:drivetrain_base",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:auto_mode",
- "//y2014/queues:hot_goal",
- "//y2014/queues:profile_params",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/queues:auto_mode_fbs",
+ "//y2014/queues:hot_goal_fbs",
],
)
@@ -96,7 +81,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/actors/autonomous_actor.cc b/y2014/actors/autonomous_actor.cc
index 4e2c9aa..7d1eb3f 100644
--- a/y2014/actors/autonomous_actor.cc
+++ b/y2014/actors/autonomous_actor.cc
@@ -7,18 +7,16 @@
#include "aos/actions/actions.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/actors/shoot_actor.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
+#include "y2014/queues/hot_goal_generated.h"
namespace y2014 {
namespace actors {
@@ -26,49 +24,50 @@
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
using ::aos::monotonic_clock;
-using ::frc971::ProfileParameters;
+using ::frc971::ProfileParametersT;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
event_loop, control_loops::GetDrivetrainConfig()),
- auto_mode_fetcher_(event_loop->MakeFetcher<::y2014::sensors::AutoMode>(
- ".y2014.sensors.auto_mode")),
- hot_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::HotGoal>(".y2014.hot_goal")),
+ auto_mode_fetcher_(
+ event_loop->MakeFetcher<::y2014::sensors::AutoMode>("/aos")),
+ hot_goal_fetcher_(event_loop->MakeFetcher<::y2014::HotGoal>("/")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>("/claw")),
claw_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Goal>("/claw")),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
shoot_action_factory_(actors::ShootActor::MakeFactory(event_loop)) {}
void AutonomousActor::PositionClawVertically(double intake_power,
double centering_power) {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.0;
- goal_message->separation_angle = 0.0;
- goal_message->intake = intake_power;
- goal_message->centering = centering_power;
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.0);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(intake_power);
+ goal_builder.add_centering(centering_power);
- if (!goal_message.Send()) {
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawBackIntake() {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = -2.273474;
- goal_message->separation_angle = 0.0;
- goal_message->intake = 12.0;
- goal_message->centering = 12.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(-2.273474);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(12.0);
+ goal_builder.add_centering(12.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
@@ -76,48 +75,63 @@
void AutonomousActor::PositionClawUpClosed() {
// Move the claw to where we're going to shoot from but keep it closed until
// it gets there.
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.86;
- goal_message->separation_angle = 0.0;
- goal_message->intake = 4.0;
- goal_message->centering = 1.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.86);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(4.0);
+ goal_builder.add_centering(1.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawForShot() {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.86;
- goal_message->separation_angle = 0.10;
- goal_message->intake = 4.0;
- goal_message->centering = 1.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.86);
+ goal_builder.add_separation_angle(0.10);
+ goal_builder.add_intake(4.0);
+ goal_builder.add_centering(1.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::SetShotPower(double power) {
AOS_LOG(INFO, "Setting shot power to %f\n", power);
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = power;
- goal_message->shot_requested = false;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(power);
+ goal_builder.add_shot_requested(false);
+ goal_builder.add_unload_requested(false);
+ goal_builder.add_load_requested(false);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
}
}
-const ProfileParameters kFastDrive = {3.0, 2.5};
-const ProfileParameters kSlowDrive = {2.5, 2.5};
-const ProfileParameters kFastWithBallDrive = {3.0, 2.0};
-const ProfileParameters kSlowWithBallDrive = {2.5, 2.0};
-const ProfileParameters kFastTurn = {3.0, 10.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kFastDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kSlowDrive = MakeProfileParameters(2.5, 2.5);
+const ProfileParametersT kFastWithBallDrive = MakeProfileParameters(3.0, 2.0);
+const ProfileParametersT kSlowWithBallDrive = MakeProfileParameters(2.5, 2.0);
+const ProfileParametersT kFastTurn = MakeProfileParameters(3.0, 10.0);
void AutonomousActor::Shoot() {
// Shoot.
- auto shoot_action = shoot_action_factory_.Make(0.0);
+ aos::common::actions::DoubleParamT param;
+ auto shoot_action = shoot_action_factory_.Make(param);
shoot_action->Start();
WaitUntilDoneOrCanceled(::std::move(shoot_action));
}
@@ -138,12 +152,12 @@
claw_goal_fetcher_.get() == nullptr) {
continue;
}
- bool ans = claw_status_fetcher_->zeroed &&
- (::std::abs(claw_status_fetcher_->bottom_velocity) < 1.0) &&
- (::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle) < 0.10) &&
- (::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed() &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity()) < 1.0) &&
+ (::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()) < 0.4);
if (ans) {
return true;
}
@@ -160,8 +174,7 @@
void ResetCounts() {
hot_goal_fetcher_->Fetch();
if (hot_goal_fetcher_->get()) {
- start_counts_ = *hot_goal_fetcher_->get();
- AOS_LOG_STRUCT(INFO, "counts reset to", start_counts_);
+ hot_goal_fetcher_->get()->UnPackTo(&start_counts_);
start_counts_valid_ = true;
} else {
AOS_LOG(WARNING, "no hot goal message. ignoring\n");
@@ -169,30 +182,26 @@
}
}
- void Update() {
- hot_goal_fetcher_->Fetch();
- if (hot_goal_fetcher_->get())
- AOS_LOG_STRUCT(INFO, "new counts", *hot_goal_fetcher_->get());
- }
+ void Update() { hot_goal_fetcher_->Fetch(); }
bool left_triggered() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
- return (hot_goal_fetcher_->get()->left_count - start_counts_.left_count) >
+ return (hot_goal_fetcher_->get()->left_count() - start_counts_.left_count) >
kThreshold;
}
bool right_triggered() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
- return (hot_goal_fetcher_->get()->right_count - start_counts_.right_count) >
- kThreshold;
+ return (hot_goal_fetcher_->get()->right_count() -
+ start_counts_.right_count) > kThreshold;
}
bool is_left() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
const uint64_t left_difference =
- hot_goal_fetcher_->get()->left_count - start_counts_.left_count;
+ hot_goal_fetcher_->get()->left_count() - start_counts_.left_count;
const uint64_t right_difference =
- hot_goal_fetcher_->get()->right_count - start_counts_.right_count;
+ hot_goal_fetcher_->get()->right_count() - start_counts_.right_count;
if (left_difference > kThreshold) {
if (right_difference > kThreshold) {
// We've seen a lot of both, so pick the one we've seen the most of.
@@ -210,9 +219,9 @@
bool is_right() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
const uint64_t left_difference =
- hot_goal_fetcher_->get()->left_count - start_counts_.left_count;
+ hot_goal_fetcher_->get()->left_count() - start_counts_.left_count;
const uint64_t right_difference =
- hot_goal_fetcher_->get()->right_count - start_counts_.right_count;
+ hot_goal_fetcher_->get()->right_count() - start_counts_.right_count;
if (right_difference > kThreshold) {
if (left_difference > kThreshold) {
// We've seen a lot of both, so pick the one we've seen the most of.
@@ -230,14 +239,14 @@
private:
static const uint64_t kThreshold = 5;
- ::y2014::HotGoal start_counts_;
+ ::y2014::HotGoalT start_counts_;
bool start_counts_valid_;
::aos::Fetcher<::y2014::HotGoal> *hot_goal_fetcher_;
};
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams & /*params*/) {
+ const ::frc971::autonomous::AutonomousActionParams * /*params*/) {
enum class AutoVersion : uint8_t {
kStraight,
kDoubleHot,
@@ -261,9 +270,9 @@
static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
- if (auto_mode_fetcher_->voltage < kSelectorStep + kSelectorMin) {
+ if (auto_mode_fetcher_->voltage() < kSelectorStep + kSelectorMin) {
auto_version = AutoVersion::kSingleHot;
- } else if (auto_mode_fetcher_->voltage < 2 * kSelectorStep + kSelectorMin) {
+ } else if (auto_mode_fetcher_->voltage() < 2 * kSelectorStep + kSelectorMin) {
auto_version = AutoVersion::kStraight;
} else {
auto_version = AutoVersion::kDoubleHot;
@@ -272,9 +281,9 @@
AOS_LOG(INFO, "running auto %" PRIu8 "\n",
static_cast<uint8_t>(auto_version));
- const ProfileParameters &drive_params =
+ const ProfileParametersT drive_params =
(auto_version == AutoVersion::kStraight) ? kFastDrive : kSlowDrive;
- const ProfileParameters &drive_with_ball_params =
+ const ProfileParametersT drive_with_ball_params =
(auto_version == AutoVersion::kStraight) ? kFastWithBallDrive
: kSlowWithBallDrive;
diff --git a/y2014/actors/autonomous_actor.h b/y2014/actors/autonomous_actor.h
index e832980..f1b06bf 100644
--- a/y2014/actors/autonomous_actor.h
+++ b/y2014/actors/autonomous_actor.h
@@ -7,12 +7,11 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2014/actors/shoot_actor.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
+#include "y2014/queues/hot_goal_generated.h"
namespace y2014 {
namespace actors {
@@ -22,7 +21,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
@@ -41,12 +40,10 @@
::aos::Fetcher<::y2014::sensors::AutoMode> auto_mode_fetcher_;
::aos::Fetcher<::y2014::HotGoal> hot_goal_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_sender_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
actors::ShootActor::Factory shoot_action_factory_;
};
diff --git a/y2014/actors/autonomous_actor_main.cc b/y2014/actors/autonomous_actor_main.cc
index 817f0e4..44e10d0 100644
--- a/y2014/actors/autonomous_actor_main.cc
+++ b/y2014/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2014/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2014/actors/shoot_action.q b/y2014/actors/shoot_action.q
deleted file mode 100644
index 832153c..0000000
--- a/y2014/actors/shoot_action.q
+++ /dev/null
@@ -1,10 +0,0 @@
-package y2014.actors;
-
-import "aos/actions/actions.q";
-
-queue_group ShootActionQueueGroup {
- implements frc971.actions.ActionQueueGroup;
-
- queue aos.common.actions.Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2014/actors/shoot_actor.cc b/y2014/actors/shoot_actor.cc
index 7fb4c05..58df186 100644
--- a/y2014/actors/shoot_actor.cc
+++ b/y2014/actors/shoot_actor.cc
@@ -4,10 +4,11 @@
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace actors {
@@ -17,26 +18,24 @@
constexpr double ShootActor::kClawShootingSeparationGoal;
ShootActor::ShootActor(::aos::EventLoop *event_loop)
- : ::aos::common::actions::ActorBase<actors::ShootActionQueueGroup>(
- event_loop, ".y2014.actors.shoot_action"),
+ : ::aos::common::actions::ActorBase<aos::common::actions::Goal>(
+ event_loop, "/shoot_action"),
claw_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Goal>("/claw")),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>("/claw")),
shooter_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ShooterQueue::Status>(
- ".y2014.control_loops.shooter_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::shooter::Status>(
+ "/shooter")),
shooter_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")) {}
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")) {}
double ShootActor::SpeedToAngleOffset(double speed) {
const constants::Values &values = constants::GetValues();
@@ -52,14 +51,17 @@
// turn it off.
return true;
} else {
- auto goal_message = claw_goal_sender_.MakeMessage();
+ auto builder = claw_goal_sender_.MakeBuilder();
- goal_message->bottom_angle = claw_goal_fetcher_->bottom_angle;
- goal_message->separation_angle = claw_goal_fetcher_->separation_angle;
- goal_message->intake = 0.0;
- goal_message->centering = 0.0;
+ control_loops::claw::Goal::Builder claw_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
- if (!goal_message.Send()) {
+ claw_builder.add_bottom_angle(claw_goal_fetcher_->bottom_angle());
+ claw_builder.add_separation_angle(claw_goal_fetcher_->separation_angle());
+ claw_builder.add_intake(0.0);
+ claw_builder.add_centering(0.0);
+
+ if (!builder.Send(claw_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
return false;
}
@@ -67,7 +69,7 @@
return true;
}
-bool ShootActor::RunAction(const double&) {
+bool ShootActor::RunAction(const aos::common::actions::DoubleParam *) {
InnerRunAction();
// Now do our 'finally' block and make sure that we aren't requesting shots
@@ -76,12 +78,15 @@
if (shooter_goal_fetcher_.get() == nullptr) {
return true;
}
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shooter_goal_fetcher_->shot_power;
- goal_message->shot_requested = false;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder shooter_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+
+ shooter_builder.add_shot_power(shooter_goal_fetcher_->shot_power());
+ shooter_builder.add_shot_requested(false);
+ shooter_builder.add_unload_requested(false);
+ shooter_builder.add_load_requested(false);
+ if (!builder.Send(shooter_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
return false;
}
@@ -104,16 +109,18 @@
shooter_status_fetcher_.Fetch();
// Get the number of shots fired up to this point. This should not be updated
// again for another few cycles.
- previous_shots_ = shooter_status_fetcher_->shots;
+ previous_shots_ = shooter_status_fetcher_->shots();
// Shoot!
shooter_goal_fetcher_.Fetch();
{
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shooter_goal_fetcher_->shot_power;
- goal_message->shot_requested = true;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(shooter_goal_fetcher_->shot_power());
+ goal_builder.add_shot_requested(true);
+ goal_builder.add_unload_requested(false);
+ goal_builder.add_load_requested(false);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
return;
}
@@ -128,20 +135,20 @@
bool ShootActor::ClawIsReady() {
claw_goal_fetcher_.Fetch();
- bool ans = claw_status_fetcher_->zeroed &&
- (::std::abs(claw_status_fetcher_->bottom_velocity) < 0.5) &&
- (::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle) < 0.10) &&
- (::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed() &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity()) < 0.5) &&
+ (::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()) < 0.4);
AOS_LOG(DEBUG,
"Claw is %sready zeroed %d bottom_velocity %f bottom %f sep %f\n",
- ans ? "" : "not ", claw_status_fetcher_->zeroed,
- ::std::abs(claw_status_fetcher_->bottom_velocity),
- ::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle),
- ::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle));
+ ans ? "" : "not ", claw_status_fetcher_->zeroed(),
+ ::std::abs(claw_status_fetcher_->bottom_velocity()),
+ ::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()),
+ ::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()));
return ans;
}
@@ -149,14 +156,14 @@
shooter_goal_fetcher_.Fetch();
AOS_LOG(DEBUG, "Power error is %f - %f -> %f, ready %d\n",
- shooter_status_fetcher_->hard_stop_power,
- shooter_goal_fetcher_->shot_power,
- ::std::abs(shooter_status_fetcher_->hard_stop_power -
- shooter_goal_fetcher_->shot_power),
- shooter_status_fetcher_->ready);
- return (::std::abs(shooter_status_fetcher_->hard_stop_power -
- shooter_goal_fetcher_->shot_power) < 1.0) &&
- shooter_status_fetcher_->ready;
+ shooter_status_fetcher_->hard_stop_power(),
+ shooter_goal_fetcher_->shot_power(),
+ ::std::abs(shooter_status_fetcher_->hard_stop_power() -
+ shooter_goal_fetcher_->shot_power()),
+ shooter_status_fetcher_->ready());
+ return (::std::abs(shooter_status_fetcher_->hard_stop_power() -
+ shooter_goal_fetcher_->shot_power()) < 1.0) &&
+ shooter_status_fetcher_->ready();
}
bool ShootActor::DoneSetupShot() {
@@ -174,7 +181,7 @@
bool ShootActor::DonePreShotOpen() {
claw_status_fetcher_.Fetch();
- if (claw_status_fetcher_->separation > kClawShootingSeparation) {
+ if (claw_status_fetcher_->separation() > kClawShootingSeparation) {
AOS_LOG(INFO, "Opened up enough to shoot.\n");
return true;
}
@@ -184,7 +191,7 @@
bool ShootActor::DoneShot() {
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get() &&
- shooter_status_fetcher_->shots > previous_shots_) {
+ shooter_status_fetcher_->shots() > previous_shots_) {
AOS_LOG(INFO, "Shot succeeded!\n");
return true;
}
diff --git a/y2014/actors/shoot_actor.h b/y2014/actors/shoot_actor.h
index a4412ff..88a2358 100644
--- a/y2014/actors/shoot_actor.h
+++ b/y2014/actors/shoot_actor.h
@@ -5,29 +5,30 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "y2014/actors/shoot_action.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace actors {
class ShootActor
- : public ::aos::common::actions::ActorBase<actors::ShootActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<aos::common::actions::Goal> {
public:
typedef ::aos::common::actions::TypedActionFactory<
- actors::ShootActionQueueGroup>
+ aos::common::actions::Goal>
Factory;
explicit ShootActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2014.actors.shoot_action");
+ return Factory(event_loop, "/shoot_action");
}
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- bool RunAction(const double ¶ms) override;
+ bool RunAction(const aos::common::actions::DoubleParam *params) override;
void InnerRunAction();
// calc an offset to our requested shot based on robot speed
@@ -50,16 +51,13 @@
bool ClawIsReady();
private:
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<::y2014::control_loops::ShooterQueue::Status>
+ ::aos::Fetcher<::y2014::control_loops::claw::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::shooter::Status>
shooter_status_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_fetcher_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::shooter::Goal> shooter_goal_fetcher_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
// to track when shot is complete
int previous_shots_;
diff --git a/y2014/actors/shoot_actor_main.cc b/y2014/actors/shoot_actor_main.cc
index 8575783..ef06915 100644
--- a/y2014/actors/shoot_actor_main.cc
+++ b/y2014/actors/shoot_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2014/actors/shoot_action.q.h"
#include "y2014/actors/shoot_actor.h"
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014::actors::ShootActor shoot(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 6c162e6..670dd76 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -1,16 +1,40 @@
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 = "claw_queue",
+flatbuffer_cc_library(
+ name = "claw_goal_fbs",
srcs = [
- "claw.q",
+ "claw_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_position_fbs",
+ srcs = [
+ "claw_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "claw_output_fbs",
+ srcs = [
+ "claw_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_status_fbs",
+ srcs = [
+ "claw_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -40,12 +64,13 @@
"-lm",
],
deps = [
- ":claw_queue",
+ ":claw_goal_fbs",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:hall_effect_tracker",
"//frc971/control_loops:state_feedback_loop",
@@ -58,9 +83,13 @@
srcs = [
"claw_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":claw_goal_fbs",
":claw_lib",
- ":claw_queue",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -76,6 +105,6 @@
deps = [
":claw_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 14dbf11..56ca6b0 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -2,10 +2,7 @@
#include <algorithm>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/commonmath.h"
#include "y2014/constants.h"
@@ -46,11 +43,11 @@
namespace y2014 {
namespace control_loops {
+namespace claw {
using ::frc971::HallEffectTracker;
using ::y2014::control_loops::claw::kDt;
using ::frc971::control_loops::DoCoerceGoal;
-using ::y2014::control_loops::ClawPositionToLog;
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -94,7 +91,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
- AOS_LOG_MATRIX(DEBUG, "U at start", U());
+ VLOG(1) << "U at start " << U();
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -116,7 +113,7 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
- AOS_LOG_MATRIX(DEBUG, "error", error);
+ VLOG(1) << "error " << error;
const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
@@ -183,9 +180,9 @@
}
}
- AOS_LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ VLOG(1) << "adjusted_pos_error " << adjusted_pos_error;
mutable_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
- AOS_LOG_MATRIX(DEBUG, "U is now", U());
+ VLOG(1) << "U is now" << U();
{
const auto values = constants::GetValues().claw;
@@ -221,69 +218,68 @@
encoder_(0.0),
last_encoder_(0.0) {}
-void ZeroedStateFeedbackLoop::SetPositionValues(
- const ::y2014::control_loops::HalfClawPosition &claw) {
- front_.Update(claw.front);
- calibration_.Update(claw.calibration);
- back_.Update(claw.back);
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition *claw) {
+ front_.Update(claw->front());
+ calibration_.Update(claw->calibration());
+ back_.Update(claw->back());
bool any_sensor_triggered = any_triggered();
if (any_sensor_triggered && any_triggered_last_) {
// We are still on the hall effect and nothing has changed.
min_hall_effect_on_angle_ =
- ::std::min(min_hall_effect_on_angle_, claw.position);
+ ::std::min(min_hall_effect_on_angle_, claw->position());
max_hall_effect_on_angle_ =
- ::std::max(max_hall_effect_on_angle_, claw.position);
+ ::std::max(max_hall_effect_on_angle_, claw->position());
} else if (!any_sensor_triggered && !any_triggered_last_) {
// We are still off the hall effect and nothing has changed.
min_hall_effect_off_angle_ =
- ::std::min(min_hall_effect_off_angle_, claw.position);
+ ::std::min(min_hall_effect_off_angle_, claw->position());
max_hall_effect_off_angle_ =
- ::std::max(max_hall_effect_off_angle_, claw.position);
+ ::std::max(max_hall_effect_off_angle_, claw->position());
}
if (front_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.front.posedge_value, claw.position);
+ ::std::min(claw->front()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.front.posedge_value, claw.position);
+ ::std::max(claw->front()->posedge_value(), claw->position());
}
if (calibration_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.calibration.posedge_value, claw.position);
+ ::std::min(claw->calibration()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.calibration.posedge_value, claw.position);
+ ::std::max(claw->calibration()->posedge_value(), claw->position());
}
if (back_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.back.posedge_value, claw.position);
+ ::std::min(claw->back()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.back.posedge_value, claw.position);
+ ::std::max(claw->back()->posedge_value(), claw->position());
}
if (front_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.front.negedge_value, claw.position);
+ ::std::min(claw->front()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.front.negedge_value, claw.position);
+ ::std::max(claw->front()->negedge_value(), claw->position());
}
if (calibration_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.calibration.negedge_value, claw.position);
+ ::std::min(claw->calibration()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.calibration.negedge_value, claw.position);
+ ::std::max(claw->calibration()->negedge_value(), claw->position());
}
if (back_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.back.negedge_value, claw.position);
+ ::std::min(claw->back()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.back.negedge_value, claw.position);
+ ::std::max(claw->back()->negedge_value(), claw->position());
}
last_encoder_ = encoder_;
@@ -292,23 +288,22 @@
} else {
last_off_encoder_ = encoder_;
}
- encoder_ = claw.position;
+ encoder_ = claw->position();
any_triggered_last_ = any_sensor_triggered;
}
-void ZeroedStateFeedbackLoop::Reset(
- const ::y2014::control_loops::HalfClawPosition &claw) {
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition *claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset(claw.front);
- calibration_.Reset(claw.calibration);
- back_.Reset(claw.back);
+ front_.Reset(claw->front());
+ calibration_.Reset(claw->calibration());
+ back_.Reset(claw->back());
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
- min_hall_effect_on_angle_ = claw.position;
- max_hall_effect_on_angle_ = claw.position;
- min_hall_effect_off_angle_ = claw.position;
- max_hall_effect_off_angle_ = claw.position;
+ min_hall_effect_on_angle_ = claw->position();
+ max_hall_effect_on_angle_ = claw->position();
+ min_hall_effect_off_angle_ = claw->position();
+ max_hall_effect_off_angle_ = claw->position();
any_triggered_last_ = any_triggered();
}
@@ -374,8 +369,8 @@
}
ClawMotor::ClawMotor(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ClawQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -563,45 +558,33 @@
// first update position based on angle limit
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.soft_max_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (separation < values.claw.soft_min_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
// now move both goals in unison
if (*bottom_goal < values.claw.lower_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
*bottom_goal = values.claw.lower_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*bottom_goal > values.claw.lower_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
*bottom_goal = values.claw.lower_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal < values.claw.upper_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
*top_goal = values.claw.upper_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal > values.claw.upper_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
*top_goal = values.claw.upper_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
}
@@ -609,7 +592,7 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- ((has_joystick_state() ? joystick_state().autonomous : true) &&
+ ((has_joystick_state() ? joystick_state().autonomous() : true) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -621,43 +604,45 @@
bool ClawMotor::is_zeroing() const { return !is_ready(); }
// Positive angle is up, and positive power is up.
-void ClawMotor::RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status) {
+void ClawMotor::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
// Disable the motors now so that all early returns will return with the
// motors disabled.
+ OutputT output_struct;
if (output) {
- output->top_claw_voltage = 0;
- output->bottom_claw_voltage = 0;
- output->intake_voltage = 0;
- output->tusk_voltage = 0;
+ output_struct.top_claw_voltage = 0;
+ output_struct.bottom_claw_voltage = 0;
+ output_struct.intake_voltage = 0;
+ output_struct.tusk_voltage = 0;
}
+ StatusT status_struct;
if (goal) {
- if (::std::isnan(goal->bottom_angle) ||
- ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
- ::std::isnan(goal->centering)) {
+ if (::std::isnan(goal->bottom_angle()) ||
+ ::std::isnan(goal->separation_angle()) ||
+ ::std::isnan(goal->intake()) || ::std::isnan(goal->centering())) {
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
return;
}
}
if (WasReset()) {
- top_claw_.Reset(position->top);
- bottom_claw_.Reset(position->bottom);
+ top_claw_.Reset(position->top());
+ bottom_claw_.Reset(position->bottom());
}
const constants::Values &values = constants::GetValues();
if (position) {
Eigen::Matrix<double, 2, 1> Y;
- Y << position->bottom.position + bottom_claw_.offset(),
- position->top.position + top_claw_.offset();
+ Y << position->bottom()->position() + bottom_claw_.offset(),
+ position->top()->position() + top_claw_.offset();
claw_.Correct(Y);
- top_claw_.SetPositionValues(position->top);
- bottom_claw_.SetPositionValues(position->bottom);
+ top_claw_.SetPositionValues(position->top());
+ bottom_claw_.SetPositionValues(position->bottom());
if (!has_top_claw_goal_) {
has_top_claw_goal_ = true;
@@ -671,15 +656,12 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- AOS_LOG_STRUCT(DEBUG, "absolute position",
- ClawPositionToLog(top_claw_.absolute_position(),
- bottom_claw_.absolute_position()));
}
bool autonomous, enabled;
if (has_joystick_state()) {
- autonomous = joystick_state().autonomous;
- enabled = joystick_state().enabled;
+ autonomous = joystick_state().autonomous();
+ enabled = joystick_state().enabled();
} else {
autonomous = true;
enabled = false;
@@ -700,8 +682,8 @@
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
- bottom_claw_goal_ = goal->bottom_angle;
- top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
+ bottom_claw_goal_ = goal->bottom_angle();
+ top_claw_goal_ = goal->bottom_angle() + goal->separation_angle();
has_bottom_claw_goal_ = true;
has_top_claw_goal_ = true;
doing_calibration_fine_tune_ = false;
@@ -759,7 +741,7 @@
bottom_claw_.back())) {
// do calibration
bottom_claw_.SetCalibration(
- position->bottom.calibration.posedge_value,
+ position->bottom()->calibration()->posedge_value(),
values.claw.lower_claw.calibration.lower_angle);
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning bottom
@@ -816,7 +798,7 @@
top_claw_.front(), top_claw_.back())) {
// do calibration
top_claw_.SetCalibration(
- position->top.calibration.posedge_value,
+ position->top()->calibration()->posedge_value(),
values.claw.upper_claw.calibration.lower_angle);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning top
@@ -836,10 +818,11 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position + top_claw_.offset();
- bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
+ top_claw_goal_ = position->top()->position() + top_claw_.offset();
+ bottom_claw_goal_ =
+ position->bottom()->position() + bottom_claw_.offset();
initial_separation_ =
- position->top.position - position->bottom.position;
+ position->top()->position() - position->bottom()->position();
} else {
has_top_claw_goal_ = false;
has_bottom_claw_goal_ = false;
@@ -907,7 +890,6 @@
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.mutable_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
- AOS_LOG_MATRIX(DEBUG, "actual goal", claw_.R());
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -973,55 +955,60 @@
if (output) {
if (goal) {
//setup the intake
- output->intake_voltage =
- (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
- : goal->intake;
- output->tusk_voltage = goal->centering;
- output->tusk_voltage =
- (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ output_struct.intake_voltage =
+ (goal->intake() > 12.0)
+ ? 12
+ : (goal->intake() < -12.0) ? -12.0 : goal->intake();
+ output_struct.tusk_voltage = goal->centering();
+ output_struct.tusk_voltage =
+ (goal->centering() > 12.0) ? 12 : (goal->centering() < -12.0)
? -12.0
- : goal->centering;
+ : goal->centering();
}
- output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output_struct.top_claw_voltage = claw_.U(1, 0);
+ output_struct.bottom_claw_voltage = claw_.U(0, 0);
- if (output->top_claw_voltage > kMaxVoltage) {
- output->top_claw_voltage = kMaxVoltage;
- } else if (output->top_claw_voltage < -kMaxVoltage) {
- output->top_claw_voltage = -kMaxVoltage;
+ if (output_struct.top_claw_voltage > kMaxVoltage) {
+ output_struct.top_claw_voltage = kMaxVoltage;
+ } else if (output_struct.top_claw_voltage < -kMaxVoltage) {
+ output_struct.top_claw_voltage = -kMaxVoltage;
}
- if (output->bottom_claw_voltage > kMaxVoltage) {
- output->bottom_claw_voltage = kMaxVoltage;
- } else if (output->bottom_claw_voltage < -kMaxVoltage) {
- output->bottom_claw_voltage = -kMaxVoltage;
+ if (output_struct.bottom_claw_voltage > kMaxVoltage) {
+ output_struct.bottom_claw_voltage = kMaxVoltage;
+ } else if (output_struct.bottom_claw_voltage < -kMaxVoltage) {
+ output_struct.bottom_claw_voltage = -kMaxVoltage;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
- status->bottom = bottom_absolute_position();
- status->separation = top_absolute_position() - bottom_absolute_position();
- status->bottom_velocity = claw_.X_hat(2, 0);
- status->separation_velocity = claw_.X_hat(3, 0);
+ status_struct.bottom = bottom_absolute_position();
+ status_struct.separation =
+ top_absolute_position() - bottom_absolute_position();
+ status_struct.bottom_velocity = claw_.X_hat(2, 0);
+ status_struct.separation_velocity = claw_.X_hat(3, 0);
if (goal) {
bool bottom_done =
- ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
- bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle()) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status_struct.bottom_velocity) < 0.2;
bool separation_done =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.020;
+ goal->separation_angle()) < 0.020;
bool separation_done_with_ball =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.06;
- status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
- status->done_with_ball =
- is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ goal->separation_angle()) < 0.06;
+ status_struct.done =
+ is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status_struct.done_with_ball = is_ready() && separation_done_with_ball &&
+ bottom_done && bottom_velocity_done;
} else {
- status->done = status->done_with_ball = false;
+ status_struct.done = status_struct.done_with_ball = false;
}
- status->zeroed = is_ready();
- status->zeroed_for_auto =
+ status_struct.zeroed = is_ready();
+ status_struct.zeroed_for_auto =
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -1029,8 +1016,11 @@
bottom_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+
was_enabled_ = enabled;
}
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index 082ddce..5bcccc1 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -5,15 +5,19 @@
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
-#include "y2014/constants.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/claw/claw_motor_plant.h"
#include "frc971/control_loops/hall_effect_tracker.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
class WindupClawTest;
};
@@ -77,9 +81,9 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- void SetPositionValues(const ::y2014::control_loops::HalfClawPosition &claw);
+ void SetPositionValues(const HalfClawPosition *claw);
- void Reset(const ::y2014::control_loops::HalfClawPosition &claw);
+ void Reset(const HalfClawPosition *claw);
double absolute_position() const { return encoder() + offset(); }
@@ -183,11 +187,10 @@
};
class ClawMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ClawQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ClawMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.claw_queue");
+ explicit ClawMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/claw");
// True if the state machine is ready.
bool capped_goal() const { return capped_goal_; }
@@ -217,11 +220,9 @@
CalibrationMode mode() const { return mode_; }
protected:
- virtual void RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status);
+ virtual void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
@@ -262,6 +263,7 @@
void LimitClawGoal(double *bottom_goal, double *top_goal,
const constants::Values &values);
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
deleted file mode 100644
index ff4c8b8..0000000
--- a/y2014/control_loops/claw/claw.q
+++ /dev/null
@@ -1,74 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct HalfClawPosition {
- // The current position of this half of the claw.
- double position;
-
- // The hall effect sensor at the front limit.
- .frc971.HallEffectStruct front;
- // The hall effect sensor in the middle to use for real calibration.
- .frc971.HallEffectStruct calibration;
- // The hall effect at the back limit.
- .frc971.HallEffectStruct back;
-};
-
-// All angles here are 0 vertical, positive "up" (aka backwards).
-// Published on ".y2014.control_loops.claw_queue"
-queue_group ClawQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The angle of the bottom claw.
- double bottom_angle;
- // How much higher the top claw is.
- double separation_angle;
- // top claw intake roller
- double intake;
- // bottom claw tusk centering
- double centering;
- };
-
- message Position {
- // All the top claw information.
- HalfClawPosition top;
- // All the bottom claw information.
- HalfClawPosition bottom;
- };
-
- message Output {
- double intake_voltage;
- double top_claw_voltage;
- double bottom_claw_voltage;
- double tusk_voltage;
- };
-
- message Status {
- // True if zeroed enough for the current period (autonomous or teleop).
- bool zeroed;
- // True if zeroed as much as we will force during autonomous.
- bool zeroed_for_auto;
- // True if zeroed and within tolerance for separation and bottom angle.
- bool done;
- // True if zeroed and within tolerance for separation and bottom angle.
- // seperation allowance much wider as a ball may be included
- bool done_with_ball;
- // Dump the values of the state matrix.
- double bottom;
- double bottom_velocity;
- double separation;
- double separation_velocity;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ClawPositionToLog {
- double top;
- double bottom;
-};
diff --git a/y2014/control_loops/claw/claw_goal.fbs b/y2014/control_loops/claw/claw_goal.fbs
new file mode 100644
index 0000000..eb5860c
--- /dev/null
+++ b/y2014/control_loops/claw/claw_goal.fbs
@@ -0,0 +1,15 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Goal {
+ // The angle of the bottom claw.
+ bottom_angle:double;
+ // How much higher the top claw is.
+ separation_angle:double;
+ // top claw intake roller
+ intake:double;
+ // bottom claw tusk centering
+ centering:double;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 41dbfca..55e186e 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -8,16 +8,18 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/claw/claw.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
-using ::y2014::control_loops::claw::MakeClawPlant;
-using ::frc971::HallEffectStruct;
-using ::y2014::control_loops::HalfClawPosition;
+using ::frc971::HallEffectStructT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -38,10 +40,8 @@
double initial_top_position,
double initial_bottom_position)
: event_loop_(event_loop),
- claw_position_sender_(event_loop_->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")),
- claw_output_fetcher_(event_loop_->MakeFetcher<ClawQueue::Output>(
- ".y2014.control_loops.claw_queue.output")),
+ claw_position_sender_(event_loop_->MakeSender<Position>("/claw")),
+ claw_output_fetcher_(event_loop_->MakeFetcher<Output>("/claw")),
claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())) {
Reinitialize(initial_top_position, initial_bottom_position);
@@ -69,7 +69,9 @@
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
- last_position_.Zero();
+
+ last_position_.top.reset();
+ last_position_.bottom.reset();
SetPhysicalSensors(&last_position_);
}
@@ -99,44 +101,58 @@
void SetHallEffect(double pos,
const constants::Values::Claws::AnglePair &pair,
- HallEffectStruct *hall_effect) {
+ HallEffectStructT *hall_effect) {
hall_effect->current = CheckRange(pos, pair);
}
void SetClawHallEffects(double pos,
const constants::Values::Claws::Claw &claw,
- HalfClawPosition *half_claw) {
- SetHallEffect(pos, claw.front, &half_claw->front);
- SetHallEffect(pos, claw.calibration, &half_claw->calibration);
- SetHallEffect(pos, claw.back, &half_claw->back);
+ HalfClawPositionT *half_claw) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ SetHallEffect(pos, claw.front, half_claw->front.get());
+ SetHallEffect(pos, claw.calibration, half_claw->calibration.get());
+ SetHallEffect(pos, claw.back, half_claw->back.get());
}
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ClawQueue::Position *position) {
- position->top.position = GetPosition(TOP_CLAW);
- position->bottom.position = GetPosition(BOTTOM_CLAW);
+ void SetPhysicalSensors(PositionT *position) {
+ if (!position->top) {
+ position->top.reset(new HalfClawPositionT());
+ }
+ if (!position->bottom) {
+ position->bottom.reset(new HalfClawPositionT());
+ }
+
+ position->top->position = GetPosition(TOP_CLAW);
+ position->bottom->position = GetPosition(BOTTOM_CLAW);
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
AOS_LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n",
pos[TOP_CLAW], pos[BOTTOM_CLAW]);
- const constants::Values& values = constants::GetValues();
+ const constants::Values &values = constants::GetValues();
// Signal that each hall effect sensor has been triggered if it is within
// the correct range.
- SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw, &position->top);
+ SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw,
+ position->top.get());
SetClawHallEffects(pos[BOTTOM_CLAW], values.claw.lower_claw,
- &position->bottom);
+ position->bottom.get());
}
- void UpdateHallEffect(double angle,
- double last_angle,
- double initial_position,
- HallEffectStruct *position,
- const HallEffectStruct &last_position,
+ void UpdateHallEffect(double angle, double last_angle,
+ double initial_position, HallEffectStructT *position,
+ const HallEffectStructT &last_position,
const constants::Values::Claws::AnglePair &pair,
const char *claw_name, const char *hall_effect_name) {
if (position->current && !last_position.current) {
@@ -167,49 +183,64 @@
}
}
- void UpdateClawHallEffects(
- HalfClawPosition *position,
- const HalfClawPosition &last_position,
- const constants::Values::Claws::Claw &claw, double initial_position,
- const char *claw_name) {
- UpdateHallEffect(position->position + initial_position,
+ void UpdateClawHallEffects(HalfClawPositionT *half_claw,
+ const HalfClawPositionT &last_position,
+ const constants::Values::Claws::Claw &claw,
+ double initial_position, const char *claw_name) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->front, last_position.front,
- claw.front, claw_name, "front");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->front.get(),
+ *last_position.front.get(), claw.front, claw_name, "front");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->calibration,
- last_position.calibration, claw.calibration, claw_name,
- "calibration");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->calibration.get(),
+ *last_position.calibration.get(), claw.calibration,
+ claw_name, "calibration");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->back, last_position.back,
- claw.back, claw_name, "back");
+ initial_position, half_claw->back.get(),
+ *last_position.back.get(), claw.back, claw_name, "back");
}
// Sends out the position queue messages.
void SendPositionMessage() {
- ::aos::Sender<::y2014::control_loops::ClawQueue::Position>::Message
- position = claw_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ claw_position_sender_.MakeBuilder();
// Initialize all the counters to their previous values.
- *position = last_position_;
- SetPhysicalSensors(position.get());
+ PositionT position;
- const constants::Values& values = constants::GetValues();
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<Position> position_offset =
+ Position::Pack(fbb, &last_position_);
+ fbb.Finish(position_offset);
- UpdateClawHallEffects(&position->top, last_position_.top,
+ flatbuffers::GetRoot<Position>(fbb.GetBufferPointer())->UnPackTo(&position);
+ SetPhysicalSensors(&position);
+
+ const constants::Values &values = constants::GetValues();
+
+ UpdateClawHallEffects(position.top.get(), *last_position_.top.get(),
values.claw.upper_claw, initial_position_[TOP_CLAW],
"Top");
- UpdateClawHallEffects(&position->bottom, last_position_.bottom,
+ UpdateClawHallEffects(position.bottom.get(), *last_position_.bottom.get(),
values.claw.lower_claw,
initial_position_[BOTTOM_CLAW], "Bottom");
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- last_position_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+ last_position_ = std::move(position);
}
// Simulates the claw moving for one timestep.
@@ -218,8 +249,8 @@
EXPECT_TRUE(claw_output_fetcher_.Fetch());
Eigen::Matrix<double, 2, 1> U;
- U << claw_output_fetcher_->bottom_claw_voltage,
- claw_output_fetcher_->top_claw_voltage;
+ U << claw_output_fetcher_->bottom_claw_voltage(),
+ claw_output_fetcher_->top_claw_voltage();
claw_plant_->Update(U);
// Check that the claw is within the limits.
@@ -237,8 +268,8 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::aos::Fetcher<ClawQueue::Output> claw_output_fetcher_;
+ ::aos::Sender<Position> claw_position_sender_;
+ ::aos::Fetcher<Output> claw_output_fetcher_;
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -252,18 +283,18 @@
double initial_position_[CLAW_COUNT];
- ::y2014::control_loops::ClawQueue::Position last_position_;
+ PositionT last_position_;
};
class ClawTest : public ::aos::testing::ControlLoopTest {
protected:
ClawTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2014/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- claw_goal_sender_(test_event_loop_->MakeSender<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
- claw_goal_fetcher_(test_event_loop_->MakeFetcher<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ claw_goal_sender_(test_event_loop_->MakeSender<Goal>("/claw")),
+ claw_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/claw")),
claw_event_loop_(MakeEventLoop()),
claw_motor_(claw_event_loop_.get()),
claw_plant_event_loop_(MakeEventLoop()),
@@ -275,14 +306,14 @@
double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
double separation =
claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
- EXPECT_NEAR(claw_goal_fetcher_->bottom_angle, bottom, 1e-4);
- EXPECT_NEAR(claw_goal_fetcher_->separation_angle, separation, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->bottom_angle(), bottom, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->separation_angle(), separation, 1e-4);
EXPECT_LE(min_separation_, separation);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<ClawQueue::Goal> claw_goal_fetcher_;
+ ::aos::Sender<Goal> claw_goal_sender_;
+ ::aos::Fetcher<Goal> claw_goal_fetcher_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> claw_event_loop_;
@@ -294,15 +325,15 @@
// Minimum amount of acceptable separation between the top and bottom of the
// claw.
double min_separation_;
-
};
TEST_F(ClawTest, HandlesNAN) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = ::std::nan("");
- message->separation_angle = ::std::nan("");
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(::std::nan(""));
+ goal_builder.add_separation_angle(::std::nan(""));
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -312,10 +343,11 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -409,10 +441,11 @@
claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -586,10 +619,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupPositive) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -602,10 +636,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegative) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -618,10 +653,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegativeFineTune) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::FINE_TUNE_BOTTOM,
@@ -631,5 +667,6 @@
}
} // namespace testing
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
index 65627d5..f21c7c5 100644
--- a/y2014/control_loops/claw/claw_main.cc
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/claw/claw.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ClawMotor claw(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::claw::ClawMotor claw(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/claw/claw_output.fbs b/y2014/control_loops/claw/claw_output.fbs
new file mode 100644
index 0000000..8cb42e0
--- /dev/null
+++ b/y2014/control_loops/claw/claw_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Output {
+ intake_voltage:double;
+ top_claw_voltage:double;
+ bottom_claw_voltage:double;
+ tusk_voltage:double;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/claw/claw_position.fbs b/y2014/control_loops/claw/claw_position.fbs
new file mode 100644
index 0000000..17d9f17
--- /dev/null
+++ b/y2014/control_loops/claw/claw_position.fbs
@@ -0,0 +1,25 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table HalfClawPosition {
+ // The current position of this half of the claw.
+ position:double;
+
+ // The hall effect sensor at the front limit.
+ front:frc971.HallEffectStruct;
+ // The hall effect sensor in the middle to use for real calibration.
+ calibration:frc971.HallEffectStruct;
+ // The hall effect at the back limit.
+ back:frc971.HallEffectStruct;
+}
+
+table Position {
+ // All the top claw information.
+ top:HalfClawPosition;
+ // All the bottom claw information.
+ bottom:HalfClawPosition;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/claw/claw_status.fbs b/y2014/control_loops/claw/claw_status.fbs
new file mode 100644
index 0000000..eda9279
--- /dev/null
+++ b/y2014/control_loops/claw/claw_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ zeroed:bool;
+ // True if zeroed as much as we will force during autonomous.
+ zeroed_for_auto:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ done:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ done_with_ball:bool;
+ // Dump the values of the state matrix.
+ bottom:double;
+ bottom_velocity:double;
+ separation:double;
+ separation_velocity:double;
+}
+
+root_type Status;
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 858f7e0..c458208 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -77,7 +75,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 7749631..c210770 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/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 "y2014/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index 075cafc..a2073c9 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -1,16 +1,40 @@
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 = "shooter_queue",
+flatbuffer_cc_library(
+ name = "shooter_goal_fbs",
srcs = [
- "shooter.q",
+ "shooter_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_position_fbs",
+ srcs = [
+ "shooter_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "shooter_output_fbs",
+ srcs = [
+ "shooter_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_status_fbs",
+ srcs = [
+ "shooter_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -44,9 +68,12 @@
"-lm",
],
deps = [
- ":shooter_queue",
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//y2014:constants",
],
@@ -57,9 +84,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -75,6 +106,6 @@
deps = [
":shooter_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index bf14aad..fb115c7 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -6,15 +6,13 @@
#include <limits>
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
@@ -45,10 +43,6 @@
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- AOS_LOG_STRUCT(
- DEBUG, "shooter_output",
- ::y2014::control_loops::ShooterVoltageToLog(X_hat(2, 0), voltage_));
-
last_voltage_ = voltage_;
capped_goal_ = false;
}
@@ -67,8 +61,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (index() == 0) {
@@ -82,8 +74,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -100,7 +90,6 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
@@ -110,16 +99,12 @@
if (index() == 0) {
mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
- AOS_LOG_STRUCT(DEBUG, "sensor edge (fake?)",
- ::y2014::control_loops::ShooterChangeCalibration(
- encoder_val, known_position, old_position,
- absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue>(
- event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
cycles_not_moved_(0),
@@ -137,12 +122,8 @@
(kMaxExtension - values.shooter.upper_limit) *
(kMaxExtension - values.shooter.upper_limit));
if (power < 0) {
- AOS_LOG_STRUCT(WARNING, "negative power",
- ::y2014::control_loops::PowerAdjustment(power, 0));
power = 0;
} else if (power > maxpower) {
- AOS_LOG_STRUCT(WARNING, "power too high",
- ::y2014::control_loops::PowerAdjustment(power, maxpower));
power = maxpower;
}
@@ -166,23 +147,23 @@
return power;
}
-void ShooterMotor::CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position) {
+void ShooterMotor::CheckCalibrations(const Position *position) {
AOS_CHECK_NOTNULL(position);
const constants::Values &values = constants::GetValues();
// TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
- if (position->pusher_proximal.posedge_count != last_proximal_posedge_count_ &&
+ if (position->pusher_proximal()->posedge_count() !=
+ last_proximal_posedge_count_ &&
!last_proximal_current_) {
proximal_posedge_validation_cycles_left_ = 2;
}
if (proximal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_proximal.current) {
+ if (position->pusher_proximal()->current()) {
--proximal_posedge_validation_cycles_left_;
if (proximal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_proximal.posedge_value,
+ position->pusher_proximal()->posedge_value(),
values.shooter.pusher_proximal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using proximal sensor\n");
@@ -193,16 +174,17 @@
}
}
- if (position->pusher_distal.posedge_count != last_distal_posedge_count_ &&
+ if (position->pusher_distal()->posedge_count() !=
+ last_distal_posedge_count_ &&
!last_distal_current_) {
distal_posedge_validation_cycles_left_ = 2;
}
if (distal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_distal.current) {
+ if (position->pusher_distal()->current()) {
--distal_posedge_validation_cycles_left_;
if (distal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_distal.posedge_value,
+ position->pusher_distal()->posedge_value(),
values.shooter.pusher_distal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using distal sensor\n");
@@ -216,43 +198,44 @@
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) {
+ const Goal *goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
- if (goal && ::std::isnan(goal->shot_power)) {
+ if (goal && ::std::isnan(goal->shot_power())) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because got a shot power of NAN.\n");
}
// we must always have these or we have issues.
if (status == NULL) {
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
AOS_LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
- status->ready = false;
+ bool status_ready = false;
if (WasReset()) {
state_ = STATE_INITIALIZE;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
if (position) {
- shooter_.CorrectPosition(position->position);
+ shooter_.CorrectPosition(position->position());
}
// Disable the motors now so that all early returns will return with the
// motors disabled.
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
const constants::Values &values = constants::GetValues();
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- const bool disabled = !has_joystick_state() || !joystick_state().enabled;
+ const bool disabled = !has_joystick_state() || !joystick_state().enabled();
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -263,7 +246,7 @@
if (position) {
int last_index = shooter_.index();
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
// Use the controller without the spring if the latch is set and the
// plunger is back
shooter_.set_index(1);
@@ -280,27 +263,27 @@
case STATE_INITIALIZE:
if (position) {
// Reinitialize the internal filter state.
- shooter_.InitializeState(position->position);
+ shooter_.InitializeState(position->position());
// Start off with the assumption that we are at the value
// futhest back given our sensors.
- if (position->pusher_distal.current) {
- shooter_.SetCalibration(position->position,
+ if (position->pusher_distal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_distal.lower_angle);
- } else if (position->pusher_proximal.current) {
- shooter_.SetCalibration(position->position,
+ } else if (position->pusher_proximal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_proximal.upper_angle);
} else {
- shooter_.SetCalibration(position->position,
+ shooter_.SetCalibration(position->position(),
values.shooter.upper_limit);
}
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
brake_piston_ = false;
- if (position->latch == latch_piston_) {
+ if (position->latch() == latch_piston_) {
state_ = STATE_REQUEST_LOAD;
} else {
shooter_loop_disable = true;
@@ -317,20 +300,20 @@
case STATE_REQUEST_LOAD:
if (position) {
zeroed_ = false;
- if (position->pusher_distal.current ||
- position->pusher_proximal.current) {
+ if (position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
state_ = STATE_LOAD_BACKTRACK;
// The plunger is already back and latched. Don't release it.
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
latch_piston_ = true;
} else {
latch_piston_ = false;
}
- } else if (position->plunger && position->latch) {
+ } else if (position->plunger() && position->latch()) {
// The plunger is back and we are latched. We most likely got here
// from Initialize, in which case we want to 'load' again anyways to
// zero.
@@ -361,11 +344,11 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current &&
- !position->pusher_proximal.current) {
+ if (!position->pusher_distal()->current() &&
+ !position->pusher_proximal()->current()) {
Load(monotonic_now);
}
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
}
brake_piston_ = false;
@@ -391,18 +374,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current || position->plunger;
+ latch_piston_ =
+ position->pusher_distal()->current() || position->plunger();
// Check if we are latched and back. Make sure the plunger is all the
// way back as well.
- if (position->plunger && position->latch &&
- position->pusher_distal.current) {
+ if (position->plunger() && position->latch() &&
+ position->pusher_distal()->current()) {
if (!zeroed_) {
state_ = STATE_REQUEST_LOAD;
} else {
state_ = STATE_PREPARE_SHOT;
}
- } else if (position->plunger &&
+ } else if (position->plunger() &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
@@ -415,10 +399,9 @@
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
- if (!(position->pusher_distal.current ||
- position->pusher_proximal.current) ||
- (load_timeout_ + chrono::milliseconds(500) <
- monotonic_now)) {
+ if (!(position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) ||
+ (load_timeout_ + chrono::milliseconds(500) < monotonic_now)) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -436,7 +419,7 @@
if (monotonic_now > loading_problem_end_time_) {
// Timeout by unloading.
Unload(monotonic_now);
- } else if (position && position->plunger && position->latch) {
+ } else if (position && position->plunger() && position->latch()) {
// If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
}
@@ -448,7 +431,7 @@
shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
if (position) {
AOS_LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger, position->latch);
+ position->plunger(), position->latch());
}
latch_piston_ = true;
@@ -459,16 +442,16 @@
// TODO(austin): Timeout. Low priority.
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
AOS_LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
shooter_.absolute_position(),
- goal ? PowerToPosition(goal->shot_power)
+ goal ? PowerToPosition(goal->shot_power())
: ::std::numeric_limits<double>::quiet_NaN());
if (goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.001 &&
+ PowerToPosition(goal->shot_power())) < 0.001 &&
::std::abs(shooter_.absolute_velocity()) < 0.005) {
// We are there, set the brake and move on.
latch_piston_ = true;
@@ -479,7 +462,7 @@
latch_piston_ = true;
brake_piston_ = false;
}
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -488,12 +471,12 @@
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
if (monotonic_now > shooter_brake_set_time_) {
- status->ready = true;
+ status_ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
AOS_LOG(DEBUG, "Brake is now set\n");
- if (goal && goal->shot_requested && !disabled) {
+ if (goal && goal->shot_requested() && !disabled) {
AOS_LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -503,24 +486,24 @@
}
if (state_ == STATE_READY && goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
+ PowerToPosition(goal->shot_power())) > 0.002) {
// TODO(austin): Add a state to release the brake.
// TODO(austin): Do we want to set the brake here or after shooting?
// Depends on air usage.
- status->ready = false;
+ status_ready = false;
AOS_LOG(DEBUG, "Preparing shot again.\n");
state_ = STATE_PREPARE_SHOT;
}
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
latch_piston_ = true;
brake_piston_ = true;
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -528,7 +511,7 @@
case STATE_FIRE:
if (disabled) {
if (position) {
- if (position->plunger) {
+ if (position->plunger()) {
// If disabled and the plunger is still back there, reset the
// timeout.
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -537,7 +520,7 @@
}
shooter_loop_disable = true;
// Count the number of contiguous cycles during which we haven't moved.
- if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+ if (::std::abs(last_position_position_ - shooter_.absolute_position()) <
0.0005) {
++cycles_not_moved_;
} else {
@@ -551,7 +534,7 @@
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
monotonic_now > shot_end_time_) &&
- robot_state().voltage_battery > 10.5) {
+ robot_state().voltage_battery() > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
}
@@ -566,9 +549,9 @@
// the plunger.
bool all_back;
if (position) {
- all_back = position->plunger && position->latch;
+ all_back = position->plunger() && position->latch();
} else {
- all_back = last_position_.plunger && last_position_.latch;
+ all_back = last_position_plunger_ && last_position_latch_;
}
if (all_back) {
@@ -632,7 +615,7 @@
brake_piston_ = false;
} break;
case STATE_READY_UNLOAD:
- if (goal && goal->load_requested) {
+ if (goal && goal->load_requested()) {
state_ = STATE_REQUEST_LOAD;
}
// If we are ready to load again,
@@ -652,9 +635,6 @@
}
if (!shooter_loop_disable) {
- AOS_LOG_STRUCT(DEBUG, "running the loop",
- ::y2014::control_loops::ShooterStatusToLog(
- shooter_.goal_position(), shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -664,49 +644,55 @@
}
// We don't really want to output anything if we went through everything
// assuming the motors weren't working.
- if (output) output->voltage = shooter_.voltage();
+ if (output) output_struct.voltage = shooter_.voltage();
} else {
shooter_.Update(true);
shooter_.ZeroPower();
- if (output) output->voltage = 0.0;
+ if (output) output_struct.voltage = 0.0;
}
- status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_ready(status_ready);
+ status_builder.add_hard_stop_power(
+ PositionToPower(shooter_.absolute_position()));
if (output) {
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+ output_struct.latch_piston = latch_piston_;
+ output_struct.brake_piston = brake_piston_;
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (position) {
- AOS_LOG_STRUCT(
- DEBUG, "internal state",
- ::y2014::control_loops::ShooterStateToLog(
- shooter_.absolute_position(), shooter_.absolute_velocity(), state_,
- position->latch, position->pusher_proximal.current,
- position->pusher_distal.current, position->plunger, brake_piston_,
- latch_piston_));
+ last_position_latch_ = position->latch();
+ last_position_plunger_ = position->plunger();
+ last_position_position_ = position->position();
- last_position_ = *position;
-
- last_distal_posedge_count_ = position->pusher_distal.posedge_count;
- last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_posedge_count_ = position->pusher_distal()->posedge_count();
+ last_proximal_posedge_count_ = position->pusher_proximal()->posedge_count();
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
- status->absolute_position = shooter_.absolute_position();
- status->absolute_velocity = shooter_.absolute_velocity();
- status->state = state_;
+ status_builder.add_absolute_position(shooter_.absolute_position());
+ status_builder.add_absolute_velocity(shooter_.absolute_velocity());
+ status_builder.add_state(state_);
- status->shots = shot_count_;
+ status_builder.add_shots(shot_count_);
+
+ status->Send(status_builder.Finish());
}
-void ShooterMotor::Zero(::y2014::control_loops::ShooterQueue::Output *output) {
- output->voltage = 0.0;
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+flatbuffers::Offset<Output> ShooterMotor::Zero(
+ aos::Sender<Output>::Builder *output) {
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage(0.0);
+ output_builder.add_latch_piston(latch_piston_);
+ output_builder.add_brake_piston(brake_piston_);
+ return output_builder.Finish();
}
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index e755b0f..1e7d4aa 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -8,11 +8,15 @@
#include "aos/time/time.h"
#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
class ShooterTest_UnloadWindupPositive_Test;
class ShooterTest_UnloadWindupNegative_Test;
@@ -127,19 +131,17 @@
::std::chrono::milliseconds(40);
class ShooterMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ShooterMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.shooter_queue");
+ explicit ShooterMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
double PositionToPower(double position);
- void CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position);
+ void CheckCalibrations(const Position *position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -159,15 +161,14 @@
State state() { return state_; }
protected:
- void RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
// We have to override this to keep the pistons in the correct positions.
- void Zero(::y2014::control_loops::ShooterQueue::Output *output) override;
+ flatbuffers::Offset<Output> Zero(
+ aos::Sender<Output>::Builder *output) override;
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
@@ -185,7 +186,9 @@
load_timeout_ = monotonic_now + kLoadTimeout;
}
- ::y2014::control_loops::ShooterQueue::Position last_position_;
+ bool last_position_latch_ = false;
+ bool last_position_plunger_ = false;
+ double last_position_position_ = 0.0;
ZeroedStateFeedbackLoop shooter_;
@@ -232,6 +235,7 @@
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
deleted file mode 100644
index c7ed97d..0000000
--- a/y2014/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,103 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// Published on ".y2014.control_loops.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- message Output {
- double voltage;
- // true: latch engaged, false: latch open
- bool latch_piston;
- // true: brake engaged false: brake released
- bool brake_piston;
- };
- message Goal {
- // Shot power in joules.
- double shot_power;
- // Shoots as soon as this is true.
- bool shot_requested;
- bool unload_requested;
- bool load_requested;
- };
-
- // Back is when the springs are all the way stretched.
- message Position {
- // In meters, out is positive.
- double position;
-
- // If the latch piston is fired and this hall effect has been triggered, the
- // plunger is all the way back and latched.
- bool plunger;
- // Gets triggered when the pusher is all the way back.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_distal;
- // Triggers just before pusher_distal.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_proximal;
- // Triggers when the latch engages.
- bool latch;
- };
- message Status {
- // Whether it's ready to shoot right now.
- bool ready;
- // Whether the plunger is in and out of the way of grabbing a ball.
- // TODO(ben): Populate these!
- bool cocked;
- // How many times we've shot.
- int32_t shots;
- bool done;
- // What we think the current position of the hard stop on the shooter is, in
- // shot power (Joules).
- double hard_stop_power;
-
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ShooterStateToLog {
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- bool latch_sensor;
- bool proximal;
- bool distal;
- bool plunger;
- bool brake;
- bool latch_piston;
-};
-
-struct ShooterVoltageToLog {
- double X_hat;
- double applied;
-};
-
-struct ShooterMovingGoal {
- double dx;
-};
-
-struct ShooterChangeCalibration {
- double encoder;
- double real_position;
- double old_position;
- double new_position;
- double old_offset;
- double new_offset;
-};
-
-struct ShooterStatusToLog {
- double goal;
- double position;
-};
-
-struct PowerAdjustment {
- double requested_power;
- double actual_power;
-};
diff --git a/y2014/control_loops/shooter/shooter_goal.fbs b/y2014/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..3326562
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,12 @@
+namespace y2014.control_loops.shooter;
+
+table Goal {
+ // Shot power in joules.
+ shot_power:double;
+ // Shoots as soon as this is true.
+ shot_requested:bool;
+ unload_requested:bool;
+ load_requested:bool;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index f910b6d..ad24cf9 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -9,7 +9,10 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
namespace chrono = ::std::chrono;
@@ -17,6 +20,7 @@
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
using ::y2014::control_loops::shooter::kMaxExtension;
@@ -31,11 +35,8 @@
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
double initial_position)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2014.control_loops.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2014.control_loops.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
latch_piston_state_(false),
latch_delay_count_(0),
@@ -69,6 +70,7 @@
plant->mutable_Y() = plant->C() * plant->X();
last_voltage_ = 0.0;
last_plant_position_ = 0.0;
+ last_position_ = 0.0;
SetPhysicalSensors(&last_position_message_);
}
@@ -89,8 +91,7 @@
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ShooterQueue::Position *position) {
+ void SetPhysicalSensors(PositionT *position) {
const constants::Values &values = constants::GetValues();
position->position = GetPosition();
@@ -113,17 +114,27 @@
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
- position->pusher_distal.current =
+
+ if (!position->pusher_distal) {
+ position->pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+ if (!position->pusher_proximal) {
+ position->pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+
+ position->pusher_distal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
- position->pusher_proximal.current =
+ position->pusher_proximal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
}
void UpdateEffectEdge(
- ::frc971::PosedgeOnlyCountedHallEffectStruct *sensor,
- const ::frc971::PosedgeOnlyCountedHallEffectStruct &last_sensor,
+ ::frc971::PosedgeOnlyCountedHallEffectStructT *sensor,
+ const ::frc971::PosedgeOnlyCountedHallEffectStructT &last_sensor,
const constants::Values::AnglePair &limits,
- const ::y2014::control_loops::ShooterQueue::Position &last_position) {
+ float last_position) {
sensor->posedge_count = last_sensor.posedge_count;
sensor->negedge_count = last_sensor.negedge_count;
@@ -131,7 +142,7 @@
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
- if (last_position.position + initial_position_ < limits.lower_angle) {
+ if (last_position + initial_position_ < limits.lower_angle) {
AOS_LOG(DEBUG,
"Posedge value on lower edge of sensor, count is now %d\n",
sensor->posedge_count);
@@ -159,8 +170,15 @@
// it into a state using the plunger_in_, brake_in_, and latch_in_ values.
void SendPositionMessage() {
const constants::Values &values = constants::GetValues();
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
+
+ PositionT position;
+
+ position.pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ position.pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
if (use_passed_) {
plunger_latched_ = latch_in_ && plunger_in_;
@@ -168,43 +186,45 @@
brake_piston_state_ = brake_in_;
}
- SetPhysicalSensors(position.get());
+ SetPhysicalSensors(&position);
- position->latch = latch_piston_state_;
+ position.latch = latch_piston_state_;
// Handle pusher distal hall effect
- UpdateEffectEdge(&position->pusher_distal,
- last_position_message_.pusher_distal,
- values.shooter.pusher_distal, last_position_message_);
+ UpdateEffectEdge(position.pusher_distal.get(),
+ *last_position_message_.pusher_distal.get(),
+ values.shooter.pusher_distal, last_position_);
// Handle pusher proximal hall effect
- UpdateEffectEdge(&position->pusher_proximal,
- last_position_message_.pusher_proximal,
- values.shooter.pusher_proximal, last_position_message_);
+ UpdateEffectEdge(position.pusher_proximal.get(),
+ *last_position_message_.pusher_proximal.get(),
+ values.shooter.pusher_proximal, last_position_);
- last_position_message_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+
+ last_position_ = position.position;
+ last_position_message_ = std::move(position);
}
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = GetAbsolutePosition();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- if (shooter_output_fetcher_->latch_piston && !latch_piston_state_ &&
+ if (shooter_output_fetcher_->latch_piston() && !latch_piston_state_ &&
latch_delay_count_ <= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = 6;
- } else if (!shooter_output_fetcher_->latch_piston &&
+ } else if (!shooter_output_fetcher_->latch_piston() &&
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = -6;
}
- if (shooter_output_fetcher_->brake_piston && !brake_piston_state_ &&
+ if (shooter_output_fetcher_->brake_piston() && !brake_piston_state_ &&
brake_delay_count_ <= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = 5;
- } else if (!shooter_output_fetcher_->brake_piston &&
+ } else if (!shooter_output_fetcher_->brake_piston() &&
brake_piston_state_ && brake_delay_count_ >= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = -5;
@@ -265,13 +285,13 @@
EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
GetAbsolutePosition());
- last_voltage_ = shooter_output_fetcher_->voltage;
+ last_voltage_ = shooter_output_fetcher_->voltage();
}
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Sender<Position> shooter_position_sender_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
bool first_ = true;
@@ -300,7 +320,8 @@
double initial_position_;
double last_voltage_;
- ShooterQueue::Position last_position_message_;
+ double last_position_ = 0.0;
+ PositionT last_position_message_;
double last_plant_position_;
};
@@ -310,13 +331,12 @@
protected:
ShooterTestTemplated()
: ::aos::testing::ControlLoopTestTemplated<TestType>(
+ aos::configuration::ReadConfig("y2014/config.json"),
// TODO(austin): I think this runs at 5 ms in real life.
chrono::microseconds(5000)),
test_event_loop_(this->MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_event_loop_(this->MakeEventLoop()),
shooter_motor_(shooter_event_loop_.get()),
shooter_plant_event_loop_(this->MakeEventLoop()),
@@ -330,12 +350,12 @@
void VerifyNearGoal() {
shooter_goal_fetcher_.Fetch();
const double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_goal_fetcher_->shot_power, pos, 1e-4);
+ EXPECT_NEAR(shooter_goal_fetcher_->shot_power(), pos, 1e-4);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -381,18 +401,18 @@
TEST_F(ShooterTest, GoesToValue) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -400,19 +420,19 @@
TEST_F(ShooterTest, Fire) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1200));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 35.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(35.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -421,11 +441,12 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 17.0;
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(17.0);
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -433,8 +454,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -443,20 +465,20 @@
TEST_F(ShooterTest, FireLong) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 0.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(0.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -465,10 +487,11 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -476,8 +499,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -487,10 +511,10 @@
TEST_F(ShooterTest, LoadTooFar) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 500.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(500.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(1600))) {
@@ -505,19 +529,19 @@
TEST_F(ShooterTest, MoveGoal) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 14.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(14.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
@@ -525,8 +549,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -534,18 +558,18 @@
TEST_F(ShooterTest, Unload) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -563,10 +587,10 @@
TEST_F(ShooterTest, RezeroWhileUnloading) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
@@ -576,10 +600,10 @@
RunFor(chrono::milliseconds(500));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -597,18 +621,18 @@
TEST_F(ShooterTest, UnloadWindupNegative) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -640,18 +664,18 @@
TEST_F(ShooterTest, UnloadWindupPositive) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -688,18 +712,18 @@
SetEnabled(true);
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -710,18 +734,18 @@
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(3));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -741,10 +765,10 @@
bool initialized = false;
Reinitialize(start_pos);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 120.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(120.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(2))) {
@@ -759,8 +783,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -784,5 +808,6 @@
// TODO(austin): Test all the timeouts...
} // namespace testing
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index 2da76c3..4161fdf 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/shooter/shooter.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ShooterMotor shooter(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::shooter::ShooterMotor shooter(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/shooter/shooter_output.fbs b/y2014/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..e1900c4
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.shooter;
+
+table Output {
+ voltage:double;
+ // true: latch engaged, false: latch open
+ latch_piston:bool;
+ // true: brake engaged false: brake released
+ brake_piston:bool;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/shooter/shooter_position.fbs b/y2014/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..9c73a1a
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,21 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.shooter;
+
+// Back is when the springs are all the way stretched.
+table Position {
+ // In meters, out is positive.
+ position:double;
+
+ // If the latch piston is fired and this hall effect has been triggered, the
+ // plunger is all the way back and latched.
+ plunger:bool;
+ // Gets triggered when the pusher is all the way back.
+ pusher_distal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers just before pusher_distal.
+ pusher_proximal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers when the latch engages.
+ latch:bool;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/shooter/shooter_status.fbs b/y2014/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..4e76e27
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.shooter;
+
+table Status {
+ // Whether it's ready to shoot right now.
+ ready:bool;
+ // Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
+ //cocked:bool;
+ // How many times we've shot.
+ shots:int;
+ //done:bool;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ hard_stop_power:double;
+
+ absolute_position:double;
+ absolute_velocity:double;
+ state:uint;
+}
+
+root_type Status;
diff --git a/y2014/hot_goal_reader.cc b/y2014/hot_goal_reader.cc
index 773397d..8eb4efb 100644
--- a/y2014/hot_goal_reader.cc
+++ b/y2014/hot_goal_reader.cc
@@ -7,20 +7,22 @@
#include <unistd.h>
#include "aos/byteorder.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/queues/hot_goal_generated.h"
int main() {
::aos::InitNRT();
- ::aos::ShmEventLoop shm_event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop shm_event_loop(&config.message());
::aos::Sender<::y2014::HotGoal> hot_goal_sender =
- shm_event_loop.MakeSender<::y2014::HotGoal>(".y2014.hot_goal");
+ shm_event_loop.MakeSender<::y2014::HotGoal>("/");
uint64_t left_count = 0, right_count = 0;
int my_socket = -1;
@@ -82,11 +84,12 @@
}
if (data & 0x01) ++right_count;
if (data & 0x02) ++left_count;
- auto message = hot_goal_sender.MakeMessage();
- message->left_count = left_count;
- message->right_count = right_count;
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- message.Send();
+ auto builder = hot_goal_sender.MakeBuilder();
+ y2014::HotGoal::Builder hot_goal_builder =
+ builder.MakeBuilder<y2014::HotGoal>();
+ hot_goal_builder.add_left_count(left_count);
+ hot_goal_builder.add_right_count(right_count);
+ builder.Send(hot_goal_builder.Finish());
} break;
case 0:
AOS_LOG(WARNING, "read on %d timed out\n", connection);
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 29cb354..6f94bfe 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -11,14 +11,13 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2014/actors/shoot_actor.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -158,18 +157,18 @@
event_loop, control_loops::GetDrivetrainConfig(),
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>(
+ "/claw")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
shot_power_(80.0),
goal_angle_(0.0),
separation_angle_(kGrabSeparation),
@@ -326,7 +325,8 @@
}
if (data.PosEdge(kFire)) {
- EnqueueAction(shoot_action_factory_.Make(0.0));
+ aos::common::actions::DoubleParamT param;
+ EnqueueAction(shoot_action_factory_.Make(param));
} else if (data.NegEdge(kFire)) {
CancelCurrentAction();
}
@@ -354,7 +354,7 @@
double goal_angle = goal_angle_;
if (drivetrain_status_fetcher_.get()) {
goal_angle +=
- SpeedToAngleOffset(drivetrain_status_fetcher_->robot_speed);
+ SpeedToAngleOffset(drivetrain_status_fetcher_->robot_speed());
} else {
AOS_LOG_INTERVAL(no_drivetrain_status_);
}
@@ -362,7 +362,7 @@
if (moving_for_shot_) {
claw_status_fetcher_.Fetch();
if (claw_status_fetcher_.get()) {
- if (::std::abs(claw_status_fetcher_->bottom - goal_angle) < 0.2) {
+ if (::std::abs(claw_status_fetcher_->bottom() - goal_angle) < 0.2) {
moving_for_shot_ = false;
separation_angle_ = shot_separation_angle_;
}
@@ -381,26 +381,30 @@
data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
{
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = goal_angle;
- goal_message->separation_angle = separation_angle;
- goal_message->intake =
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(goal_angle);
+ goal_builder.add_separation_angle(separation_angle);
+ goal_builder.add_intake(
intaking ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_);
- goal_message->centering = intaking ? 12.0 : 0.0;
+ : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_));
+ goal_builder.add_centering(intaking ? 12.0 : 0.0);
- if (!goal_message.Send()) {
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
{
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shot_power_;
- goal_message->shot_requested = data.IsPressed(kFire);
- goal_message->unload_requested = data.IsPressed(kUnload);
- goal_message->load_requested = data.IsPressed(kReload);
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(shot_power_);
+ goal_builder.add_shot_requested(data.IsPressed(kFire));
+ goal_builder.add_unload_requested(data.IsPressed(kUnload));
+ goal_builder.add_load_requested(data.IsPressed(kReload));
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
}
}
@@ -415,11 +419,10 @@
}
private:
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal> shooter_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
double shot_power_;
@@ -443,7 +446,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());
::y2014::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2014/queues/BUILD b/y2014/queues/BUILD
index 182ab7d..dba212d 100644
--- a/y2014/queues/BUILD
+++ b/y2014/queues/BUILD
@@ -1,24 +1,19 @@
-package(default_visibility = ['//visibility:public'])
+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 = 'profile_params',
- srcs = [
- 'profile_params.q',
- ],
+flatbuffer_cc_library(
+ name = "auto_mode_fbs",
+ srcs = [
+ "auto_mode.fbs",
+ ],
+ gen_reflections = 1,
)
-queue_library(
- name = 'hot_goal',
- srcs = [
- 'hot_goal.q',
- ],
-)
-
-queue_library(
- name = 'auto_mode',
- srcs = [
- 'auto_mode.q',
- ],
+flatbuffer_cc_library(
+ name = "hot_goal_fbs",
+ srcs = [
+ "hot_goal.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/y2014/queues/auto_mode.fbs b/y2014/queues/auto_mode.fbs
new file mode 100644
index 0000000..bca34f8
--- /dev/null
+++ b/y2014/queues/auto_mode.fbs
@@ -0,0 +1,9 @@
+namespace y2014.sensors;
+
+// Published on "/aos"
+table AutoMode {
+ // Voltage of the analog auto selector knob.
+ voltage:double;
+}
+
+root_type AutoMode;
diff --git a/y2014/queues/auto_mode.q b/y2014/queues/auto_mode.q
deleted file mode 100644
index b07f526..0000000
--- a/y2014/queues/auto_mode.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2014.sensors;
-
-// Published on ".y2014.sensors.auto_mode"
-message AutoMode {
- // Voltage of the analog auto selector knob.
- double voltage;
-};
diff --git a/y2014/queues/hot_goal.fbs b/y2014/queues/hot_goal.fbs
new file mode 100644
index 0000000..5c4bd0d
--- /dev/null
+++ b/y2014/queues/hot_goal.fbs
@@ -0,0 +1,9 @@
+namespace y2014;
+
+// Published on "/"
+table HotGoal {
+ left_count:ulong;
+ right_count:ulong;
+}
+
+root_type HotGoal;
diff --git a/y2014/queues/hot_goal.q b/y2014/queues/hot_goal.q
deleted file mode 100644
index 8aabb46..0000000
--- a/y2014/queues/hot_goal.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2014;
-
-// Published on ".y2014.hot_goal"
-message HotGoal {
- uint64_t left_count;
- uint64_t right_count;
-};
diff --git a/y2014/queues/profile_params.q b/y2014/queues/profile_params.q
deleted file mode 100644
index 206ceff..0000000
--- a/y2014/queues/profile_params.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package y2014;
-
-struct ProfileParams {
- double velocity;
- double acceleration;
-};
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index 7952adc..48d9c4b 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -18,18 +18,18 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/shifter_hall_effect.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -40,21 +40,19 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::y2014::control_loops::ClawQueue;
-using ::y2014::control_loops::ShooterQueue;
+namespace claw = ::y2014::control_loops::claw;
+namespace shooter = ::y2014::control_loops::shooter;
using aos::make_unique;
namespace y2014 {
@@ -118,16 +116,15 @@
public:
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
- auto_mode_sender_(event_loop->MakeSender<::y2014::sensors::AutoMode>(
- ".y2014.sensors.auto_mode")),
- shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
- ".y2014.control_loops.shooter_queue.position")),
- claw_position_sender_(event_loop->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")),
+ auto_mode_sender_(
+ event_loop->MakeSender<::y2014::sensors::AutoMode>("/aos")),
+ shooter_position_sender_(
+ event_loop->MakeSender<shooter::Position>("/shooter")),
+ claw_position_sender_(event_loop->MakeSender<claw::Position>("/claw")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateMediumEncoderFilterHz(kMaximumEncoderPulsesPerSecond);
@@ -249,59 +246,87 @@
const auto &values = constants::GetValues();
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_right_encoder(
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message->low_left_hall = low_left_drive_hall_->GetVoltage();
- drivetrain_message->high_left_hall = high_left_drive_hall_->GetVoltage();
- drivetrain_message->left_shifter_position =
- hall_translate(values.left_drive, drivetrain_message->low_left_hall,
- drivetrain_message->high_left_hall);
+ const double low_left_hall = low_left_drive_hall_->GetVoltage();
+ const double high_left_hall = high_left_drive_hall_->GetVoltage();
+ position_builder.add_low_left_hall(low_left_hall);
+ position_builder.add_high_left_hall(high_left_hall);
+ position_builder.add_left_shifter_position(
+ hall_translate(values.left_drive, low_left_hall, high_left_hall));
- drivetrain_message->low_right_hall = low_right_drive_hall_->GetVoltage();
- drivetrain_message->high_right_hall =
- high_right_drive_hall_->GetVoltage();
- drivetrain_message->right_shifter_position =
- hall_translate(values.right_drive, drivetrain_message->low_right_hall,
- drivetrain_message->high_right_hall);
+ const double low_right_hall = low_right_drive_hall_->GetVoltage();
+ const double high_right_hall = high_right_drive_hall_->GetVoltage();
+ position_builder.add_low_right_hall(low_right_hall);
+ position_builder.add_high_right_hall(high_right_hall);
+ position_builder.add_right_shifter_position(
+ hall_translate(values.right_drive, low_right_hall, high_right_hall));
- drivetrain_message.Send();
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->voltage = auto_selector_analog_->GetVoltage();
- auto_mode_message.Send();
+ auto builder = auto_mode_sender_.MakeBuilder();
+ y2014::sensors::AutoMode::Builder auto_builder =
+ builder.MakeBuilder<y2014::sensors::AutoMode>();
+ auto_builder.add_voltage(auto_selector_analog_->GetVoltage());
+ builder.Send(auto_builder.Finish());
}
}
void RunDmaIteration() {
{
- auto shooter_message = shooter_position_sender_.MakeMessage();
- shooter_message->position = shooter_translate(shooter_encoder_->GetRaw());
- shooter_message->plunger = !shooter_plunger_reader_->value();
- shooter_message->latch = !shooter_latch_reader_->value();
+ auto builder = shooter_position_sender_.MakeBuilder();
+ ::frc971::PosedgeOnlyCountedHallEffectStructT pusher_proximal;
CopyShooterPosedgeCounts(shooter_proximal_counter_.get(),
- &shooter_message->pusher_proximal);
- CopyShooterPosedgeCounts(shooter_distal_counter_.get(),
- &shooter_message->pusher_distal);
+ &pusher_proximal);
+ flatbuffers::Offset<::frc971::PosedgeOnlyCountedHallEffectStruct>
+ pusher_proximal_offset =
+ ::frc971::PosedgeOnlyCountedHallEffectStruct::Pack(
+ *builder.fbb(), &pusher_proximal);
- shooter_message.Send();
+ ::frc971::PosedgeOnlyCountedHallEffectStructT pusher_distal;
+ CopyShooterPosedgeCounts(shooter_distal_counter_.get(), &pusher_distal);
+ flatbuffers::Offset<::frc971::PosedgeOnlyCountedHallEffectStruct>
+ pusher_distal_offset =
+ ::frc971::PosedgeOnlyCountedHallEffectStruct::Pack(
+ *builder.fbb(), &pusher_distal);
+
+ control_loops::shooter::Position::Builder position_builder =
+ builder.MakeBuilder<control_loops::shooter::Position>();
+ position_builder.add_position(
+ shooter_translate(shooter_encoder_->GetRaw()));
+ position_builder.add_plunger(!shooter_plunger_reader_->value());
+ position_builder.add_latch(!shooter_latch_reader_->value());
+ position_builder.add_pusher_distal(pusher_distal_offset);
+ position_builder.add_pusher_proximal(pusher_proximal_offset);
+
+ builder.Send(position_builder.Finish());
}
{
- auto claw_message = claw_position_sender_.MakeMessage();
- top_reader_.RunIteration(&claw_message->top);
- bottom_reader_.RunIteration(&claw_message->bottom);
+ auto builder = claw_position_sender_.MakeBuilder();
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> top_offset =
+ top_reader_.ReadPosition(builder.fbb());
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> bottom_offset =
+ bottom_reader_.ReadPosition(builder.fbb());
- claw_message.Send();
+ control_loops::claw::Position::Builder position_builder =
+ builder.MakeBuilder<control_loops::claw::Position>();
+
+ position_builder.add_top(top_offset);
+ position_builder.add_bottom(bottom_offset);
+ builder.Send(position_builder.Finish());
}
}
@@ -347,22 +372,42 @@
void Quit() { synchronizer_.Quit(); }
- void RunIteration(control_loops::HalfClawPosition *half_claw_position) {
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> ReadPosition(
+ flatbuffers::FlatBufferBuilder *fbb) {
const double multiplier = reversed_ ? -1.0 : 1.0;
synchronizer_.RunIteration();
- CopyPosition(front_counter_.get(), &half_claw_position->front);
- CopyPosition(calibration_counter_.get(),
- &half_claw_position->calibration);
- CopyPosition(back_counter_.get(), &half_claw_position->back);
- half_claw_position->position =
- multiplier * claw_translate(synchronized_encoder_->get());
+
+ ::frc971::HallEffectStructT front;
+ CopyPosition(front_counter_.get(), &front);
+ flatbuffers::Offset<::frc971::HallEffectStruct> front_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &front);
+
+ ::frc971::HallEffectStructT calibration;
+ CopyPosition(calibration_counter_.get(), &calibration);
+ flatbuffers::Offset<::frc971::HallEffectStruct> calibration_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &calibration);
+
+ ::frc971::HallEffectStructT back;
+ CopyPosition(back_counter_.get(), &back);
+ flatbuffers::Offset<::frc971::HallEffectStruct> back_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &back);
+
+ control_loops::claw::HalfClawPosition::Builder half_claw_position_builder(
+ *fbb);
+
+ half_claw_position_builder.add_front(front_offset);
+ half_claw_position_builder.add_calibration(calibration_offset);
+ half_claw_position_builder.add_back(back_offset);
+ half_claw_position_builder.add_position(
+ multiplier * claw_translate(synchronized_encoder_->get()));
+ return half_claw_position_builder.Finish();
}
private:
void CopyPosition(const ::frc971::wpilib::EdgeCounter *counter,
- ::frc971::HallEffectStruct *out) {
+ ::frc971::HallEffectStructT *out) {
const double multiplier = reversed_ ? -1.0 : 1.0;
out->current = !counter->polled_value();
@@ -392,7 +437,7 @@
void CopyShooterPosedgeCounts(
const ::frc971::wpilib::DMAEdgeCounter *counter,
- ::frc971::PosedgeOnlyCountedHallEffectStruct *output) {
+ ::frc971::PosedgeOnlyCountedHallEffectStructT *output) {
output->current = !counter->polled_value();
// These are inverted because the hall effects give logical false when
// there's a magnet in front of them.
@@ -403,9 +448,9 @@
}
::aos::Sender<::y2014::sensors::AutoMode> auto_mode_sender_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<shooter::Position> shooter_position_sender_;
+ ::aos::Sender<claw::Position> claw_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> auto_selector_analog_;
@@ -433,12 +478,13 @@
SolenoidWriter(::aos::EventLoop *event_loop,
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
- ".y2014.control_loops.shooter_queue.output")),
+ shooter_(event_loop->MakeFetcher<shooter::Output>("/shooter")),
drivetrain_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -484,26 +530,27 @@
{
shooter_.Fetch();
if (shooter_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_latch_->Set(!shooter_->latch_piston);
- shooter_brake_->Set(!shooter_->brake_piston);
+ shooter_latch_->Set(!shooter_->latch_piston());
+ shooter_brake_->Set(!shooter_->brake_piston());
}
}
{
drivetrain_.Fetch();
if (drivetrain_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(!drivetrain_->left_high);
- drivetrain_right_->Set(!drivetrain_->right_high);
+ drivetrain_left_->Set(!drivetrain_->left_high());
+ drivetrain_right_->Set(!drivetrain_->right_high());
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
{
const bool compressor_on = !pressure_switch_->Get();
- to_log.compressor_on = compressor_on;
+ to_log_builder.add_compressor_on(compressor_on);
if (compressor_on) {
compressor_relay_->Set(::frc::Relay::kForward);
} else {
@@ -512,8 +559,8 @@
}
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
}
@@ -528,15 +575,17 @@
::std::unique_ptr<::frc::DigitalInput> pressure_switch_;
::std::unique_ptr<::frc::Relay> compressor_relay_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Fetcher<shooter::Output> shooter_;
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output> drivetrain_;
+
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
};
class ShooterWriter
- : public ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<shooter::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output>(
+ : ::frc971::wpilib::LoopOutputHandler<shooter::Output>(
event_loop, ".y2014.control_loops.shooter_queue.output") {}
void set_shooter_talon(::std::unique_ptr<::frc::Talon> t) {
@@ -544,9 +593,8 @@
}
private:
- virtual void Write(const ShooterQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- shooter_talon_->SetSpeed(output.voltage / 12.0);
+ virtual void Write(const shooter::Output &output) override {
+ shooter_talon_->SetSpeed(output.voltage() / 12.0);
}
virtual void Stop() override {
@@ -557,11 +605,10 @@
::std::unique_ptr<::frc::Talon> shooter_talon_;
};
-class ClawWriter
- : public ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output> {
+class ClawWriter : public ::frc971::wpilib::LoopOutputHandler<claw::Output> {
public:
ClawWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output>(
+ : ::frc971::wpilib::LoopOutputHandler<claw::Output>(
event_loop, ".y2014.control_loops.claw_queue.output") {}
void set_top_claw_talon(::std::unique_ptr<::frc::Talon> t) {
@@ -589,14 +636,13 @@
}
private:
- virtual void Write(const ClawQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- intake1_talon_->SetSpeed(output.intake_voltage / 12.0);
- intake2_talon_->SetSpeed(output.intake_voltage / 12.0);
- bottom_claw_talon_->SetSpeed(-output.bottom_claw_voltage / 12.0);
- top_claw_talon_->SetSpeed(output.top_claw_voltage / 12.0);
- left_tusk_talon_->SetSpeed(output.tusk_voltage / 12.0);
- right_tusk_talon_->SetSpeed(-output.tusk_voltage / 12.0);
+ virtual void Write(const claw::Output &output) override {
+ intake1_talon_->SetSpeed(output.intake_voltage() / 12.0);
+ intake2_talon_->SetSpeed(output.intake_voltage() / 12.0);
+ bottom_claw_talon_->SetSpeed(-output.bottom_claw_voltage() / 12.0);
+ top_claw_talon_->SetSpeed(output.top_claw_voltage() / 12.0);
+ left_tusk_talon_->SetSpeed(output.tusk_voltage() / 12.0);
+ right_tusk_talon_->SetSpeed(-output.tusk_voltage() / 12.0);
}
virtual void Stop() override {
@@ -625,19 +671,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
// Create this first to make sure it ends up in one of the lower-numbered
@@ -680,12 +729,12 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop gyro_event_loop;
+ ::aos::ShmEventLoop gyro_event_loop(&config.message());
::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
AddLoop(&gyro_event_loop);
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(make_unique<::frc::Talon>(5), true);
drivetrain_writer.set_right_controller0(make_unique<::frc::Talon>(2),
@@ -705,7 +754,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
diff --git a/y2014/y2014.json b/y2014/y2014.json
new file mode 100644
index 0000000..38440a0
--- /dev/null
+++ b/y2014/y2014.json
@@ -0,0 +1,54 @@
+{
+ "channels":
+ [
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Position",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}
diff --git a/y2014_bot3/BUILD b/y2014_bot3/BUILD
index 25a6445..6d1f3fd 100644
--- a/y2014_bot3/BUILD
+++ b/y2014_bot3/BUILD
@@ -13,12 +13,9 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/queues:gyro",
"//y2014_bot3/control_loops/drivetrain:drivetrain_base",
- "//y2014_bot3/control_loops/rollers:rollers_queue",
+ "//y2014_bot3/control_loops/rollers:rollers_goal_fbs",
],
)
@@ -43,21 +40,21 @@
"//aos:make_unique",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
diff --git a/y2014_bot3/actors/BUILD b/y2014_bot3/actors/BUILD
index ed6dfa1..ad60458 100644
--- a/y2014_bot3/actors/BUILD
+++ b/y2014_bot3/actors/BUILD
@@ -8,12 +8,11 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2014_bot3/control_loops/drivetrain:drivetrain_base",
],
)
@@ -27,7 +26,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014_bot3/actors/autonomous_actor.cc b/y2014_bot3/actors/autonomous_actor.cc
index ee67813..68064a5 100644
--- a/y2014_bot3/actors/autonomous_actor.cc
+++ b/y2014_bot3/actors/autonomous_actor.cc
@@ -4,23 +4,30 @@
#include <chrono>
#include <cmath>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
namespace y2014_bot3 {
namespace actors {
-using ::frc971::ProfileParameters;
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
namespace {
-const ProfileParameters kDrive = {5.0, 2.5};
-const ProfileParameters kTurn = {8.0, 3.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kDrive = MakeProfileParameters(5.0, 2.5);
+const ProfileParametersT kTurn = MakeProfileParameters(8.0, 3.0);
} // namespace
@@ -29,10 +36,10 @@
event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
Reset();
StartDrive(1.0, 0.0, kDrive, kTurn);
diff --git a/y2014_bot3/actors/autonomous_actor.h b/y2014_bot3/actors/autonomous_actor.h
index d0e71da..e4dfe53 100644
--- a/y2014_bot3/actors/autonomous_actor.h
+++ b/y2014_bot3/actors/autonomous_actor.h
@@ -6,7 +6,7 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/autonomous/base_autonomous_actor.h"
namespace y2014_bot3 {
@@ -17,7 +17,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
diff --git a/y2014_bot3/actors/autonomous_actor_main.cc b/y2014_bot3/actors/autonomous_actor_main.cc
index 235b84a..9dfc422 100644
--- a/y2014_bot3/actors/autonomous_actor_main.cc
+++ b/y2014_bot3/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2014_bot3/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014_bot3::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2014_bot3/control_loops/drivetrain/BUILD b/y2014_bot3/control_loops/drivetrain/BUILD
index 18e0e90..7060ac1 100644
--- a/y2014_bot3/control_loops/drivetrain/BUILD
+++ b/y2014_bot3/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -77,7 +75,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index 48f09c5..05d09b3 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/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 "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop,
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
index 9535489..e0d7dc6 100644
--- a/y2014_bot3/control_loops/rollers/BUILD
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -1,16 +1,37 @@
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 = "rollers_queue",
+flatbuffer_cc_library(
+ name = "rollers_goal_fbs",
srcs = [
- "rollers.q",
+ "rollers_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "rollers_position_fbs",
+ srcs = [
+ "rollers_position.fbs",
],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "rollers_output_fbs",
+ srcs = [
+ "rollers_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "rollers_status_fbs",
+ srcs = [
+ "rollers_status.fbs",
+ ],
+ gen_reflections = 1,
)
cc_library(
@@ -22,7 +43,10 @@
"rollers.h",
],
deps = [
- ":rollers_queue",
+ ":rollers_goal_fbs",
+ ":rollers_output_fbs",
+ ":rollers_position_fbs",
+ ":rollers_status_fbs",
"//aos/controls:control_loop",
"//aos/logging",
],
@@ -36,6 +60,6 @@
deps = [
":rollers_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014_bot3/control_loops/rollers/rollers.cc b/y2014_bot3/control_loops/rollers/rollers.cc
index d2b4da9..d62831a 100644
--- a/y2014_bot3/control_loops/rollers/rollers.cc
+++ b/y2014_bot3/control_loops/rollers/rollers.cc
@@ -1,46 +1,51 @@
#include "y2014_bot3/control_loops/rollers/rollers.h"
#include "aos/logging/logging.h"
+#include "y2014_bot3/control_loops/rollers/rollers_goal_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_output_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_position_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_status_generated.h"
namespace y2014_bot3 {
namespace control_loops {
+namespace rollers {
Rollers::Rollers(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::RollersQueue>(event_loop,
- name) {}
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name) {}
-void Rollers::RunIteration(
- const control_loops::RollersQueue::Goal *goal,
- const control_loops::RollersQueue::Position * /*position*/,
- control_loops::RollersQueue::Output *output,
- control_loops::RollersQueue::Status * /*status*/) {
+void Rollers::RunIteration(const Goal *goal, const Position * /*position*/,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
constexpr double k2014Bot3IntakeForwardVoltage = 12.0;
constexpr double k2014Bot3IntakeBackwardVoltage = -12.0;
constexpr double k2014Bot3LowGoalForwardVoltage = 6.0;
constexpr double k2014Bot3LowGoalBackwardVoltage = -6.0;
+ status->Send(status->MakeBuilder<Status>().Finish());
+
if (!output || !goal) {
return;
}
- const int intake = goal->intake;
- const int low_spit = goal->low_spit;
- const bool human_player = goal->human_player;
+ const int intake = goal->intake();
+ const int low_spit = goal->low_spit();
+ const bool human_player = goal->human_player();
- output->Zero();
+ OutputT output_struct;
switch (low_spit) {
case 1:
// Spit towards front
- output->low_goal_voltage = k2014Bot3LowGoalBackwardVoltage;
- output->front_intake_voltage = k2014Bot3IntakeBackwardVoltage;
- output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
+ output_struct.low_goal_voltage = k2014Bot3LowGoalBackwardVoltage;
+ output_struct.front_intake_voltage = k2014Bot3IntakeBackwardVoltage;
+ output_struct.back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
break;
case -1:
// Spit towards back
- output->low_goal_voltage = k2014Bot3LowGoalForwardVoltage;
- output->back_intake_voltage = -k2014Bot3IntakeBackwardVoltage;
- output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+ output_struct.low_goal_voltage = k2014Bot3LowGoalForwardVoltage;
+ output_struct.back_intake_voltage = -k2014Bot3IntakeBackwardVoltage;
+ output_struct.front_intake_voltage = k2014Bot3IntakeForwardVoltage;
break;
default:
// Stationary
@@ -50,17 +55,17 @@
switch (intake) {
case 1:
// Front intake.
- output->front_extended = true;
- output->back_extended = false;
- output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
- output->back_intake_voltage = 0.0;
+ output_struct.front_extended = true;
+ output_struct.back_extended = false;
+ output_struct.front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+ output_struct.back_intake_voltage = 0.0;
break;
case -1:
// Back intake.
- output->back_extended = true;
- output->front_extended = false;
- output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
- output->front_intake_voltage = 0.0;
+ output_struct.back_extended = true;
+ output_struct.front_extended = false;
+ output_struct.back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
+ output_struct.front_intake_voltage = 0.0;
break;
default:
// Stationary
@@ -69,12 +74,15 @@
if (human_player) {
// Intake for human player.
- output->front_extended = false;
- output->back_extended = false;
- output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
- output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
+ output_struct.front_extended = false;
+ output_struct.back_extended = false;
+ output_struct.front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+ output_struct.back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
+} // namespace rollers
} // namespace control_loops
} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/rollers/rollers.h b/y2014_bot3/control_loops/rollers/rollers.h
index 9890871..8903cd2 100644
--- a/y2014_bot3/control_loops/rollers/rollers.h
+++ b/y2014_bot3/control_loops/rollers/rollers.h
@@ -3,27 +3,31 @@
#include "aos/controls/control_loop.h"
-#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers_goal_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_output_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_position_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_status_generated.h"
namespace y2014_bot3 {
namespace control_loops {
+namespace rollers {
-class Rollers : public aos::controls::ControlLoop<control_loops::RollersQueue> {
+class Rollers
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
// Constructs a control loops which can take a rollers or defaults to the
// rollers at ::2014_bot3::control_loops::rollers.
- explicit Rollers(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014_bot3.control_loops.rollers_queue");
+ explicit Rollers(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/rollers");
protected:
// Executes one cycle of the control loop.
- void RunIteration(const control_loops::RollersQueue::Goal *goal,
- const control_loops::RollersQueue::Position *position,
- control_loops::RollersQueue::Output *output,
- control_loops::RollersQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
};
+} // namespace rollers
} // namespace control_loops
} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/rollers/rollers.q b/y2014_bot3/control_loops/rollers/rollers.q
deleted file mode 100644
index 1e40369..0000000
--- a/y2014_bot3/control_loops/rollers/rollers.q
+++ /dev/null
@@ -1,40 +0,0 @@
-package y2014_bot3.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// on ".y2014_bot3.control_loops.rollers_queue"
-queue_group RollersQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // -1 = back intake, 1 = front intake, all else = stationary.
- int16_t intake;
- // -1 = backwards, 1 = forwards, all else = stationary.
- int16_t low_spit;
- // Whether we want the human player load function.
- bool human_player;
- };
-
- message Position {};
-
- message Output {
- // Positive voltage = intaking, Negative = spitting.
- double front_intake_voltage;
- double back_intake_voltage;
- // Voltage for the low goal rollers.
- // Positive voltage = ball towards back, Negative = ball towards front.
- double low_goal_voltage;
-
- // Whether the front and back intake pistons are extended.
- bool front_extended;
- bool back_extended;
- };
-
- message Status {};
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2014_bot3/control_loops/rollers/rollers_goal.fbs b/y2014_bot3/control_loops/rollers/rollers_goal.fbs
new file mode 100644
index 0000000..92ee589
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/rollers_goal.fbs
@@ -0,0 +1,12 @@
+namespace y2014_bot3.control_loops.rollers;
+
+table Goal {
+ // -1 = back intake, 1 = front intake, all else = stationary.
+ intake:int;
+ // -1 = backwards, 1 = forwards, all else = stationary.
+ low_spit:int;
+ // Whether we want the human player load function.
+ human_player:bool;
+}
+
+root_type Goal;
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index 1a0d82b..906c067 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -1,13 +1,16 @@
#include "y2014_bot3/control_loops/rollers/rollers.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014_bot3::control_loops::Rollers rollers(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014_bot3::control_loops::rollers::Rollers rollers(&event_loop);
event_loop.Run();
diff --git a/y2014_bot3/control_loops/rollers/rollers_output.fbs b/y2014_bot3/control_loops/rollers/rollers_output.fbs
new file mode 100644
index 0000000..daae09b
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/rollers_output.fbs
@@ -0,0 +1,16 @@
+namespace y2014_bot3.control_loops.rollers;
+
+table Output {
+ // Positive voltage = intaking, Negative = spitting.
+ front_intake_voltage:double;
+ back_intake_voltage:double;
+ // Voltage for the low goal rollers.
+ // Positive voltage = ball towards back, Negative = ball towards front.
+ low_goal_voltage:double;
+
+ // Whether the front and back intake pistons are extended.
+ front_extended:bool;
+ back_extended:bool;
+}
+
+root_type Output;
diff --git a/y2014_bot3/control_loops/rollers/rollers_position.fbs b/y2014_bot3/control_loops/rollers/rollers_position.fbs
new file mode 100644
index 0000000..040502c
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/rollers_position.fbs
@@ -0,0 +1,6 @@
+namespace y2014_bot3.control_loops.rollers;
+
+table Position {
+}
+
+root_type Position;
diff --git a/y2014_bot3/control_loops/rollers/rollers_status.fbs b/y2014_bot3/control_loops/rollers/rollers_status.fbs
new file mode 100644
index 0000000..2486bb7
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/rollers_status.fbs
@@ -0,0 +1,6 @@
+namespace y2014_bot3.control_loops.rollers;
+
+table Status {
+}
+
+root_type Status;
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index dc43f30..47546a7 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -11,12 +11,10 @@
#include "aos/time/time.h"
#include "aos/input/drivetrain_input.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
-#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers_goal_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::POVLocation;
@@ -47,9 +45,8 @@
Reader(::aos::EventLoop *event_loop)
: ::aos::input::JoystickInput(event_loop),
rollers_goal_sender_(
- event_loop
- ->MakeSender<::y2014_bot3::control_loops::RollersQueue::Goal>(
- ".y2014_bot3.control_loops.rollers_queue.goal")),
+ event_loop->MakeSender<::y2014_bot3::control_loops::rollers::Goal>(
+ "/rollers")),
autonomous_action_factory_(
::frc971::autonomous::BaseAutonomousActor::MakeFactory(
event_loop)) {
@@ -89,20 +86,21 @@
}
// Rollers.
- auto rollers_goal = rollers_goal_sender_.MakeMessage();
- rollers_goal->Zero();
+ auto builder = rollers_goal_sender_.MakeBuilder();
+ control_loops::rollers::GoalT rollers_goal;
if (data.IsPressed(kFrontRollersIn)) {
- rollers_goal->intake = 1;
+ rollers_goal.intake = 1;
} else if (data.IsPressed(kFrontRollersOut)) {
- rollers_goal->low_spit = 1;
+ rollers_goal.low_spit = 1;
} else if (data.IsPressed(kBackRollersIn)) {
- rollers_goal->intake = -1;
+ rollers_goal.intake = -1;
} else if (data.IsPressed(kBackRollersOut)) {
- rollers_goal->low_spit = -1;
+ rollers_goal.low_spit = -1;
} else if (data.IsPressed(kHumanPlayer)) {
- rollers_goal->human_player = true;
+ rollers_goal.human_player = true;
}
- if (!rollers_goal.Send()) {
+ if (!builder.Send(control_loops::rollers::Goal::Pack(*builder.fbb(),
+ &rollers_goal))) {
AOS_LOG(WARNING, "Sending rollers values failed.\n");
}
}
@@ -110,7 +108,7 @@
private:
void StartAuto() {
AOS_LOG(INFO, "Starting auto mode.\n");
- ::frc971::autonomous::AutonomousActionParams params;
+ ::frc971::autonomous::AutonomousActionParamsT params;
params.mode = 0;
action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
}
@@ -129,7 +127,7 @@
::aos::common::actions::ActionQueue action_queue_;
::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
- ::aos::Sender<::y2014_bot3::control_loops::RollersQueue::Goal>
+ ::aos::Sender<::y2014_bot3::control_loops::rollers::Goal>
rollers_goal_sender_;
::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
@@ -142,7 +140,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());
::y2014_bot3::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 9b1d4e8..89b3ad1 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -18,31 +18,32 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
#include "y2014_bot3/control_loops/rollers/rollers.h"
-#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers_output_generated.h"
+#include "y2014_bot3/control_loops/rollers/rollers_position_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -77,41 +78,45 @@
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
rollers_position_sender_(
- event_loop->MakeSender<
- ::y2014_bot3::control_loops::RollersQueue::Position>(
- ".y2014_bot3.control_loops.rollers_queue.position")),
+ event_loop
+ ->MakeSender<::y2014_bot3::control_loops::rollers::Position>(
+ "/rollers")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {}
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {}
void RunIteration() {
// Drivetrain
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
- drivetrain_message.Send();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_right_encoder(
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
+
+ builder.Send(position_builder.Finish());
}
// Rollers
{
- auto rollers_message = rollers_position_sender_.MakeMessage();
- rollers_message.Send();
+ auto builder = rollers_position_sender_.MakeBuilder();
+ builder.Send(
+ builder.MakeBuilder<control_loops::rollers::Position>().Finish());
}
}
private:
- ::aos::Sender<::y2014_bot3::control_loops::RollersQueue::Position>
+ ::aos::Sender<::y2014_bot3::control_loops::rollers::Position>
rollers_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
};
@@ -123,11 +128,14 @@
: pcm_(pcm),
drivetrain_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
- rollers_(event_loop->MakeFetcher<
- ::y2014_bot3::control_loops::RollersQueue::Output>(
- ".y2014_bot3.control_loops.rollers_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ rollers_(
+ event_loop
+ ->MakeFetcher<::y2014_bot3::control_loops::rollers::Output>(
+ "/rollers")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -170,9 +178,8 @@
{
drivetrain_.Fetch();
if (drivetrain_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(drivetrain_->left_high);
- drivetrain_right_->Set(drivetrain_->right_high);
+ drivetrain_left_->Set(drivetrain_->left_high());
+ drivetrain_right_->Set(drivetrain_->right_high());
}
}
@@ -180,19 +187,22 @@
{
rollers_.Fetch();
if (rollers_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *rollers_);
- rollers_front_->Set(rollers_->front_extended);
- rollers_back_->Set(rollers_->back_extended);
+ rollers_front_->Set(rollers_->front_extended());
+ rollers_back_->Set(rollers_->back_extended());
}
}
// Compressor
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
+
{
// Refill if pneumatic pressure goes too low.
const bool compressor_on = !pressure_switch_->Get();
- to_log.compressor_on = compressor_on;
+ to_log_builder.add_compressor_on(compressor_on);
if (compressor_on) {
compressor_relay_->Set(::frc::Relay::kForward);
} else {
@@ -201,8 +211,8 @@
}
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
}
@@ -215,18 +225,19 @@
::std::unique_ptr<::frc::DigitalInput> pressure_switch_;
::std::unique_ptr<::frc::Relay> compressor_relay_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Fetcher<::y2014_bot3::control_loops::RollersQueue::Output> rollers_;
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output> drivetrain_;
+ ::aos::Fetcher<::y2014_bot3::control_loops::rollers::Output> rollers_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
};
// Writes out rollers voltages.
class RollersWriter : public LoopOutputHandler<
- ::y2014_bot3::control_loops::RollersQueue::Output> {
+ ::y2014_bot3::control_loops::rollers::Output> {
public:
RollersWriter(::aos::EventLoop *event_loop)
: ::frc971::wpilib::LoopOutputHandler<
- ::y2014_bot3::control_loops::RollersQueue::Output>(
- event_loop, ".y2014_bot3.control_loops.rollers_queue.output") {}
+ ::y2014_bot3::control_loops::rollers::Output>(event_loop,
+ "/rollers") {}
void set_rollers_front_intake_talon(::std::unique_ptr<::frc::Talon> t_left,
::std::unique_ptr<::frc::Talon> t_right) {
@@ -245,18 +256,17 @@
}
private:
- virtual void Write(const ::y2014_bot3::control_loops::RollersQueue::Output
+ virtual void Write(const ::y2014_bot3::control_loops::rollers::Output
&output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- rollers_front_left_intake_talon_->SetSpeed(output.front_intake_voltage /
+ rollers_front_left_intake_talon_->SetSpeed(output.front_intake_voltage() /
12.0);
rollers_front_right_intake_talon_->SetSpeed(
- -(output.front_intake_voltage / 12.0));
- rollers_back_left_intake_talon_->SetSpeed(output.back_intake_voltage /
+ -(output.front_intake_voltage() / 12.0));
+ rollers_back_left_intake_talon_->SetSpeed(output.back_intake_voltage() /
12.0);
rollers_back_right_intake_talon_->SetSpeed(
- -(output.back_intake_voltage / 12.0));
- rollers_low_goal_talon_->SetSpeed(output.low_goal_voltage / 12.0);
+ -(output.back_intake_voltage() / 12.0));
+ rollers_low_goal_talon_->SetSpeed(output.low_goal_voltage() / 12.0);
}
virtual void Stop() override {
@@ -280,33 +290,36 @@
::frc::Encoder::k4X);
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
// Sensors
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(4));
sensor_reader.set_drivetrain_right_encoder(make_encoder(5));
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop gyro_event_loop;
+ ::aos::ShmEventLoop gyro_event_loop(&config.message());
GyroSender gyro_sender(&gyro_event_loop);
AddLoop(&gyro_event_loop);
// Thread 5.
// Outputs
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::Talon>(new ::frc::Talon(5)), true);
@@ -326,7 +339,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
diff --git a/y2016/BUILD b/y2016/BUILD
index ee77f00..f3887b5 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
cc_library(
name = "constants",
@@ -34,16 +35,18 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/queues:gyro",
"//y2016/actors:autonomous_action_lib",
"//y2016/actors:superstructure_action_lib",
"//y2016/actors:vision_align_action_lib",
- "//y2016/control_loops/shooter:shooter_queue",
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
"//y2016/control_loops/superstructure:superstructure_lib",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -65,6 +68,28 @@
],
)
+aos_config(
+ name = "config",
+ src = "y2016.json",
+ flatbuffers = [
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/shooter:shooter_output_fbs",
+ "//y2016/control_loops/shooter:shooter_position_fbs",
+ "//y2016/control_loops/shooter:shooter_status_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_output_fbs",
+ "//y2016/control_loops/superstructure:superstructure_position_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2017/vision:vision_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
+)
+
cc_binary(
name = "wpilib_interface",
srcs = [
@@ -78,16 +103,16 @@
"//aos:math",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
@@ -97,16 +122,18 @@
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
- "//y2016/control_loops/shooter:shooter_queue",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
+ "//y2016/control_loops/shooter:shooter_output_fbs",
+ "//y2016/control_loops/shooter:shooter_position_fbs",
+ "//y2016/control_loops/superstructure:superstructure_output_fbs",
+ "//y2016/control_loops/superstructure:superstructure_position_fbs",
+ "//y2016/queues:ball_detector_fbs",
],
)
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index f5b2566..f80128d 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
filegroup(
name = "binaries",
@@ -10,14 +10,13 @@
],
)
-queue_library(
- name = "superstructure_action_queue",
+flatbuffer_cc_library(
+ name = "superstructure_action_fbs",
srcs = [
- "superstructure_action.q",
+ "superstructure_action.fbs",
],
- deps = [
- "//aos/actions:action_queue",
- ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -29,12 +28,14 @@
"superstructure_actor.h",
],
deps = [
- ":superstructure_action_queue",
+ ":superstructure_action_fbs",
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
- "//y2016/control_loops/superstructure:superstructure_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
],
)
@@ -44,10 +45,10 @@
"superstructure_actor_main.cc",
],
deps = [
+ ":superstructure_action_fbs",
":superstructure_action_lib",
- ":superstructure_action_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -65,13 +66,13 @@
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2016/control_loops/drivetrain:drivetrain_base",
- "//y2016/control_loops/shooter:shooter_queue",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
- "//y2016/queues:profile_params",
- "//y2016/vision:vision_queue",
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/shooter:shooter_status_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2016/vision:vision_fbs",
],
)
@@ -86,14 +87,13 @@
],
)
-queue_library(
- name = "vision_align_action_queue",
+flatbuffer_cc_library(
+ name = "vision_align_action_fbs",
srcs = [
- "vision_align_action.q",
+ "vision_align_action.fbs",
],
- deps = [
- "//aos/actions:action_queue",
- ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -105,19 +105,19 @@
"vision_align_actor.h",
],
deps = [
- ":vision_align_action_queue",
+ ":vision_align_action_fbs",
"//aos:math",
"//aos/actions:action_lib",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
"//aos/util:phased_loop",
"//aos/util:trapezoid_profile",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//third_party/eigen",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:drivetrain_base",
- "//y2016/vision:vision_queue",
+ "//y2016/vision:vision_fbs",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -127,8 +127,8 @@
"vision_align_actor_main.cc",
],
deps = [
+ ":vision_align_action_fbs",
":vision_align_action_lib",
- ":vision_align_action_queue",
"//aos:init",
],
)
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 498a6f1..a665c28 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -8,39 +8,52 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
using ::aos::monotonic_clock;
+using ::frc971::ProfileParametersT;
+namespace superstructure = y2016::control_loops::superstructure;
+namespace shooter = y2016::control_loops::shooter;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-const ProfileParameters kSlowDrive = {0.8, 2.5};
-const ProfileParameters kLowBarDrive = {1.3, 2.5};
-const ProfileParameters kMoatDrive = {1.2, 3.5};
-const ProfileParameters kRealignDrive = {2.0, 2.5};
-const ProfileParameters kRockWallDrive = {0.8, 2.5};
-const ProfileParameters kFastDrive = {3.0, 2.5};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
-const ProfileParameters kSlowTurn = {0.8, 3.0};
-const ProfileParameters kFastTurn = {3.0, 10.0};
-const ProfileParameters kStealTurn = {4.0, 15.0};
-const ProfileParameters kSwerveTurn = {2.0, 7.0};
-const ProfileParameters kFinishTurn = {2.0, 5.0};
+const ProfileParametersT kSlowDrive = MakeProfileParameters(0.8, 2.5);
+const ProfileParametersT kLowBarDrive = MakeProfileParameters(1.3, 2.5);
+const ProfileParametersT kMoatDrive = MakeProfileParameters(1.2, 3.5);
+const ProfileParametersT kRealignDrive = MakeProfileParameters(2.0, 2.5);
+const ProfileParametersT kRockWallDrive = MakeProfileParameters(0.8, 2.5);
+const ProfileParametersT kFastDrive = MakeProfileParameters(3.0, 2.5);
-const ProfileParameters kTwoBallLowDrive = {1.7, 3.5};
-const ProfileParameters kTwoBallFastDrive = {3.0, 1.5};
-const ProfileParameters kTwoBallReturnDrive = {3.0, 1.9};
-const ProfileParameters kTwoBallReturnSlow = {3.0, 2.5};
-const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};
-const ProfileParameters kTwoBallBallPickupAccel = {2.0, 2.5};
+const ProfileParametersT kSlowTurn = MakeProfileParameters(0.8, 3.0);
+const ProfileParametersT kFastTurn = MakeProfileParameters(3.0, 10.0);
+const ProfileParametersT kStealTurn = MakeProfileParameters(4.0, 15.0);
+const ProfileParametersT kSwerveTurn = MakeProfileParameters(2.0, 7.0);
+const ProfileParametersT kFinishTurn = MakeProfileParameters(2.0, 5.0);
+
+const ProfileParametersT kTwoBallLowDrive = MakeProfileParameters(1.7, 3.5);
+const ProfileParametersT kTwoBallFastDrive = MakeProfileParameters(3.0, 1.5);
+const ProfileParametersT kTwoBallReturnDrive = MakeProfileParameters(3.0, 1.9);
+const ProfileParametersT kTwoBallReturnSlow = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kTwoBallBallPickup = MakeProfileParameters(2.0, 1.75);
+const ProfileParametersT kTwoBallBallPickupAccel =
+ MakeProfileParameters(2.0, 2.5);
} // namespace
@@ -51,66 +64,66 @@
actors::VisionAlignActor::MakeFactory(event_loop)),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
shooter_goal_sender_(
- event_loop
- ->MakeSender<::y2016::control_loops::shooter::ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ event_loop->MakeSender<::y2016::control_loops::shooter::Goal>(
+ "/shooter")),
shooter_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::shooter::ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")),
+ event_loop->MakeFetcher<::y2016::control_loops::shooter::Status>(
+ "/shooter")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
- superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")) {}
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
constexpr double kDoNotTurnCare = 2.0;
void AutonomousActor::MoveSuperstructure(
double intake, double shoulder, double wrist,
- const ProfileParameters intake_params,
- const ProfileParameters shoulder_params,
- const ProfileParameters wrist_params, bool traverse_up,
+ const ProfileParametersT intake_params,
+ const ProfileParametersT shoulder_params,
+ const ProfileParametersT wrist_params, bool traverse_up,
double roller_power) {
superstructure_goal_ = {intake, shoulder, wrist};
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->angle_intake = intake;
- new_superstructure_goal->angle_shoulder = shoulder;
- new_superstructure_goal->angle_wrist = wrist;
+ superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
- new_superstructure_goal->max_angular_velocity_intake =
- intake_params.max_velocity;
- new_superstructure_goal->max_angular_velocity_shoulder =
- shoulder_params.max_velocity;
- new_superstructure_goal->max_angular_velocity_wrist =
- wrist_params.max_velocity;
+ superstructure_goal_builder.add_angle_intake(intake);
+ superstructure_goal_builder.add_angle_shoulder(shoulder);
+ superstructure_goal_builder.add_angle_wrist(wrist);
- new_superstructure_goal->max_angular_acceleration_intake =
- intake_params.max_acceleration;
- new_superstructure_goal->max_angular_acceleration_shoulder =
- shoulder_params.max_acceleration;
- new_superstructure_goal->max_angular_acceleration_wrist =
- wrist_params.max_acceleration;
+ superstructure_goal_builder.add_max_angular_velocity_intake(
+ intake_params.max_velocity);
+ superstructure_goal_builder.add_max_angular_velocity_shoulder(
+ shoulder_params.max_velocity);
+ superstructure_goal_builder.add_max_angular_velocity_wrist(
+ wrist_params.max_velocity);
- new_superstructure_goal->voltage_top_rollers = roller_power;
- new_superstructure_goal->voltage_bottom_rollers = roller_power;
+ superstructure_goal_builder.add_max_angular_acceleration_intake(
+ intake_params.max_acceleration);
+ superstructure_goal_builder.add_max_angular_acceleration_shoulder(
+ shoulder_params.max_acceleration);
+ superstructure_goal_builder.add_max_angular_acceleration_wrist(
+ wrist_params.max_acceleration);
- new_superstructure_goal->traverse_unlatched = true;
- new_superstructure_goal->traverse_down = !traverse_up;
- new_superstructure_goal->voltage_climber = 0.0;
- new_superstructure_goal->unfold_climber = false;
+ superstructure_goal_builder.add_voltage_top_rollers(roller_power);
+ superstructure_goal_builder.add_voltage_bottom_rollers(roller_power);
- if (!new_superstructure_goal.Send()) {
+ superstructure_goal_builder.add_traverse_unlatched(true);
+ superstructure_goal_builder.add_traverse_down(!traverse_up);
+ superstructure_goal_builder.add_voltage_climber(0.0);
+ superstructure_goal_builder.add_unfold_climber(false);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -118,12 +131,14 @@
void AutonomousActor::OpenShooter() {
shooter_speed_ = 0.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = true;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = false;
- if (!shooter_goal.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(true);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(false);
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -131,13 +146,15 @@
void AutonomousActor::CloseShooter() {
shooter_speed_ = 0.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = false;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(false);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -149,13 +166,15 @@
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = force_lights_on;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(force_lights_on);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -165,20 +184,22 @@
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- initial_shots = shooter_status_fetcher_->shots;
+ initial_shots = shooter_status_fetcher_->shots();
}
// In auto, we want to have the lights on whenever possible since we have no
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = true;
- shooter_goal->force_lights_on = force_lights_on;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(true);
+ shooter_goal_builder.add_force_lights_on(force_lights_on);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
@@ -191,7 +212,7 @@
// Wait for the shot count to change so we know when the shot is complete.
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- if (initial_shots < shooter_status_fetcher_->shots) {
+ if (initial_shots < shooter_status_fetcher_->shots()) {
return;
}
}
@@ -208,8 +229,8 @@
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- if (shooter_status_fetcher_->left.ready &&
- shooter_status_fetcher_->right.ready) {
+ if (shooter_status_fetcher_->left()->ready() &&
+ shooter_status_fetcher_->right()->ready()) {
return;
}
}
@@ -218,7 +239,7 @@
}
void AutonomousActor::AlignWithVisionGoal() {
- actors::VisionAlignActionParams params;
+ vision_align_action::VisionAlignActionParamsT params;
vision_action_ = vision_align_actor_factory_.Make(params);
vision_action_->Start();
}
@@ -238,25 +259,25 @@
vision_status_fetcher_.Fetch();
if (vision_status_fetcher_.get()) {
- vision_valid = (vision_status_fetcher_->left_image_valid &&
- vision_status_fetcher_->right_image_valid);
- last_angle = vision_status_fetcher_->horizontal_angle;
+ vision_valid = (vision_status_fetcher_->left_image_valid() &&
+ vision_status_fetcher_->right_image_valid());
+ last_angle = vision_status_fetcher_->horizontal_angle();
}
drivetrain_status_fetcher_.Fetch();
drivetrain_goal_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get() && drivetrain_goal_fetcher_.get()) {
- const double left_goal = drivetrain_goal_fetcher_->left_goal;
- const double right_goal = drivetrain_goal_fetcher_->right_goal;
+ const double left_goal = drivetrain_goal_fetcher_->left_goal();
+ const double right_goal = drivetrain_goal_fetcher_->right_goal();
const double left_current =
- drivetrain_status_fetcher_->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position();
const double right_current =
- drivetrain_status_fetcher_->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position();
const double left_velocity =
- drivetrain_status_fetcher_->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity();
const double right_velocity =
- drivetrain_status_fetcher_->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity();
if (vision_valid && ::std::abs(last_angle) < 0.02 &&
::std::abs((left_goal - right_goal) -
@@ -287,21 +308,23 @@
constexpr double kProfileError = 1e-5;
constexpr double kEpsilon = 0.15;
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
- if (::std::abs(superstructure_status_fetcher_->intake.goal_angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
superstructure_goal_.intake) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->intake
- .goal_angular_velocity) < kProfileError) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->goal_angular_velocity()) <
+ kProfileError) {
AOS_LOG(DEBUG, "Profile done.\n");
- if (::std::abs(superstructure_status_fetcher_->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
superstructure_goal_.intake) < kEpsilon &&
- ::std::abs(superstructure_status_fetcher_->intake
- .angular_velocity) < kEpsilon) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->angular_velocity()) <
+ kEpsilon) {
AOS_LOG(INFO, "Near goal, done.\n");
return true;
}
@@ -310,28 +333,26 @@
}
bool AutonomousActor::SuperstructureProfileDone() {
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
constexpr double kProfileError = 1e-5;
- return ::std::abs(
- superstructure_status_fetcher_->intake.goal_angle -
- superstructure_goal_.intake) < kProfileError &&
+ return ::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()->goal_angle() -
+ superstructure_goal_.shoulder) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->wrist()->goal_angle() -
+ superstructure_goal_.wrist) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->intake()
+ ->goal_angular_velocity()) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()
+ ->goal_angular_velocity()) < kProfileError &&
::std::abs(
- superstructure_status_fetcher_->shoulder.goal_angle -
- superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->wrist.goal_angle -
- superstructure_goal_.wrist) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->intake
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->shoulder
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->wrist
- .goal_angular_velocity) < kProfileError;
+ superstructure_status_fetcher_->wrist()->goal_angular_velocity()) <
+ kProfileError;
}
bool AutonomousActor::SuperstructureDone() {
@@ -340,18 +361,21 @@
constexpr double kEpsilon = 0.03;
if (SuperstructureProfileDone()) {
AOS_LOG(DEBUG, "Profile done.\n");
- if (::std::abs(superstructure_status_fetcher_->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
superstructure_goal_.intake) < (kEpsilon + 0.1) &&
- ::std::abs(superstructure_status_fetcher_->shoulder.angle -
+ ::std::abs(superstructure_status_fetcher_->shoulder()->angle() -
superstructure_goal_.shoulder) < (kEpsilon + 0.05) &&
- ::std::abs(superstructure_status_fetcher_->wrist.angle -
+ ::std::abs(superstructure_status_fetcher_->wrist()->angle() -
superstructure_goal_.wrist) < (kEpsilon + 0.01) &&
- ::std::abs(superstructure_status_fetcher_->intake
- .angular_velocity) < (kEpsilon + 0.1) &&
- ::std::abs(superstructure_status_fetcher_->shoulder
- .angular_velocity) < (kEpsilon + 0.10) &&
- ::std::abs(superstructure_status_fetcher_->wrist
- .angular_velocity) < (kEpsilon + 0.05)) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->angular_velocity()) <
+ (kEpsilon + 0.1) &&
+ ::std::abs(
+ superstructure_status_fetcher_->shoulder()->angular_velocity()) <
+ (kEpsilon + 0.10) &&
+ ::std::abs(
+ superstructure_status_fetcher_->wrist()->angular_velocity()) <
+ (kEpsilon + 0.05)) {
AOS_LOG(INFO, "Near goal, done.\n");
return true;
}
@@ -379,55 +403,71 @@
superstructure_status_fetcher_.Fetch();
return SuperstructureProfileDone() ||
- superstructure_status_fetcher_->shoulder.angle < 0.1;
+ superstructure_status_fetcher_->shoulder()->angle() < 0.1;
});
}
void AutonomousActor::BackLongShotLowBarTwoBall() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShotTwoBall() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShotTwoBallFinish() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03, {7.0, 40.0},
- {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShot() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.80, M_PI / 2.0 - 0.2, -0.62, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.80, M_PI / 2.0 - 0.2, -0.62,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackMiddleShot() {
AOS_LOG(INFO, "Expanding for back middle shot\n");
- MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665, {7.0, 40.0}, {4.0, 10.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::FrontLongShot() {
AOS_LOG(INFO, "Expanding for front long shot\n");
- MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02, {7.0, 40.0},
- {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::FrontMiddleShot() {
AOS_LOG(INFO, "Expanding for front middle shot\n");
- MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.44, {7.0, 40.0},
- {4.0, 10.0}, {10.0, 25.0}, true, 0.0);
+ MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.44,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 0.0);
}
void AutonomousActor::TuckArm(bool low_bar, bool traverse_down) {
- MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0, {7.0, 40.0},
- {4.0, 10.0}, {10.0, 25.0}, !traverse_down, 0.0);
+ MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), !traverse_down, 0.0);
}
void AutonomousActor::DoFullShot() {
@@ -498,10 +538,10 @@
if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double distance_to_go = (left_error + right_error) / 2.0;
const double distance_compensation =
goal_distance - tip_distance - distance_to_go;
@@ -540,7 +580,7 @@
void AutonomousActor::CloseIfBall() {
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (ball_detected) {
CloseShooter();
}
@@ -563,7 +603,7 @@
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (ball_detected) {
return;
}
@@ -574,8 +614,9 @@
void AutonomousActor::TwoBallAuto() {
const monotonic_clock::time_point start_time = monotonic_now();
OpenShooter();
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
if (ShouldCancel()) return;
AOS_LOG(INFO, "Waiting for the intake to come down.\n");
@@ -593,19 +634,21 @@
bool first_ball_there = true;
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
first_ball_there = ball_detected;
AOS_LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
::aos::time::DurationInSeconds(monotonic_now() - start_time));
}
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 0.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
AOS_LOG(INFO,
"Shutting off rollers at %f seconds, starting to straighten out\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
- MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 0.0);
+ MoveSuperstructure(-0.05, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
CloseShooter();
if (!WaitForDriveNear(kDriveDistance - 2.4, kDoNotTurnCare)) return;
@@ -674,13 +717,15 @@
constexpr double kBallSmallWallTurn = -0.11;
StartDrive(0, kBallSmallWallTurn, kTwoBallBallPickup, kFinishTurn);
- MoveSuperstructure(0.03, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.03, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
if (!WaitForDriveProfileDone()) return;
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
AOS_LOG(INFO, "Done backing up %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
@@ -699,7 +744,7 @@
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (!ball_detected) {
if (!WaitForDriveDone()) return;
AOS_LOG(INFO, "Aborting, no ball %f\n",
@@ -760,8 +805,9 @@
void AutonomousActor::StealAndMoveOverBy(double distance) {
OpenShooter();
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- true, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 12.0);
if (ShouldCancel()) return;
AOS_LOG(INFO, "Waiting for the intake to come down.\n");
@@ -770,8 +816,9 @@
StartDrive(-distance, M_PI / 2.0, kFastDrive, kStealTurn);
WaitForBallOrDriveDone();
if (ShouldCancel()) return;
- MoveSuperstructure(1.0, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- true, 12.0);
+ MoveSuperstructure(1.0, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 12.0);
if (!WaitForDriveDone()) return;
StartDrive(0.0, M_PI / 2.0, kFastDrive, kStealTurn);
@@ -779,15 +826,15 @@
}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
InitializeEncoders();
ResetDrivetrain();
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
LowBarDrive();
if (!WaitForDriveDone()) return true;
@@ -914,7 +961,7 @@
} break;
default:
- AOS_LOG(ERROR, "Invalid auto mode %d\n", params.mode);
+ AOS_LOG(ERROR, "Invalid auto mode %d\n", params->mode());
return true;
}
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index 8d7baaa..538322b 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -6,25 +6,25 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2016/actors/vision_align_actor.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace actors {
-using ::frc971::ProfileParameters;
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void WaitForBallOrDriveDone();
@@ -41,9 +41,9 @@
SuperstructureGoal superstructure_goal_;
void MoveSuperstructure(double intake, double shoulder, double wrist,
- const ProfileParameters intake_params,
- const ProfileParameters shoulder_params,
- const ProfileParameters wrist_params,
+ const frc971::ProfileParametersT intake_params,
+ const frc971::ProfileParametersT shoulder_params,
+ const frc971::ProfileParametersT wrist_params,
bool traverse_up, double roller_power);
void WaitForSuperstructure();
void WaitForSuperstructureProfile();
@@ -91,13 +91,12 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
- ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
- shooter_goal_sender_;
- ::aos::Fetcher<::y2016::control_loops::shooter::ShooterQueue::Status>
+ ::aos::Sender<::y2016::control_loops::shooter::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<::y2016::control_loops::shooter::Status>
shooter_status_fetcher_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::superstructure::Goal>
superstructure_goal_sender_;
};
diff --git a/y2016/actors/autonomous_actor_main.cc b/y2016/actors/autonomous_actor_main.cc
index 1e2628b..79a1250 100644
--- a/y2016/actors/autonomous_actor_main.cc
+++ b/y2016/actors/autonomous_actor_main.cc
@@ -1,13 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "y2016/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2016/actors/superstructure_action.fbs b/y2016/actors/superstructure_action.fbs
new file mode 100644
index 0000000..405f35b
--- /dev/null
+++ b/y2016/actors/superstructure_action.fbs
@@ -0,0 +1,16 @@
+namespace y2016.actors.superstructure_action;
+
+// Parameters to send with start.
+table SuperstructureActionParams {
+ partial_angle:double;
+ delay_time:double;
+ full_angle:double;
+ shooter_angle:double;
+}
+
+table Goal {
+ run:uint;
+ params:SuperstructureActionParams;
+}
+
+root_type Goal;
diff --git a/y2016/actors/superstructure_action.q b/y2016/actors/superstructure_action.q
deleted file mode 100644
index 1b937aa..0000000
--- a/y2016/actors/superstructure_action.q
+++ /dev/null
@@ -1,23 +0,0 @@
-package y2016.actors;
-
-import "aos/actions/actions.q";
-
-// Parameters to send with start.
-struct SuperstructureActionParams {
- double partial_angle;
- double delay_time;
- double full_angle;
- double shooter_angle;
-};
-
-queue_group SuperstructureActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- SuperstructureActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index 2875cbd..bed7b3a 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -2,10 +2,11 @@
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "y2016/actors/superstructure_actor.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
namespace y2016 {
namespace actors {
@@ -13,29 +14,28 @@
namespace chrono = ::std::chrono;
SuperstructureActor::SuperstructureActor(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actors::SuperstructureActionQueueGroup>(
- event_loop, ".y2016.actors.superstructure_action"),
+ : aos::common::actions::ActorBase<superstructure_action::Goal>(
+ event_loop, "/superstructure_action"),
superstructure_goal_sender_(
- event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")) {}
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool SuperstructureActor::RunAction(
- const actors::SuperstructureActionParams ¶ms) {
+ const superstructure_action::SuperstructureActionParams *params) {
AOS_LOG(INFO, "Starting superstructure action\n");
- MoveSuperstructure(params.partial_angle, params.shooter_angle, false);
+ MoveSuperstructure(params->partial_angle(), params->shooter_angle(), false);
WaitForSuperstructure();
if (ShouldCancel()) return true;
- MoveSuperstructure(params.partial_angle, params.shooter_angle, true);
+ MoveSuperstructure(params->partial_angle(), params->shooter_angle(), true);
if (!WaitOrCancel(chrono::duration_cast<::aos::monotonic_clock::duration>(
- chrono::duration<double>(params.delay_time))))
+ chrono::duration<double>(params->delay_time()))))
return true;
- MoveSuperstructure(params.full_angle, params.shooter_angle, true);
+ MoveSuperstructure(params->full_angle(), params->shooter_angle(), true);
WaitForSuperstructure();
if (ShouldCancel()) return true;
return true;
@@ -45,45 +45,46 @@
bool unfold_climber) {
superstructure_goal_ = {0, shoulder, shooter};
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->angle_intake = 0;
- new_superstructure_goal->angle_shoulder = shoulder;
- new_superstructure_goal->angle_wrist = shooter;
+ control_loops::superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
- new_superstructure_goal->max_angular_velocity_intake = 7.0;
- new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
- new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+ superstructure_goal_builder.add_angle_intake(0);
+ superstructure_goal_builder.add_angle_shoulder(shoulder);
+ superstructure_goal_builder.add_angle_wrist(shooter);
- new_superstructure_goal->max_angular_acceleration_intake = 40.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 5.0;
- new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+ superstructure_goal_builder.add_max_angular_velocity_intake(7.0);
+ superstructure_goal_builder.add_max_angular_velocity_shoulder(4.0);
+ superstructure_goal_builder.add_max_angular_velocity_wrist(10.0);
- new_superstructure_goal->voltage_top_rollers = 0;
- new_superstructure_goal->voltage_bottom_rollers = 0;
+ superstructure_goal_builder.add_max_angular_acceleration_intake(40.0);
+ superstructure_goal_builder.add_max_angular_acceleration_shoulder(5.0);
+ superstructure_goal_builder.add_max_angular_acceleration_wrist(25.0);
- new_superstructure_goal->traverse_unlatched = true;
- new_superstructure_goal->traverse_down = false;
- new_superstructure_goal->voltage_climber = 0.0;
- new_superstructure_goal->unfold_climber = unfold_climber;
+ superstructure_goal_builder.add_voltage_top_rollers(0);
+ superstructure_goal_builder.add_voltage_bottom_rollers(0);
- if (!new_superstructure_goal.Send()) {
+ superstructure_goal_builder.add_traverse_unlatched(true);
+ superstructure_goal_builder.add_traverse_down(false);
+ superstructure_goal_builder.add_voltage_climber(0.0);
+ superstructure_goal_builder.add_unfold_climber(unfold_climber);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure move failed.\n");
}
}
bool SuperstructureActor::SuperstructureProfileDone() {
constexpr double kProfileError = 1e-2;
- return ::std::abs(superstructure_status_fetcher_->intake.goal_angle -
+ return ::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
superstructure_goal_.intake) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->shoulder.goal_angle -
+ ::std::abs(superstructure_status_fetcher_->shoulder()->goal_angle() -
superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->intake.goal_angular_velocity) <
- kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->shoulder.goal_angular_velocity) <
- kProfileError;
+ ::std::abs(superstructure_status_fetcher_->intake()
+ ->goal_angular_velocity()) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()
+ ->goal_angular_velocity()) < kProfileError;
}
bool SuperstructureActor::SuperstructureDone() {
@@ -91,8 +92,8 @@
// We are no longer running if we are in the zeroing states (below 12), or
// estopped.
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
diff --git a/y2016/actors/superstructure_actor.h b/y2016/actors/superstructure_actor.h
index ab84db5..f1ae1fe 100644
--- a/y2016/actors/superstructure_actor.h
+++ b/y2016/actors/superstructure_actor.h
@@ -5,29 +5,30 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "y2016/actors/superstructure_action.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/actors/superstructure_action_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
namespace y2016 {
namespace actors {
class SuperstructureActor
- : public ::aos::common::actions::ActorBase<SuperstructureActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<superstructure_action::Goal> {
public:
typedef ::aos::common::actions::TypedActionFactory<
- SuperstructureActionQueueGroup>
+ superstructure_action::Goal>
Factory;
explicit SuperstructureActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2016.actors.superstructure_action");
+ return Factory(event_loop, "/superstructure_action");
}
private:
- ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
// Internal struct holding superstructure goals sent by autonomous to the
// loop.
@@ -37,7 +38,8 @@
double wrist;
};
SuperstructureGoal superstructure_goal_;
- bool RunAction(const actors::SuperstructureActionParams ¶ms) override;
+ bool RunAction(
+ const superstructure_action::SuperstructureActionParams *params) override;
void MoveSuperstructure(double shoulder, double shooter, bool unfold_climber);
void WaitForSuperstructure();
bool SuperstructureProfileDone();
diff --git a/y2016/actors/superstructure_actor_main.cc b/y2016/actors/superstructure_actor_main.cc
index a97802c..fe512d7 100644
--- a/y2016/actors/superstructure_actor_main.cc
+++ b/y2016/actors/superstructure_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2016/actors/superstructure_action.q.h"
#include "y2016/actors/superstructure_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::SuperstructureActor superstructure(&event_loop);
event_loop.Run();
diff --git a/y2016/actors/vision_align_action.fbs b/y2016/actors/vision_align_action.fbs
new file mode 100644
index 0000000..1dd66fc
--- /dev/null
+++ b/y2016/actors/vision_align_action.fbs
@@ -0,0 +1,13 @@
+namespace y2016.actors.vision_align_action;
+
+// Parameters to send with start.
+table VisionAlignActionParams {
+ run:int;
+}
+
+table Goal {
+ run:uint;
+ params:VisionAlignActionParams;
+}
+
+root_type Goal;
diff --git a/y2016/actors/vision_align_action.q b/y2016/actors/vision_align_action.q
deleted file mode 100644
index f9f3024..0000000
--- a/y2016/actors/vision_align_action.q
+++ /dev/null
@@ -1,20 +0,0 @@
-package y2016.actors;
-
-import "aos/actions/actions.q";
-
-// Parameters to send with start.
-struct VisionAlignActionParams {
- int32_t run;
-};
-
-queue_group VisionAlignActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- VisionAlignActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 1685d22..e1c1892 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -6,34 +6,31 @@
#include <Eigen/Dense>
-#include "aos/util/phased_loop.h"
-#include "aos/logging/logging.h"
-#include "aos/util/trapezoid_profile.h"
#include "aos/commonmath.h"
+#include "aos/logging/logging.h"
#include "aos/time/time.h"
-
-#include "y2016/actors/vision_align_actor.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/trapezoid_profile.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "y2016/constants.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
VisionAlignActor::VisionAlignActor(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actors::VisionAlignActionQueueGroup>(
- event_loop, ".y2016.actors.vision_align_action"),
+ : aos::common::actions::ActorBase<vision_align_action::Goal>(
+ event_loop, "/vision_align_action"),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")) {}
bool VisionAlignActor::RunAction(
- const actors::VisionAlignActionParams & /*params*/) {
+ const vision_align_action::VisionAlignActionParams * /*params*/) {
const double robot_radius =
control_loops::drivetrain::GetDrivetrainConfig().robot_radius;
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
@@ -52,31 +49,36 @@
if (!vision_status_fetcher_.Fetch()) {
continue;
}
- const auto &vision_status = *vision_status_fetcher_;
-
- if (!vision_status.left_image_valid || !vision_status.right_image_valid) {
+ if (!vision_status_fetcher_->left_image_valid() ||
+ !vision_status_fetcher_->right_image_valid()) {
continue;
}
const double side_distance_change =
- vision_status.horizontal_angle * robot_radius;
+ vision_status_fetcher_->horizontal_angle() * robot_radius;
if (!::std::isfinite(side_distance_change)) {
continue;
}
- const double left_current = vision_status.drivetrain_left_position;
- const double right_current = vision_status.drivetrain_right_position;
+ const double left_current =
+ vision_status_fetcher_->drivetrain_left_position();
+ const double right_current =
+ vision_status_fetcher_->drivetrain_right_position();
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->wheel = 0.0;
- drivetrain_message->throttle = 0.0;
- drivetrain_message->highgear = false;
- drivetrain_message->quickturn = false;
- drivetrain_message->controller_type = 1;
- drivetrain_message->left_goal = left_current + side_distance_change;
- drivetrain_message->right_goal = right_current - side_distance_change;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- if (!drivetrain_message.Send()) {
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Goal>();
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_highgear(false);
+ goal_builder.add_quickturn(false);
+ goal_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(left_current + side_distance_change);
+ goal_builder.add_right_goal(right_current - side_distance_change);
+
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending drivetrain goal failed\n");
}
}
diff --git a/y2016/actors/vision_align_actor.h b/y2016/actors/vision_align_actor.h
index 586f7d0..ac25439 100644
--- a/y2016/actors/vision_align_actor.h
+++ b/y2016/actors/vision_align_actor.h
@@ -5,32 +5,32 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/actors/vision_align_action.q.h"
-#include "y2016/vision/vision.q.h"
+#include "y2016/actors/vision_align_action_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
class VisionAlignActor
- : public ::aos::common::actions::ActorBase<VisionAlignActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<vision_align_action::Goal> {
public:
- typedef ::aos::common::actions::TypedActionFactory<
- VisionAlignActionQueueGroup>
+ typedef ::aos::common::actions::TypedActionFactory<vision_align_action::Goal>
Factory;
explicit VisionAlignActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2016.actors.vision_align_action");
+ return Factory(event_loop, "/vision_align_action");
}
- bool RunAction(const actors::VisionAlignActionParams ¶ms) override;
+ bool RunAction(
+ const vision_align_action::VisionAlignActionParams *params) override;
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
};
diff --git a/y2016/actors/vision_align_actor_main.cc b/y2016/actors/vision_align_actor_main.cc
index 2fd9eb2..c7aaf50 100644
--- a/y2016/actors/vision_align_actor_main.cc
+++ b/y2016/actors/vision_align_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2016/actors/vision_align_action.q.h"
#include "y2016/actors/vision_align_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::VisionAlignActor vision_align(&event_loop);
event_loop.Run();
diff --git a/y2016/control_loops/drivetrain/BUILD b/y2016/control_loops/drivetrain/BUILD
index 629c957..e706f25 100644
--- a/y2016/control_loops/drivetrain/BUILD
+++ b/y2016/control_loops/drivetrain/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 58ba7ce..b4fd9d8 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/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 "y2016/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index 8938a2b..794c4fc 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -1,16 +1,37 @@
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 = "shooter_queue",
+flatbuffer_cc_library(
+ name = "shooter_goal_fbs",
srcs = [
- "shooter.q",
+ "shooter_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_position_fbs",
+ srcs = [
+ "shooter_position.fbs",
],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_output_fbs",
+ srcs = [
+ "shooter_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_status_fbs",
+ srcs = [
+ "shooter_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -52,8 +73,11 @@
"shooter.h",
],
deps = [
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
":shooter_plants",
- ":shooter_queue",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
],
)
@@ -63,10 +87,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
- "//aos:queues",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -80,9 +107,12 @@
"shooter_main.cc",
],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index bf527ec..5156a61 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -2,10 +2,7 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2016/control_loops/shooter/shooter_plant.h"
namespace y2016 {
@@ -49,80 +46,99 @@
loop_->Update(disabled);
}
-void ShooterSide::SetStatus(ShooterSideStatus *status) {
+flatbuffers::Offset<ShooterSideStatus> ShooterSide::SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ ShooterSideStatus::Builder shooter_side_status_builder(*fbb);
// Compute the oldest point in the history.
const int oldest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
// Compute the distance moved over that time period.
- status->avg_angular_velocity =
+ const double avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
(::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
+ shooter_side_status_builder.add_avg_angular_velocity(avg_angular_velocity);
- status->angular_velocity = loop_->X_hat(1, 0);
+ shooter_side_status_builder.add_angular_velocity(loop_->X_hat(1, 0));
// Ready if average angular velocity is close to the goal.
- status->ready = (std::abs(loop_->next_R(1, 0) -
- status->avg_angular_velocity) < kTolerance &&
- loop_->next_R(1, 0) > 1.0);
+ shooter_side_status_builder.add_ready(
+ (std::abs(loop_->next_R(1, 0) - avg_angular_velocity) < kTolerance &&
+ loop_->next_R(1, 0) > 1.0));
+
+ return shooter_side_status_builder.Finish();
}
Shooter::Shooter(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<ShooterQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shots_(0),
last_pre_shot_timeout_(::aos::monotonic_clock::min_time) {}
-void Shooter::RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) {
+void Shooter::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const ::aos::monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (goal) {
// Update position/goal for our two shooter sides.
- left_.set_goal(goal->angular_velocity);
- right_.set_goal(goal->angular_velocity);
-
- // Turn the lights on if we are supposed to spin.
- if (output) {
- if (::std::abs(goal->angular_velocity) > 0.0) {
- output->lights_on = true;
- if (goal->shooting_forwards) {
- output->forwards_flashlight = true;
- output->backwards_flashlight = false;
- } else {
- output->forwards_flashlight = false;
- output->backwards_flashlight = true;
- }
- }
- if (goal->force_lights_on) {
- output->lights_on = true;
- }
- }
+ left_.set_goal(goal->angular_velocity());
+ right_.set_goal(goal->angular_velocity());
}
- left_.set_position(position->theta_left);
- right_.set_position(position->theta_right);
+ left_.set_position(position->theta_left());
+ right_.set_position(position->theta_right());
left_.Update(output == nullptr);
right_.Update(output == nullptr);
- left_.SetStatus(&status->left);
- right_.SetStatus(&status->right);
- status->ready = (status->left.ready && status->right.ready);
+ flatbuffers::Offset<ShooterSideStatus> left_status_offset =
+ left_.SetStatus(status->fbb());
+ flatbuffers::Offset<ShooterSideStatus> right_status_offset =
+ right_.SetStatus(status->fbb());
+
+ ShooterSideStatus *left_status =
+ GetMutableTemporaryPointer(*status->fbb(), left_status_offset);
+ ShooterSideStatus *right_status =
+ GetMutableTemporaryPointer(*status->fbb(), right_status_offset);
+
+ const bool ready = (left_status->ready() && right_status->ready());
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_ready((left_status->ready() && right_status->ready()));
+ status_builder.add_left(left_status_offset);
+ status_builder.add_right(right_status_offset);
if (output) {
- output->voltage_left = left_.voltage();
- output->voltage_right = right_.voltage();
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage_left(left_.voltage());
+ output_builder.add_voltage_right(right_.voltage());
+ // Turn the lights on if we are supposed to spin.
if (goal) {
+ bool lights_on = false;
+ if (::std::abs(goal->angular_velocity()) > 0.0) {
+ lights_on = true;
+ if (goal->shooting_forwards()) {
+ output_builder.add_forwards_flashlight(true);
+ output_builder.add_backwards_flashlight(false);
+ } else {
+ output_builder.add_forwards_flashlight(false);
+ output_builder.add_backwards_flashlight(true);
+ }
+ }
+ if (goal->force_lights_on()) {
+ lights_on = true;
+ }
+ output_builder.add_lights_on(lights_on);
+
bool shoot = false;
switch (state_) {
case ShooterLatchState::PASS_THROUGH:
- if (goal->push_to_shooter) {
- if (::std::abs(goal->angular_velocity) > 10) {
- if (status->ready) {
+ if (goal->push_to_shooter()) {
+ if (::std::abs(goal->angular_velocity()) > 10) {
+ if (ready) {
state_ = ShooterLatchState::WAITING_FOR_SPINDOWN;
shoot = true;
}
@@ -134,22 +150,22 @@
break;
case ShooterLatchState::WAITING_FOR_SPINDOWN:
shoot = true;
- if (left_.velocity() < goal->angular_velocity * 0.9 ||
- right_.velocity() < goal->angular_velocity * 0.9) {
+ if (left_.velocity() < goal->angular_velocity() * 0.9 ||
+ right_.velocity() < goal->angular_velocity() * 0.9) {
state_ = ShooterLatchState::WAITING_FOR_SPINUP;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
break;
case ShooterLatchState::WAITING_FOR_SPINUP:
shoot = true;
- if (left_.velocity() > goal->angular_velocity * 0.95 &&
- right_.velocity() > goal->angular_velocity * 0.95) {
+ if (left_.velocity() > goal->angular_velocity() * 0.95 &&
+ right_.velocity() > goal->angular_velocity() * 0.95) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
@@ -160,18 +176,22 @@
break;
case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
shoot = true;
- if (!goal->push_to_shooter) {
+ if (!goal->push_to_shooter()) {
state_ = ShooterLatchState::PASS_THROUGH;
}
break;
}
- output->clamp_open = goal->clamp_open;
- output->push_to_shooter = shoot;
+ output_builder.add_clamp_open(goal->clamp_open());
+ output_builder.add_push_to_shooter(shoot);
}
+
+ output->Send(output_builder.Finish());
}
- status->shots = shots_;
+ status_builder.add_shots(shots_);
+
+ status->Send(status_builder.Finish());
}
} // namespace shooter
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 9e6968f..07fee84 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,11 +4,14 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/shooter/shooter_integral_plant.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
namespace y2016 {
namespace control_loops {
@@ -28,7 +31,8 @@
void set_position(double current_position);
// Populates the status structure.
- void SetStatus(ShooterSideStatus *status);
+ flatbuffers::Offset<ShooterSideStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Returns the control loop calculated voltage.
double voltage() const;
@@ -53,11 +57,11 @@
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
-class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
+class Shooter
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Shooter(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.shooter.shooter_queue");
+ explicit Shooter(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
enum class ShooterLatchState {
// Any shoot commands will be passed through without modification.
@@ -73,10 +77,9 @@
};
protected:
- void RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
ShooterSide left_, right_;
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
deleted file mode 100644
index 583b762..0000000
--- a/y2016/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,81 +0,0 @@
-package y2016.control_loops.shooter;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct ShooterSideStatus {
- // True if the shooter side is up to speed and stable.
- bool ready;
- // The current average velocity in radians/second.
- double avg_angular_velocity;
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-};
-
-// Published on ".y2016.control_loops.shooter.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- // All angles are in radians, and angular velocities are in radians/second.
- // For all angular velocities, positive is shooting the ball out of the robot.
-
- message Goal {
- // Angular velocity goals in radians/second.
- double angular_velocity;
-
- bool clamp_open; // True to release our clamp on the ball.
- // True to push the ball into the shooter.
- // If we are in the act of shooting with a goal velocity != 0, wait until it
- // is up to speed, push the ball into the shooter, and then wait until it
- // spins up and down before letting the piston be released.
- bool push_to_shooter;
-
- // Forces the lights on.
- bool force_lights_on;
-
- // If true, the robot is shooting forwards.
- bool shooting_forwards;
- };
-
- message Position {
- // Wheel angle in radians/second.
- double theta_left;
- double theta_right;
- };
-
- message Status {
- // Left side status.
- ShooterSideStatus left;
- // Right side status.
- ShooterSideStatus right;
-
- // True if the shooter is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // The number of shots that have been fired since the start of the shooter
- // control loop.
- uint32_t shots;
- };
-
- message Output {
- // Voltage in volts of the left and right shooter motors.
- double voltage_left;
- double voltage_right;
-
- // See comments on the identical fields in Goal for details.
- bool clamp_open;
- bool push_to_shooter;
-
- // If true, the lights are on.
- bool lights_on;
-
- bool forwards_flashlight;
- bool backwards_flashlight;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/shooter/shooter_goal.fbs b/y2016/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..f041503
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,23 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Goal {
+ // Angular velocity goals in radians/second.
+ angular_velocity:double;
+
+ clamp_open:bool; // True to release our clamp on the ball.
+ // True to push the ball into the shooter.
+ // If we are in the act of shooting with a goal velocity != 0, wait until it
+ // is up to speed, push the ball into the shooter, and then wait until it
+ // spins up and down before letting the piston be released.
+ push_to_shooter:bool;
+
+ // Forces the lights on.
+ force_lights_on:bool;
+
+ // If true, the robot is shooting forwards.
+ shooting_forwards:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 30b5166..b432049 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -5,13 +5,15 @@
#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/queue.h"
#include "aos/controls/control_loop_test.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "gtest/gtest.h"
#include "y2016/control_loops/shooter/shooter.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
using ::frc971::control_loops::testing::kTeamNumber;
@@ -50,11 +52,8 @@
// Constructs a shooter simulation.
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_left_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_plant_right_(new ShooterPlant(
@@ -73,12 +72,14 @@
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
- position->theta_left = shooter_plant_left_->Y(0, 0);
- position->theta_right = shooter_plant_right_->Y(0, 0);
- position.Send();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_theta_left(shooter_plant_left_->Y(0, 0));
+ position_builder.add_theta_right(shooter_plant_right_->Y(0, 0));
+ builder.Send(position_builder.Finish());
}
void set_left_voltage_offset(double voltage_offset) {
@@ -95,9 +96,9 @@
::Eigen::Matrix<double, 1, 1> U_left;
::Eigen::Matrix<double, 1, 1> U_right;
- U_left(0, 0) = shooter_output_fetcher_->voltage_left +
+ U_left(0, 0) = shooter_output_fetcher_->voltage_left() +
shooter_plant_left_->voltage_offset();
- U_right(0, 0) = shooter_output_fetcher_->voltage_right +
+ U_right(0, 0) = shooter_output_fetcher_->voltage_right() +
shooter_plant_right_->voltage_offset();
shooter_plant_left_->Update(U_left);
@@ -107,8 +108,8 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Sender<Position> shooter_position_sender_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
@@ -118,18 +119,16 @@
class ShooterTest : public ::aos::testing::ControlLoopTest {
protected:
ShooterTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_status_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/shooter")),
shooter_output_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/shooter")),
shooter_event_loop_(MakeEventLoop()),
shooter_(shooter_event_loop_.get()),
shooter_plant_event_loop_(MakeEventLoop()),
@@ -144,22 +143,22 @@
EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->angular_velocity(), 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.avg_angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->avg_angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->avg_angular_velocity(), 10.0);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
- ::aos::Fetcher<ShooterQueue::Status> shooter_status_fetcher_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Status> shooter_status_fetcher_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -173,9 +172,10 @@
TEST_F(ShooterTest, DoesNothing) {
SetEnabled(true);
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
@@ -183,8 +183,8 @@
VerifyNearGoal();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
}
// Tests that the shooter spins up to speed and that it then spins down
@@ -193,32 +193,34 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 450.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(450.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
VerifyNearGoal();
shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Make sure we don't apply voltage on spin-down.
RunFor(dt());
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
// Continue to stop.
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
}
// Tests that the shooter doesn't say it is ready if one side isn't up to speed.
@@ -228,9 +230,10 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 20.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(20.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Cause problems by adding a voltage error on one side.
shooter_plant_.set_right_voltage_offset(-4.0);
@@ -243,9 +246,9 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Left should be up to speed, right shouldn't.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_FALSE(shooter_status_fetcher_->right.ready);
- EXPECT_FALSE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->right()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->ready());
RunFor(chrono::seconds(5));
@@ -256,22 +259,23 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Both should be up to speed.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_TRUE(shooter_status_fetcher_->right.ready);
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->right()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(ShooterTest, Disabled) {
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 200.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(200.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
SetEnabled(true);
RunFor(chrono::seconds(5));
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index b9737fd..6e46114 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -1,12 +1,15 @@
#include "y2016/control_loops/shooter/shooter.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
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());
::y2016::control_loops::shooter::Shooter shooter(&event_loop);
event_loop.Run();
diff --git a/y2016/control_loops/shooter/shooter_output.fbs b/y2016/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..6d7fcdf
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,19 @@
+namespace y2016.control_loops.shooter;
+
+table Output {
+ // Voltage in volts of the left and right shooter motors.
+ voltage_left:double;
+ voltage_right:double;
+
+ // See comments on the identical fields in Goal for details.
+ clamp_open:bool;
+ push_to_shooter:bool;
+
+ // If true, the lights are on.
+ lights_on:bool;
+
+ forwards_flashlight:bool;
+ backwards_flashlight:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/shooter/shooter_position.fbs b/y2016/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..d97268c
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,11 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Position {
+ // Wheel angle in radians/second.
+ theta_left:double;
+ theta_right:double;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/shooter/shooter_status.fbs b/y2016/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..6f6262b
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,27 @@
+namespace y2016.control_loops.shooter;
+
+table ShooterSideStatus {
+ // True if the shooter side is up to speed and stable.
+ ready:bool;
+ // The current average velocity in radians/second.
+ avg_angular_velocity:double;
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+}
+
+table Status {
+ // Left side status.
+ left:ShooterSideStatus;
+ // Right side status.
+ right:ShooterSideStatus;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // The number of shots that have been fired since the start of the shooter
+ // control loop.
+ shots:uint;
+}
+
+root_type Status;
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a14a5f..4048cdc 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,15 +1,42 @@
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:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_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",
],
)
@@ -73,8 +100,11 @@
"superstructure_controls.h",
],
deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
":superstructure_plants",
- ":superstructure_queue",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/util:trapezoid_profile",
@@ -83,7 +113,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/zeroing",
"//y2016:constants",
- "//y2016/queues:ball_detector",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -92,11 +122,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2016: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",
@@ -112,8 +145,7 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 99a130c..8f4d02e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,12 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "y2016/control_loops/superstructure/superstructure_controls.h"
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "y2016/control_loops/superstructure/integral_intake_plant.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "y2016/queues/ball_detector_generated.h"
#include "y2016/constants.h"
@@ -229,11 +228,11 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
@@ -289,10 +288,10 @@
}
void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const State state_before_switch = state_;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
@@ -304,8 +303,8 @@
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
- arm_.Correct(position->shoulder, position->wrist);
- intake_.Correct(position->intake);
+ arm_.Correct(position->shoulder(), position->wrist());
+ intake_.Correct(*position->intake());
// There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
//
@@ -571,14 +570,14 @@
4.0, // Shoulder acceleration,
4.0, // Wrist velocity
10.0); // Wrist acceleration.
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
requested_shoulder =
- ::std::max(unsafe_goal->angle_shoulder,
+ ::std::max(unsafe_goal->angle_shoulder(),
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
- requested_intake = unsafe_goal->angle_intake;
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 ||
@@ -591,18 +590,18 @@
}
} else {
// Otherwise, give the user what he asked for.
- arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder(),
+ unsafe_goal->max_angular_acceleration_shoulder(),
+ unsafe_goal->max_angular_velocity_wrist(),
+ unsafe_goal->max_angular_acceleration_wrist());
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
// Except, don't let the shoulder go all the way down.
- requested_shoulder = ::std::max(unsafe_goal->angle_shoulder,
+ requested_shoulder = ::std::max(unsafe_goal->angle_shoulder(),
kShoulderTransitionToLanded);
- requested_wrist = unsafe_goal->angle_wrist;
- requested_intake = unsafe_goal->angle_intake;
+ requested_wrist = unsafe_goal->angle_wrist();
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
@@ -641,9 +640,9 @@
if (unsafe_goal) {
constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005;
- if (unsafe_goal->voltage_climber > 1.0) {
+ if (unsafe_goal->voltage_climber() > 1.0) {
kill_shoulder_accumulator_ +=
- ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber);
+ ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber());
} else {
kill_shoulder_accumulator_ = 0.0;
}
@@ -673,24 +672,28 @@
}
// Calculate the loops for a cycle.
+ double intake_position_power;
+ double intake_velocity_power;
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power =
+ intake_position_power =
intake_.controller().controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power =
+ intake_velocity_power =
intake_.controller().controller().K(0, 1) * error(1, 0);
}
+ double shoulder_position_power;
+ double shoulder_velocity_power;
+ double wrist_position_power;
+ double wrist_velocity_power;
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power =
+ shoulder_position_power =
arm_.controller().controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power =
+ shoulder_velocity_power =
arm_.controller().controller().K(0, 1) * error(1, 0);
- status->wrist.position_power =
- arm_.controller().controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power =
- arm_.controller().controller().K(0, 3) * error(3, 0);
+ wrist_position_power = arm_.controller().controller().K(0, 2) * error(2, 0);
+ wrist_velocity_power = arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
@@ -698,98 +701,135 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.voltage();
- output->voltage_shoulder = arm_.shoulder_voltage();
- output->voltage_wrist = arm_.wrist_voltage();
+ OutputT output_struct;
+ output_struct.voltage_intake = intake_.voltage();
+ output_struct.voltage_shoulder = arm_.shoulder_voltage();
+ output_struct.voltage_wrist = arm_.wrist_voltage();
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
- output->voltage_climber = 0.0;
- output->unfold_climber = false;
+ output_struct.voltage_climber = 0.0;
+ output_struct.unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
ball_detector_fetcher_.Fetch();
bool ball_detected = false;
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
// Climber.
- output->voltage_climber = ::std::max(
+ output_struct.voltage_climber = ::std::max(
static_cast<float>(0.0),
- ::std::min(unsafe_goal->voltage_climber, kMaxClimberVoltage));
- output->unfold_climber = unsafe_goal->unfold_climber;
+ ::std::min(unsafe_goal->voltage_climber(), kMaxClimberVoltage));
+ output_struct.unfold_climber = unsafe_goal->unfold_climber();
// Intake.
- if (unsafe_goal->force_intake || !ball_detected) {
- output->voltage_top_rollers = ::std::max(
+ if (unsafe_goal->force_intake() || !ball_detected) {
+ output_struct.voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
- ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
- output->voltage_bottom_rollers =
+ ::std::min(unsafe_goal->voltage_top_rollers(), kMaxIntakeTopVoltage));
+ output_struct.voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
- ::std::min(unsafe_goal->voltage_bottom_rollers,
+ ::std::min(unsafe_goal->voltage_bottom_rollers(),
kMaxIntakeBottomVoltage));
} else {
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
- output->traverse_down = unsafe_goal->traverse_down;
+ output_struct.traverse_unlatched = unsafe_goal->traverse_unlatched();
+ output_struct.traverse_down = unsafe_goal->traverse_down();
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
// Save debug/internal state.
- status->zeroed = arm_.zeroed() && intake_.zeroed();
+ flatbuffers::Offset<frc971::EstimatorState> shoulder_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 0);
- status->shoulder.angle = arm_.X_hat(0, 0);
- status->shoulder.angular_velocity = arm_.X_hat(1, 0);
- status->shoulder.goal_angle = arm_.goal(0, 0);
- status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
- status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
- status->shoulder.unprofiled_goal_angular_velocity =
- arm_.unprofiled_goal(1, 0);
- status->shoulder.voltage_error = arm_.X_hat(4, 0);
- status->shoulder.calculated_velocity =
- (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.EstimatorState(0);
+ JointState::Builder shoulder_builder = status->MakeBuilder<JointState>();
- status->wrist.angle = arm_.X_hat(2, 0);
- status->wrist.angular_velocity = arm_.X_hat(3, 0);
- status->wrist.goal_angle = arm_.goal(2, 0);
- status->wrist.goal_angular_velocity = arm_.goal(3, 0);
- status->wrist.unprofiled_goal_angle = arm_.unprofiled_goal(2, 0);
- status->wrist.unprofiled_goal_angular_velocity = arm_.unprofiled_goal(3, 0);
- status->wrist.voltage_error = arm_.X_hat(5, 0);
- status->wrist.calculated_velocity =
- (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.EstimatorState(1);
+ shoulder_builder.add_angle(arm_.X_hat(0, 0));
+ shoulder_builder.add_angular_velocity(arm_.X_hat(1, 0));
+ shoulder_builder.add_goal_angle(arm_.goal(0, 0));
+ shoulder_builder.add_goal_angular_velocity(arm_.goal(1, 0));
+ shoulder_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(0, 0));
+ shoulder_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(1, 0));
+ shoulder_builder.add_voltage_error(arm_.X_hat(4, 0));
+ shoulder_builder.add_calculated_velocity(
+ (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005);
+ shoulder_builder.add_position_power(shoulder_position_power);
+ shoulder_builder.add_velocity_power(shoulder_velocity_power);
+ shoulder_builder.add_estimator_state(shoulder_estimator_state_offset);
- status->intake.angle = intake_.X_hat(0, 0);
- status->intake.angular_velocity = intake_.X_hat(1, 0);
- status->intake.goal_angle = intake_.goal(0, 0);
- status->intake.goal_angular_velocity = intake_.goal(1, 0);
- status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
- status->intake.unprofiled_goal_angular_velocity =
- intake_.unprofiled_goal(1, 0);
- status->intake.calculated_velocity =
- (intake_.position() - last_intake_angle_) / 0.005;
- status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.EstimatorState(0);
- status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+ flatbuffers::Offset<JointState> shoulder_offset = shoulder_builder.Finish();
- status->shoulder_controller_index = arm_.controller_index();
+ flatbuffers::Offset<frc971::EstimatorState> wrist_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 1);
+
+ JointState::Builder wrist_builder = status->MakeBuilder<JointState>();
+
+ wrist_builder.add_angle(arm_.X_hat(2, 0));
+ wrist_builder.add_angular_velocity(arm_.X_hat(3, 0));
+ wrist_builder.add_goal_angle(arm_.goal(2, 0));
+ wrist_builder.add_goal_angular_velocity(arm_.goal(3, 0));
+ wrist_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(2, 0));
+ wrist_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(3, 0));
+ wrist_builder.add_voltage_error(arm_.X_hat(5, 0));
+ wrist_builder.add_calculated_velocity(
+ (arm_.wrist_angle() - last_wrist_angle_) / 0.005);
+ wrist_builder.add_position_power(wrist_position_power);
+ wrist_builder.add_velocity_power(wrist_velocity_power);
+ wrist_builder.add_estimator_state(wrist_estimator_state_offset);
+
+ flatbuffers::Offset<JointState> wrist_offset = wrist_builder.Finish();
+
+ flatbuffers::Offset<frc971::EstimatorState> intake_estimator_state_offset =
+ intake_.EstimatorState(status->fbb(), 0);
+
+ JointState::Builder intake_builder = status->MakeBuilder<JointState>();
+ intake_builder.add_position_power(intake_position_power);
+ intake_builder.add_velocity_power(intake_velocity_power);
+ intake_builder.add_angle(intake_.X_hat(0, 0));
+ intake_builder.add_angular_velocity(intake_.X_hat(1, 0));
+ intake_builder.add_goal_angle(intake_.goal(0, 0));
+ intake_builder.add_goal_angular_velocity(intake_.goal(1, 0));
+ intake_builder.add_unprofiled_goal_angle(intake_.unprofiled_goal(0, 0));
+ intake_builder.add_unprofiled_goal_angular_velocity(
+ intake_.unprofiled_goal(1, 0));
+ intake_builder.add_calculated_velocity(
+ (intake_.position() - last_intake_angle_) / 0.005);
+ intake_builder.add_voltage_error(intake_.X_hat(2, 0));
+ intake_builder.add_estimator_state(intake_estimator_state_offset);
+ intake_builder.add_feedforwards_power(intake_.controller().ff_U(0, 0));
+
+ flatbuffers::Offset<JointState> intake_offset = intake_builder.Finish();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_shoulder(shoulder_offset);
+ status_builder.add_wrist(wrist_offset);
+ status_builder.add_intake(intake_offset);
+
+ status_builder.add_zeroed(arm_.zeroed() && intake_.zeroed());
+ status_builder.add_shoulder_controller_index(arm_.controller_index());
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
last_intake_angle_ = intake_.position();
- status->estopped = (state_ == ESTOP);
+ status_builder.add_estopped((state_ == ESTOP));
- status->state = state_;
- status->is_collided = is_collided;
+ status_builder.add_state(state_);
+ status_builder.add_is_collided(is_collided);
+
+ status->Send(status_builder.Finish());
last_state_ = state_before_switch;
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 610db8e..2219c95 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -8,9 +8,12 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace control_loops {
@@ -105,11 +108,10 @@
};
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
@@ -209,11 +211,9 @@
bool collided() const { return collision_avoidance_.collided(); }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::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:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d784929..0000000
--- a/y2016/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,145 +0,0 @@
-package y2016.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
- // Angle of the joint in radians.
- float angle;
- // Angular velocity of the joint in radians/second.
- float angular_velocity;
- // Profiled goal angle of the joint in radians.
- float goal_angle;
- // Profiled goal angular velocity of the joint in radians/second.
- float goal_angular_velocity;
- // Unprofiled goal angle of the joint in radians.
- float unprofiled_goal_angle;
- // Unprofiled goal angular velocity of the joint in radians/second.
- float unprofiled_goal_angular_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-// Published on ".y2016.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Zero on the intake is when the horizontal tube stock members are level
- // with the top frame rails of the robot. This will be essentially when we
- // are in the intaking position. Positive is up. The angle is measured
- // relative to the top
- // of the robot frame.
- // Zero on the shoulder is when the shoulder is down against the hard stop
- // blocks. Positive is up. The angle is measured relative to the top of
- // the robot frame.
- // Zero on the wrist is horizontal and landed in the bellypan. Positive is
- // the same direction as the shoulder. The angle is measured relative to
- // the top of the robot frame. For calibration, 0 is measured as parallel
- // to the big frame supporting the shooter.
-
- // Goal angles and angular velocities of the superstructure subsystems.
- double angle_intake;
- double angle_shoulder;
- // In relation to the ground plane.
- double angle_wrist;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- float max_angular_velocity_intake;
- float max_angular_velocity_shoulder;
- float max_angular_velocity_wrist;
-
- float max_angular_acceleration_intake;
- float max_angular_acceleration_shoulder;
- float max_angular_acceleration_wrist;
-
- // Voltage to send to the rollers. Positive is sucking in.
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, unlatch the climber and allow it to unfold.
- bool unfold_climber;
-
- bool force_intake;
-
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- message Status {
- // Are the superstructure subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // The internal state of the state machine.
- int32_t state;
-
-
- // Estimated angles and angular velocities of the superstructure subsystems.
- JointState intake;
- JointState shoulder;
- JointState wrist;
-
- int32_t shoulder_controller_index;
-
- // Is the superstructure collided?
- bool is_collided;
- };
-
- message Position {
- // Zero for the intake potentiometer value is horizontal, and positive is
- // up.
- // Zero for the shoulder potentiometer value is horizontal, and positive is
- // up.
- // Zero for the wrist potentiometer value is parallel to the arm and with
- // the shooter wheels pointed towards the shoulder joint. This is measured
- // relative to the arm, not the ground, not like the world we actually
- // present to users.
- .frc971.PotAndIndexPosition intake;
- .frc971.PotAndIndexPosition shoulder;
- .frc971.PotAndIndexPosition wrist;
- };
-
- message Output {
- float voltage_intake;
- float voltage_shoulder;
- float voltage_wrist;
-
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, release the latch to trigger the climber to unfold.
- bool unfold_climber;
-
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b8f3a48..269089b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -1,6 +1,5 @@
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
#include "y2016/control_loops/superstructure/integral_intake_plant.h"
@@ -96,10 +95,10 @@
// TODO(austin): Handle zeroing errors.
-void Arm::Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist) {
- estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
- estimators_[kWristIndex].UpdateEstimate(position_wrist);
+void Arm::Correct(const PotAndIndexPosition *position_shoulder,
+ const PotAndIndexPosition *position_wrist) {
+ estimators_[kShoulderIndex].UpdateEstimate(*position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(*position_wrist);
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
@@ -130,7 +129,7 @@
}
{
- Y_ << position_shoulder.encoder, position_wrist.encoder;
+ Y_ << position_shoulder->encoder(), position_wrist->encoder();
Y_ += offset_;
loop_->Correct(Y_);
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8936650..d301054 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -11,7 +11,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
namespace y2016 {
namespace control_loops {
@@ -111,8 +111,8 @@
Arm();
// Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position_shoulder,
- ::frc971::PotAndIndexPosition position_wrist);
+ void Correct(const ::frc971::PotAndIndexPosition *position_shoulder,
+ const ::frc971::PotAndIndexPosition *position_wrist);
// Forces the goal to be the provided goal.
void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
diff --git a/y2016/control_loops/superstructure/superstructure_goal.fbs b/y2016/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..4274bd8
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,50 @@
+namespace y2016.control_loops.superstructure;
+
+table Goal {
+ // Zero on the intake is when the horizontal tube stock members are level
+ // with the top frame rails of the robot. This will be essentially when we
+ // are in the intaking position. Positive is up. The angle is measured
+ // relative to the top
+ // of the robot frame.
+ // Zero on the shoulder is when the shoulder is down against the hard stop
+ // blocks. Positive is up. The angle is measured relative to the top of
+ // the robot frame.
+ // Zero on the wrist is horizontal and landed in the bellypan. Positive is
+ // the same direction as the shoulder. The angle is measured relative to
+ // the top of the robot frame. For calibration, 0 is measured as parallel
+ // to the big frame supporting the shooter.
+
+ // Goal angles and angular velocities of the superstructure subsystems.
+ angle_intake:double;
+ angle_shoulder:double;
+ // In relation to the ground plane.
+ angle_wrist:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ max_angular_velocity_intake:float;
+ max_angular_velocity_shoulder:float;
+ max_angular_velocity_wrist:float;
+
+ max_angular_acceleration_intake:float;
+ max_angular_acceleration_shoulder:float;
+ max_angular_acceleration_wrist:float;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, unlatch the climber and allow it to unfold.
+ unfold_climber:bool;
+
+ force_intake:bool;
+
+ // If true, release the latch which holds the traverse mechanism in the
+ // middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 79b4d24..d530cb3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,14 +7,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "aos/time/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
#include "y2016/constants.h"
@@ -85,14 +87,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
@@ -144,14 +143,32 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
- position = superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- pot_encoder_intake_.GetSensorValues(&position->intake);
- pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
- pot_encoder_wrist_.GetSensorValues(&position->wrist);
+ frc971::PotAndIndexPosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
- position.Send();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ pot_encoder_intake_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndIndexPosition::Builder shoulder_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ pot_encoder_shoulder_.GetSensorValues(&shoulder_builder);
+
+ frc971::PotAndIndexPosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ pot_encoder_wrist_.GetSensorValues(&wrist_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
double shoulder_angle() const { return arm_plant_->X(0, 0); }
@@ -180,37 +197,39 @@
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
- arm_U << superstructure_output_fetcher_->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder() +
arm_plant_->shoulder_voltage_offset(),
- superstructure_output_fetcher_->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist() +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
- if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
- superstructure_status_fetcher_->state ==
+ if (superstructure_status_fetcher_->state() == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state() ==
Superstructure::LANDING_RUNNING) {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kOperatingVoltage);
} else {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
+ AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -309,9 +328,9 @@
bool first_ = true;
- ::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_;
::std::unique_ptr<IntakePlant> intake_plant_;
::std::unique_ptr<ArmPlant> arm_plant_;
@@ -334,17 +353,16 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -357,26 +375,26 @@
EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
- superstructure_status_fetcher_->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
superstructure_plant_.wrist_angle(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -390,17 +408,18 @@
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0;
- message->angle_shoulder = 0;
- message->angle_wrist = 0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0);
+ goal_builder.add_angle_shoulder(0);
+ goal_builder.add_angle_wrist(0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -412,17 +431,18 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI / 4.0;
- message->angle_shoulder = 1.4;
- message->angle_wrist = M_PI / 4.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI / 4.0);
+ goal_builder.add_angle_shoulder(1.4);
+ goal_builder.add_angle_wrist(M_PI / 4.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -437,43 +457,45 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = M_PI * 10;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(M_PI * 10);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = -M_PI * 10.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(-M_PI * 10.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -481,26 +503,27 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = -M_PI * 10;
- message->angle_shoulder = -M_PI * 10;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(-M_PI * 10);
+ goal_builder.add_angle_shoulder(-M_PI * 10);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -508,27 +531,28 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -557,12 +581,13 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the elevator in a safe position as well.
RunFor(chrono::seconds(15));
@@ -580,11 +605,12 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the superstructure in a safe position as
// well.
@@ -604,11 +630,12 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.3);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -628,11 +655,13 @@
superstructure_plant_.set_check_for_collisions(false);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
- message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
- message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.03);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower +
+ 0.03);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower + 0.03);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -659,17 +688,18 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -730,17 +760,18 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = constants::Values::kWristRange.lower;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -812,11 +843,12 @@
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
@@ -834,12 +866,13 @@
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- constants::Values::kShoulderEncoderIndexDifference * 10;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderEncoderIndexDifference * 10);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -883,17 +916,18 @@
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -902,17 +936,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.5;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.5);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// TODO(austin): The profile isn't feasible, so when we try to track it, we
@@ -930,17 +965,18 @@
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -949,17 +985,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.20);
@@ -974,18 +1011,19 @@
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -994,18 +1032,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.5;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.5);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.05);
@@ -1021,18 +1060,19 @@
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1041,18 +1081,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 4.5;
- message->max_angular_acceleration_intake = 800;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 100;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(4.5);
+ goal_builder.add_max_angular_acceleration_intake(800);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(100);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(4.65);
@@ -1068,17 +1109,18 @@
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1087,17 +1129,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.9;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 5.0;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.9);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(5.0);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1113,18 +1156,19 @@
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1133,18 +1177,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 1.3;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 1.0;
- message->max_angular_acceleration_shoulder = 1.0;
- message->max_angular_velocity_wrist = 10.0;
- message->max_angular_acceleration_wrist = 160.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(1.3);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(1.0);
+ goal_builder.add_max_angular_acceleration_shoulder(1.0);
+ goal_builder.add_max_angular_velocity_wrist(10.0);
+ goal_builder.add_max_angular_acceleration_wrist(160.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1165,21 +1210,26 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
- message->angle_wrist = 0.0; // Stowed
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderRange.lower); // Down
+ goal_builder.add_angle_wrist(0.0); // Stowed
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 4.0; // in the collision area
- message->angle_wrist = M_PI / 2.0; // down
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 4.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI / 2.0); // down
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1188,27 +1238,30 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(), M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder()->angle(),
+ 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist()->angle(),
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 2.0; // in the collision area
- message->angle_wrist = M_PI; // forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 2.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI); // forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1224,11 +1277,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = M_PI; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(M_PI); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -1237,12 +1291,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake()->angle(), 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Make sure that we can properly detect a collision.
@@ -1250,11 +1304,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.5;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.5);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1290,11 +1345,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1333,11 +1389,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1353,7 +1410,7 @@
RunFor(chrono::seconds(2));
superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_goal_fetcher_->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder());
}
// Make sure that we land slowly.
@@ -1361,27 +1418,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.25;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.25);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
@@ -1400,27 +1459,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI / 2.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI / 2.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 00fa7dd..abe2ec9 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2016/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() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2016/control_loops/superstructure/superstructure_output.fbs b/y2016/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..40a0091
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,22 @@
+namespace y2016.control_loops.superstructure;
+
+table Output {
+ voltage_intake:float;
+ voltage_shoulder:float;
+ voltage_wrist:float;
+
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, release the latch to trigger the climber to unfold.
+ unfold_climber:bool;
+
+ // If true, release the latch to hold the traverse mechanism in the middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/superstructure/superstructure_position.fbs b/y2016/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..fb356e0
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,19 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table Position {
+ // Zero for the intake potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the shoulder potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the wrist potentiometer value is parallel to the arm and with
+ // the shooter wheels pointed towards the shoulder joint. This is measured
+ // relative to the arm, not the ground, not like the world we actually
+ // present to users.
+ intake:frc971.PotAndIndexPosition;
+ shoulder:frc971.PotAndIndexPosition;
+ wrist:frc971.PotAndIndexPosition;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/superstructure/superstructure_status.fbs b/y2016/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..373bfe2
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,56 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table JointState {
+ // Angle of the joint in radians.
+ angle:float;
+ // Angular velocity of the joint in radians/second.
+ angular_velocity:float;
+ // Profiled goal angle of the joint in radians.
+ goal_angle:float;
+ // Profiled goal angular velocity of the joint in radians/second.
+ goal_angular_velocity:float;
+ // Unprofiled goal angle of the joint in radians.
+ unprofiled_goal_angle:float;
+ // Unprofiled goal angular velocity of the joint in radians/second.
+ unprofiled_goal_angular_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table Status {
+ // Are the superstructure subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // The internal state of the state machine.
+ state:int;
+
+
+ // Estimated angles and angular velocities of the superstructure subsystems.
+ intake:JointState;
+ shoulder:JointState;
+ wrist:JointState;
+
+ shoulder_controller_index:int;
+
+ // Is the superstructure collided?
+ is_collided:bool;
+}
+
+root_type Status;
diff --git a/y2016/dashboard/BUILD b/y2016/dashboard/BUILD
index f3998a1..dbc6214 100644
--- a/y2016/dashboard/BUILD
+++ b/y2016/dashboard/BUILD
@@ -1,5 +1,5 @@
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
-load("//aos/downloader:downloader.bzl", "aos_downloader_dir")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
gen_embedded(
name = "gen_embedded",
@@ -28,16 +28,17 @@
deps = [
":gen_embedded",
"//aos:init",
- "//aos/events:event-loop",
- "//aos/events:shm-event-loop",
+ "//aos/events:event_loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
"//aos/seasocks:seasocks_logger",
"//aos/time",
"//aos/util:phased_loop",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
"//third_party/seasocks",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
- "//y2016/vision:vision_queue",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2016/vision:vision_fbs",
],
)
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 27036e0..7f13fbe 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -11,17 +11,18 @@
#include "internal/Embedded.h"
#include "seasocks/Server.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
#include "aos/seasocks/seasocks_logger.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace chrono = ::std::chrono;
@@ -52,17 +53,17 @@
: event_loop_(event_loop),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
autonomous_mode_fetcher_(
event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")),
cur_raw_data_("no data"),
sample_id_(0),
measure_index_(0),
@@ -121,22 +122,22 @@
// TODO(comran): Grab detected voltage from joystick_reader. Except this
// value may not change, so it may be worth it to keep it as it is right
// now.
- if (ball_detector_fetcher_->voltage > 2.5) {
+ if (ball_detector_fetcher_->voltage() > 2.5) {
big_indicator = big_indicator::kBallIntaked;
}
}
if (superstructure_status_fetcher_.get()) {
- if (!superstructure_status_fetcher_->zeroed) {
+ if (!superstructure_status_fetcher_->zeroed()) {
superstructure_state_indicator = superstructure_indicator::kNotZeroed;
}
- if (superstructure_status_fetcher_->estopped) {
+ if (superstructure_status_fetcher_->estopped()) {
superstructure_state_indicator = superstructure_indicator::kEstopped;
}
}
if (autonomous_mode_fetcher_.get()) {
- auto_mode_indicator = autonomous_mode_fetcher_->mode;
+ auto_mode_indicator = autonomous_mode_fetcher_->mode();
}
AddPoint("big indicator", big_indicator);
@@ -285,7 +286,10 @@
::aos::InitNRT();
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::seasocks::Server server(::std::shared_ptr<seasocks::Logger>(
new ::aos::seasocks::SeasocksLogger(::seasocks::Logger::Level::Info)));
diff --git a/y2016/dashboard/dashboard.h b/y2016/dashboard/dashboard.h
index ed5a579..f019ec7 100644
--- a/y2016/dashboard/dashboard.h
+++ b/y2016/dashboard/dashboard.h
@@ -14,13 +14,13 @@
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace dashboard {
@@ -71,7 +71,7 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
// Storage vector that is written and overwritten with data in a FIFO fashion.
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 276fda6..50f2f8a 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -11,19 +11,20 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2016/actors/autonomous_actor.h"
#include "y2016/actors/superstructure_actor.h"
#include "y2016/actors/vision_align_actor.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
+#include "y2016/vision/vision_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
@@ -73,30 +74,28 @@
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
shooter_goal_sender_(
- event_loop->MakeSender<
- ::y2016::control_loops::shooter::ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ event_loop->MakeSender<::y2016::control_loops::shooter::Goal>(
+ "/shooter")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")),
superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ ->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")),
drivetrain_goal_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -116,14 +115,14 @@
vision_status_fetcher_.Fetch();
if (vision_status_fetcher_.get()) {
- vision_valid_ = (vision_status_fetcher_->left_image_valid &&
- vision_status_fetcher_->right_image_valid);
- last_angle_ = vision_status_fetcher_->horizontal_angle;
+ vision_valid_ = (vision_status_fetcher_->left_image_valid() &&
+ vision_status_fetcher_->right_image_valid());
+ last_angle_ = vision_status_fetcher_->horizontal_angle();
}
if (data.IsPressed(kVisionAlign)) {
if (vision_valid_ && !vision_action_running_) {
- actors::VisionAlignActionParams params;
+ actors::vision_align_action::VisionAlignActionParamsT params;
EnqueueAction(vision_align_action_factory_.Make(params));
vision_action_running_ = true;
AOS_LOG(INFO, "Starting vision align\n");
@@ -167,7 +166,7 @@
}
if (superstructure_status_fetcher_.get() &&
- superstructure_status_fetcher_->zeroed) {
+ superstructure_status_fetcher_->zeroed()) {
if (waiting_for_zero_) {
AOS_LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
waiting_for_zero_ = false;
@@ -191,7 +190,7 @@
wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle();
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -201,7 +200,7 @@
wrist_goal_ = -0.62 - 0.02;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle();
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -225,7 +224,7 @@
intake_goal_ = 0.0;
if (data.PosEdge(kExpand)) {
is_expanding_ = true;
- actors::SuperstructureActionParams params;
+ actors::superstructure_action::SuperstructureActionParamsT params;
params.partial_angle = 0.3;
params.delay_time = 0.7;
params.full_angle = shoulder_goal_;
@@ -248,7 +247,7 @@
bool ball_detected = false;
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
if (data.PosEdge(kIntakeIn)) {
saw_ball_when_started_intaking_ = ball_detected;
@@ -271,16 +270,16 @@
drivetrain_goal_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get() &&
drivetrain_goal_fetcher_.get()) {
- const double left_goal = drivetrain_goal_fetcher_->left_goal;
- const double right_goal = drivetrain_goal_fetcher_->right_goal;
+ const double left_goal = drivetrain_goal_fetcher_->left_goal();
+ const double right_goal = drivetrain_goal_fetcher_->right_goal();
const double left_current =
- drivetrain_status_fetcher_->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position();
const double right_current =
- drivetrain_status_fetcher_->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position();
const double left_velocity =
- drivetrain_status_fetcher_->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity();
const double right_velocity =
- drivetrain_status_fetcher_->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity();
if (vision_action_running_ && ::std::abs(last_angle_) < 0.02 &&
::std::abs((left_goal - right_goal) -
(left_current - right_current)) /
@@ -331,54 +330,57 @@
if (!waiting_for_zero_) {
if (!is_expanding_) {
- auto new_superstructure_goal =
- superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->angle_intake = intake_goal_;
- new_superstructure_goal->angle_shoulder = shoulder_goal_;
- new_superstructure_goal->angle_wrist = wrist_goal_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->max_angular_velocity_intake = 7.0;
- new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
- new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+ ::y2016::control_loops::superstructure::Goal::Builder
+ superstructure_builder = builder.MakeBuilder<
+ ::y2016::control_loops::superstructure::Goal>();
+ superstructure_builder.add_angle_intake(intake_goal_);
+ superstructure_builder.add_angle_shoulder(shoulder_goal_);
+ superstructure_builder.add_angle_wrist(wrist_goal_);
+
+ superstructure_builder.add_max_angular_velocity_intake(7.0);
+ superstructure_builder.add_max_angular_velocity_shoulder(4.0);
+ superstructure_builder.add_max_angular_velocity_wrist(10.0);
if (use_slow_profile) {
- new_superstructure_goal->max_angular_acceleration_intake = 10.0;
+ superstructure_builder.add_max_angular_acceleration_intake(10.0);
} else {
- new_superstructure_goal->max_angular_acceleration_intake = 40.0;
+ superstructure_builder.add_max_angular_acceleration_intake(40.0);
}
- new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
+ superstructure_builder.add_max_angular_acceleration_shoulder(10.0);
if (shoulder_goal_ > 1.0) {
- new_superstructure_goal->max_angular_acceleration_wrist = 45.0;
+ superstructure_builder.add_max_angular_acceleration_wrist(45.0);
} else {
- new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+ superstructure_builder.add_max_angular_acceleration_wrist(25.0);
}
// Granny mode
/*
- new_superstructure_goal->max_angular_velocity_intake = 0.2;
- new_superstructure_goal->max_angular_velocity_shoulder = 0.2;
- new_superstructure_goal->max_angular_velocity_wrist = 0.2;
- new_superstructure_goal->max_angular_acceleration_intake = 1.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 1.0;
- new_superstructure_goal->max_angular_acceleration_wrist = 1.0;
+ superstructure_builder.add_max_angular_velocity_intake(0.2);
+ superstructure_builder.add_max_angular_velocity_shoulder(0.2);
+ superstructure_builder.add_max_angular_velocity_wrist(0.2);
+ superstructure_builder.add_max_angular_acceleration_intake(1.0);
+ superstructure_builder.add_max_angular_acceleration_shoulder(1.0);
+ superstructure_builder.add_max_angular_acceleration_wrist(1.0);
*/
if (is_intaking_) {
- new_superstructure_goal->voltage_top_rollers = 12.0;
- new_superstructure_goal->voltage_bottom_rollers = 12.0;
+ superstructure_builder.add_voltage_top_rollers(12.0);
+ superstructure_builder.add_voltage_bottom_rollers(12.0);
} else if (is_outtaking_) {
- new_superstructure_goal->voltage_top_rollers = -12.0;
- new_superstructure_goal->voltage_bottom_rollers = -7.0;
+ superstructure_builder.add_voltage_top_rollers(-12.0);
+ superstructure_builder.add_voltage_bottom_rollers(-7.0);
} else {
- new_superstructure_goal->voltage_top_rollers = 0.0;
- new_superstructure_goal->voltage_bottom_rollers = 0.0;
+ superstructure_builder.add_voltage_top_rollers(0.0);
+ superstructure_builder.add_voltage_bottom_rollers(0.0);
}
- new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
- new_superstructure_goal->unfold_climber = false;
- new_superstructure_goal->voltage_climber = voltage_climber;
- new_superstructure_goal->traverse_down = traverse_down_;
- new_superstructure_goal->force_intake = true;
+ superstructure_builder.add_traverse_unlatched(traverse_unlatched_);
+ superstructure_builder.add_unfold_climber(false);
+ superstructure_builder.add_voltage_climber(voltage_climber);
+ superstructure_builder.add_traverse_down(traverse_down_);
+ superstructure_builder.add_force_intake(true);
- if (!new_superstructure_goal.Send()) {
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
} else {
AOS_LOG(DEBUG, "sending goals: intake: %f, shoulder: %f, wrist: %f\n",
@@ -386,15 +388,19 @@
}
}
- auto shooter_message = shooter_goal_sender_.MakeMessage();
- shooter_message->angular_velocity = shooter_velocity_;
- shooter_message->clamp_open = is_intaking_ || is_outtaking_;
- shooter_message->push_to_shooter = fire_;
- shooter_message->force_lights_on = force_lights_on;
- shooter_message->shooting_forwards = wrist_goal_ > 0;
+ {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ y2016::control_loops::shooter::Goal::Builder shooter_builder =
+ builder.MakeBuilder<y2016::control_loops::shooter::Goal>();
+ shooter_builder.add_angular_velocity(shooter_velocity_);
+ shooter_builder.add_clamp_open(is_intaking_ || is_outtaking_);
+ shooter_builder.add_push_to_shooter(fire_);
+ shooter_builder.add_force_lights_on(force_lights_on);
+ shooter_builder.add_shooting_forwards(wrist_goal_ > 0);
- if (!shooter_message.Send()) {
- AOS_LOG(ERROR, "Sending shooter goal failed.\n");
+ if (!builder.Send(shooter_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending shooter goal failed.\n");
+ }
}
}
}
@@ -402,15 +408,15 @@
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
- ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::shooter::Goal>
shooter_goal_sender_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
// Whatever these are set to are our default goals to send out after zeroing.
@@ -457,7 +463,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());
::y2016::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
index 0c5d97b..79ec2d9 100644
--- a/y2016/queues/BUILD
+++ b/y2016/queues/BUILD
@@ -1,17 +1,11 @@
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 = 'ball_detector',
- srcs = [
- 'ball_detector.q',
- ],
-)
-
-queue_library(
- name = 'profile_params',
- srcs = [
- 'profile_params.q',
- ],
+flatbuffer_cc_library(
+ name = "ball_detector_fbs",
+ srcs = [
+ "ball_detector.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/y2016/queues/ball_detector.q b/y2016/queues/ball_detector.fbs
similarity index 69%
rename from y2016/queues/ball_detector.q
rename to y2016/queues/ball_detector.fbs
index 9dc686f..c4ef07c 100644
--- a/y2016/queues/ball_detector.q
+++ b/y2016/queues/ball_detector.fbs
@@ -1,7 +1,7 @@
-package y2016.sensors;
+namespace y2016.sensors;
-// Published on ".y2016.sensors.ball_detector"
-message BallDetector {
+// Published on "/superstructure"
+table BallDetector {
// Voltage measured by the ball detector sensor.
// Higher voltage means ball is closer to detector, lower voltage means ball
@@ -9,5 +9,7 @@
// TODO(comran): Check to see if our sensor's output corresponds with the
// comment above.
- double voltage;
-};
+ voltage:double;
+}
+
+root_type BallDetector;
diff --git a/y2016/queues/profile_params.q b/y2016/queues/profile_params.q
deleted file mode 100644
index 56b2ab3..0000000
--- a/y2016/queues/profile_params.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package y2016;
-
-struct ProfileParams {
- double velocity;
- double acceleration;
-};
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 3427973..c4d472b 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -1,12 +1,13 @@
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
-queue_library(
- name = "vision_queue",
+flatbuffer_cc_library(
+ name = "vision_fbs",
srcs = [
- "vision.q",
+ "vision.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -130,15 +131,15 @@
deps = [
":stereo_geometry",
":vision_data",
- ":vision_queue",
+ ":vision_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/mutex",
"//aos/time",
"//aos/vision/events:udp",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2016:constants",
],
)
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index a66be59..b946f8b 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -10,19 +10,18 @@
#include <thread>
#include <vector>
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
#include "aos/vision/events/udp.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2016/constants.h"
#include "y2016/vision/stereo_geometry.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace vision {
@@ -197,17 +196,20 @@
DrivetrainOffsetCalculator(::aos::EventLoop *event_loop)
: drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")) {}
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {}
// Takes a vision status message with everything except
// drivetrain_{left,right}_position set and sets those.
// Returns false if it doesn't have enough data to fill them out.
- bool CompleteVisionStatus(::y2016::vision::VisionStatus *status) {
+ bool CompleteVisionStatus(::y2016::vision::VisionStatusT *status) {
while (drivetrain_status_fetcher_.FetchNext()) {
- data_[data_index_].time = drivetrain_status_fetcher_->sent_time;
- data_[data_index_].left = drivetrain_status_fetcher_->estimated_left_position;
- data_[data_index_].right = drivetrain_status_fetcher_->estimated_right_position;
+ data_[data_index_].time =
+ drivetrain_status_fetcher_.context().monotonic_sent_time;
+ data_[data_index_].left =
+ drivetrain_status_fetcher_->estimated_left_position();
+ data_[data_index_].right =
+ drivetrain_status_fetcher_->estimated_right_position();
++data_index_;
if (data_index_ == data_.size()) data_index_ = 0;
if (valid_data_ < data_.size()) ++valid_data_;
@@ -286,7 +288,7 @@
}
}
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::std::array<DrivetrainData, 200> data_;
@@ -297,11 +299,13 @@
};
void Main() {
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::aos::Sender<::y2016::vision::VisionStatus> vision_status_sender =
- event_loop.MakeSender<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status");
+ event_loop.MakeSender<::y2016::vision::VisionStatus>("/superstructure");
StereoGeometry stereo(constants::GetValues().vision_name);
AOS_LOG(INFO, "calibration: %s\n",
@@ -339,9 +343,10 @@
const bool left_image_valid = left.is_valid();
const bool right_image_valid = right.is_valid();
- auto new_vision_status = vision_status_sender.MakeMessage();
- new_vision_status->left_image_valid = left_image_valid;
- new_vision_status->right_image_valid = right_image_valid;
+ auto builder = vision_status_sender.MakeBuilder();
+ VisionStatusT new_vision_status;
+ new_vision_status.left_image_valid = left_image_valid;
+ new_vision_status.right_image_valid = right_image_valid;
if (left_image_valid && right_image_valid) {
::aos::vision::Vector<2> center_left(0.0, 0.0);
::aos::vision::Vector<2> center_right(0.0, 0.0);
@@ -367,7 +372,7 @@
if (left.capture_time() < right.capture_time()) {
filtered_center_left = center_left;
filtered_angle_left = angle_left;
- new_vision_status->target_time =
+ new_vision_status.target_time =
chrono::duration_cast<chrono::nanoseconds>(
left.capture_time().time_since_epoch())
.count();
@@ -377,7 +382,7 @@
} else {
filtered_center_right = center_right;
filtered_angle_right = angle_right;
- new_vision_status->target_time =
+ new_vision_status.target_time =
chrono::duration_cast<chrono::nanoseconds>(
right.capture_time().time_since_epoch())
.count();
@@ -389,28 +394,27 @@
double distance, horizontal_angle, vertical_angle;
stereo.Process(filtered_center_left, filtered_center_right, &distance,
&horizontal_angle, &vertical_angle);
- new_vision_status->left_image_timestamp =
+ new_vision_status.left_image_timestamp =
left.target().image_timestamp();
- new_vision_status->right_image_timestamp =
+ new_vision_status.right_image_timestamp =
right.target().image_timestamp();
- new_vision_status->left_send_timestamp = left.target().send_timestamp();
- new_vision_status->right_send_timestamp =
+ new_vision_status.left_send_timestamp = left.target().send_timestamp();
+ new_vision_status.right_send_timestamp =
right.target().send_timestamp();
- new_vision_status->horizontal_angle = horizontal_angle;
- new_vision_status->vertical_angle = vertical_angle;
- new_vision_status->distance = distance;
- new_vision_status->angle =
+ new_vision_status.horizontal_angle = horizontal_angle;
+ new_vision_status.vertical_angle = vertical_angle;
+ new_vision_status.distance = distance;
+ new_vision_status.angle =
(filtered_angle_left + filtered_angle_right) / 2.0;
}
- if (drivetrain_offset.CompleteVisionStatus(new_vision_status.get())) {
- AOS_LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
+ if (drivetrain_offset.CompleteVisionStatus(&new_vision_status)) {
+ if (!builder.Send(
+ VisionStatus::Pack(*builder.fbb(), &new_vision_status))) {
AOS_LOG(ERROR, "Failed to send vision information\n");
}
} else {
- AOS_LOG_STRUCT(WARNING, "vision without drivetrain",
- *new_vision_status);
+ AOS_LOG(WARNING, "vision without drivetrain");
}
}
diff --git a/y2016/vision/vision.q b/y2016/vision/vision.fbs
similarity index 65%
rename from y2016/vision/vision.q
rename to y2016/vision/vision.fbs
index 06deb1f..4394b10 100644
--- a/y2016/vision/vision.q
+++ b/y2016/vision/vision.fbs
@@ -1,36 +1,38 @@
-package y2016.vision;
+namespace y2016.vision;
// Published on ".y2016.vision.vision_status"
-message VisionStatus {
- bool left_image_valid;
- bool right_image_valid;
+table VisionStatus {
+ left_image_valid:bool;
+ right_image_valid:bool;
// Times when the images were taken as nanoseconds on CLOCK_MONOTONIC on the
// TK1.
- int64_t left_image_timestamp;
- int64_t right_image_timestamp;
+ left_image_timestamp:long;
+ right_image_timestamp:long;
// Times when the images were sent from the TK1 as nanoseconds on the TK1's
// CLOCK_MONOTONIC.
- int64_t left_send_timestamp;
- int64_t right_send_timestamp;
+ left_send_timestamp:long;
+ right_send_timestamp:long;
// Horizontal angle of the goal in radians.
// TODO(Brian): Figure out which way is positive.
- double horizontal_angle;
+ horizontal_angle:double;
// Vertical angle of the goal in radians.
// TODO(Brian): Figure out which way is positive.
- double vertical_angle;
+ vertical_angle:double;
// Distance to the target in meters.
- double distance;
+ distance:double;
// The angle in radians of the bottom of the target.
- double angle;
+ angle:double;
// Capture time of the angle using the clock behind monotonic_clock::now().
- int64_t target_time;
+ target_time:long;
// The estimated positions of both sides of the drivetrain when the frame
// was captured.
// These are the estimated_left_position and estimated_right_position members
// of the drivetrain queue.
- double drivetrain_left_position;
- double drivetrain_right_position;
-};
+ drivetrain_left_position:double;
+ drivetrain_right_position:double;
+}
+
+root_type VisionStatus;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 9b31a0f..c36bab1 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -20,18 +20,18 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+//#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -42,21 +42,22 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
-using ::y2016::control_loops::shooter::ShooterQueue;
-using ::y2016::control_loops::SuperstructureQueue;
+namespace shooter = ::y2016::control_loops::shooter;
+namespace superstructure = ::y2016::control_loops::superstructure;
namespace y2016 {
namespace wpilib {
@@ -149,19 +150,19 @@
: ::frc971::wpilib::SensorReader(event_loop),
ball_detector_sender_(
event_loop->MakeSender<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
- shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")),
+ "/aos")),
+ shooter_position_sender_(
+ event_loop->MakeSender<shooter::Position>("/shooter")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -258,78 +259,101 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- drivetrain_message->left_shifter_position =
- hall_translate(drivetrain_left_hall_->GetVoltage());
- drivetrain_message->right_shifter_position =
- hall_translate(drivetrain_right_hall_->GetVoltage());
+ position_builder.add_right_encoder(
+ drivetrain_translate(-drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ position_builder.add_left_shifter_position(
+ hall_translate(drivetrain_left_hall_->GetVoltage()));
+ position_builder.add_right_shifter_position(
+ hall_translate(drivetrain_right_hall_->GetVoltage()));
+
+ builder.Send(position_builder.Finish());
}
}
void RunDmaIteration() {
const auto &values = constants::GetValues();
{
- auto shooter_message = shooter_position_sender_.MakeMessage();
- shooter_message->theta_left =
- shooter_translate(-shooter_left_encoder_->GetRaw());
- shooter_message->theta_right =
- shooter_translate(shooter_right_encoder_->GetRaw());
- shooter_message.Send();
+ auto builder = shooter_position_sender_.MakeBuilder();
+ shooter::Position::Builder shooter_builder =
+ builder.MakeBuilder<shooter::Position>();
+ shooter_builder.add_theta_left(
+ shooter_translate(-shooter_left_encoder_->GetRaw()));
+ shooter_builder.add_theta_right(
+ shooter_translate(shooter_right_encoder_->GetRaw()));
+ builder.Send(shooter_builder.Finish());
}
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
- CopyPosition(intake_encoder_, &superstructure_message->intake,
- intake_translate, intake_pot_translate, false,
- values.intake.pot_offset);
- CopyPosition(shoulder_encoder_, &superstructure_message->shoulder,
- shoulder_translate, shoulder_pot_translate, false,
- values.shoulder.pot_offset);
- CopyPosition(wrist_encoder_, &superstructure_message->wrist,
- wrist_translate, wrist_pot_translate, true,
- values.wrist.pot_offset);
+ auto builder = superstructure_position_sender_.MakeBuilder();
- superstructure_message.Send();
+ frc971::PotAndIndexPositionT intake;
+ CopyPosition(intake_encoder_, &intake, intake_translate,
+ intake_pot_translate, false, values.intake.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &intake);
+
+ frc971::PotAndIndexPositionT shoulder;
+ CopyPosition(shoulder_encoder_, &shoulder, shoulder_translate,
+ shoulder_pot_translate, false, values.shoulder.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &shoulder);
+
+ frc971::PotAndIndexPositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist, wrist_translate, wrist_pot_translate,
+ true, values.wrist.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &wrist);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
{
- auto ball_detector_message = ball_detector_sender_.MakeMessage();
- ball_detector_message->voltage = ball_detector_->GetVoltage();
- AOS_LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
- ball_detector_message.Send();
+ auto builder = ball_detector_sender_.MakeBuilder();
+ ::y2016::sensors::BallDetector::Builder ball_detector_builder =
+ builder.MakeBuilder<y2016::sensors::BallDetector>();
+ ball_detector_builder.add_voltage(ball_detector_->GetVoltage());
+ builder.Send(ball_detector_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+ ::frc971::autonomous::AutonomousMode::Builder auto_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+ int mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
+ mode |= 1 << i;
}
}
- AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
+ auto_builder.add_mode(mode);
+ builder.Send(auto_builder.Finish());
}
}
private:
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<shooter::Position> shooter_position_sender_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
@@ -349,17 +373,19 @@
class SolenoidWriter {
public:
SolenoidWriter(::aos::EventLoop *event_loop,
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ ::frc971::wpilib::BufferedPcm *pcm)
: pcm_(pcm),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")),
drivetrain_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
- shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
- superstructure_(event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ shooter_(event_loop->MakeFetcher<shooter::Output>("/shooter")),
+ superstructure_(
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Output>(
+ "/superstructure")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -420,27 +446,25 @@
{
drivetrain_.Fetch();
if (drivetrain_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
drivetrain_shifter_->Set(
- !(drivetrain_->left_high || drivetrain_->right_high));
+ !(drivetrain_->left_high() || drivetrain_->right_high()));
}
}
{
shooter_.Fetch();
if (shooter_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_clamp_->Set(shooter_->clamp_open);
- shooter_pusher_->Set(shooter_->push_to_shooter);
- lights_->Set(shooter_->lights_on);
- if (shooter_->forwards_flashlight) {
- if (shooter_->backwards_flashlight) {
+ shooter_clamp_->Set(shooter_->clamp_open());
+ shooter_pusher_->Set(shooter_->push_to_shooter());
+ lights_->Set(shooter_->lights_on());
+ if (shooter_->forwards_flashlight()) {
+ if (shooter_->backwards_flashlight()) {
flashlight_->Set(::frc::Relay::kOn);
} else {
flashlight_->Set(::frc::Relay::kReverse);
}
} else {
- if (shooter_->backwards_flashlight) {
+ if (shooter_->backwards_flashlight()) {
flashlight_->Set(::frc::Relay::kForward);
} else {
flashlight_->Set(::frc::Relay::kOff);
@@ -452,26 +476,29 @@
{
superstructure_.Fetch();
if (superstructure_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ climber_trigger_->Set(superstructure_->unfold_climber());
- climber_trigger_->Set(superstructure_->unfold_climber);
-
- traverse_->Set(superstructure_->traverse_down);
- traverse_latch_->Set(superstructure_->traverse_unlatched);
+ traverse_->Set(superstructure_->traverse_down());
+ traverse_latch_->Set(superstructure_->traverse_unlatched());
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
- { to_log.compressor_on = compressor_->Enabled(); }
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
+
+ to_log_builder.add_compressor_on(compressor_->Enabled());
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
}
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
+ ::frc971::wpilib::BufferedPcm *pcm_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_,
shooter_clamp_, shooter_pusher_, lights_, traverse_, traverse_latch_,
@@ -479,17 +506,16 @@
::std::unique_ptr<::frc::Relay> flashlight_;
::std::unique_ptr<::frc::Compressor> compressor_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output> drivetrain_;
+ ::aos::Fetcher<shooter::Output> shooter_;
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Output>
superstructure_;
};
-class ShooterWriter : public LoopOutputHandler<ShooterQueue::Output> {
+class ShooterWriter : public LoopOutputHandler<shooter::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : LoopOutputHandler<ShooterQueue::Output>(
- event_loop, ".y2016.control_loops.shooter.shooter_queue.output") {}
+ : LoopOutputHandler<shooter::Output>(event_loop, "/shooter") {}
void set_shooter_left_talon(::std::unique_ptr<::frc::Talon> t) {
shooter_left_talon_ = ::std::move(t);
@@ -500,11 +526,9 @@
}
private:
- void Write(const ShooterQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
-
- shooter_left_talon_->SetSpeed(output.voltage_left / 12.0);
- shooter_right_talon_->SetSpeed(-output.voltage_right / 12.0);
+ void Write(const shooter::Output &output) override {
+ shooter_left_talon_->SetSpeed(output.voltage_left() / 12.0);
+ shooter_right_talon_->SetSpeed(-output.voltage_right() / 12.0);
}
void Stop() override {
@@ -516,13 +540,11 @@
::std::unique_ptr<::frc::Talon> shooter_left_talon_, shooter_right_talon_;
};
-class SuperstructureWriter
- : public LoopOutputHandler<
- ::y2016::control_loops::SuperstructureQueue::Output> {
+class SuperstructureWriter : public LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : LoopOutputHandler<::y2016::control_loops::SuperstructureQueue::Output>(
- event_loop, ".y2016.control_loops.superstructure_queue.output") {}
+ : LoopOutputHandler<superstructure::Output>(event_loop,
+ "/superstructure") {}
void set_intake_talon(::std::unique_ptr<::frc::Talon> t) {
intake_talon_ = ::std::move(t);
@@ -549,21 +571,19 @@
}
private:
- virtual void Write(const ::y2016::control_loops::SuperstructureQueue::Output
- &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- intake_talon_->SetSpeed(::aos::Clip(output.voltage_intake,
+ virtual void Write(const superstructure::Output &output) override {
+ intake_talon_->SetSpeed(::aos::Clip(output.voltage_intake(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- shoulder_talon_->SetSpeed(::aos::Clip(-output.voltage_shoulder,
+ shoulder_talon_->SetSpeed(::aos::Clip(-output.voltage_shoulder(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_talon_->SetSpeed(
- ::aos::Clip(output.voltage_wrist, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- top_rollers_talon_->SetSpeed(-output.voltage_top_rollers / 12.0);
- bottom_rollers_talon_->SetSpeed(-output.voltage_bottom_rollers / 12.0);
- climber_talon_->SetSpeed(-output.voltage_climber / 12.0);
+ wrist_talon_->SetSpeed(::aos::Clip(output.voltage_wrist(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ top_rollers_talon_->SetSpeed(-output.voltage_top_rollers() / 12.0);
+ bottom_rollers_talon_->SetSpeed(-output.voltage_bottom_rollers() / 12.0);
+ climber_talon_->SetSpeed(-output.voltage_climber() / 12.0);
}
virtual void Stop() override {
@@ -585,19 +605,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(5));
@@ -631,19 +654,19 @@
// TODO(Brian): This interacts poorly with the SpiRxClearer in ADIS16448.
// Thread 4.
- ::aos::ShmEventLoop gyro_event_loop;
+ ::aos::ShmEventLoop gyro_event_loop(&config.message());
::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
AddLoop(&gyro_event_loop);
// Thread 5.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
auto imu_trigger = make_unique<::frc::DigitalInput>(3);
::frc971::wpilib::ADIS16448 imu(&imu_event_loop, ::frc::SPI::Port::kMXP,
imu_trigger.get());
AddLoop(&imu_event_loop);
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::Talon>(new ::frc::Talon(5)), false);
@@ -673,10 +696,10 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm.get());
solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
diff --git a/y2016/y2016.json b/y2016/y2016.json
new file mode 100644
index 0000000..eb8ecd5
--- /dev/null
+++ b/y2016/y2016.json
@@ -0,0 +1,54 @@
+{
+ "channels":
+ [
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.sensors.BallDetector",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}
diff --git a/y2017/BUILD b/y2017/BUILD
index 404683b..8bf2ff8 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
cc_library(
name = "constants",
@@ -39,11 +40,27 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2017/actors:autonomous_action_lib",
"//y2017/control_loops/drivetrain:drivetrain_base",
- "//y2017/control_loops/superstructure:superstructure_queue",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+aos_config(
+ name = "config",
+ src = "y2017.json",
+ flatbuffers = [
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_output_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "//y2017/vision:vision_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
],
)
@@ -60,16 +77,16 @@
"//aos:math",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
@@ -78,13 +95,14 @@
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
- "//y2017/control_loops/superstructure:superstructure_queue",
+ "//y2017/control_loops/superstructure:superstructure_output_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
],
)
diff --git a/y2017/actors/BUILD b/y2017/actors/BUILD
index 8c1b1cd..3b3e0a2 100644
--- a/y2017/actors/BUILD
+++ b/y2017/actors/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
filegroup(
name = "binaries",
srcs = [
@@ -19,14 +17,15 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2017/control_loops/drivetrain:drivetrain_base",
- "//y2017/control_loops/superstructure:superstructure_queue",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
],
)
@@ -38,7 +37,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index a678e79..1124780 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -7,30 +7,37 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
namespace y2017 {
namespace actors {
using ::aos::monotonic_clock;
+using ::frc971::ProfileParametersT;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-const ProfileParameters kGearBallBackDrive = {3.0, 3.5};
-const ProfileParameters kGearDrive = {1.5, 2.0};
-const ProfileParameters kGearFastDrive = {2.0, 2.5};
-const ProfileParameters kGearSlowDrive = {1.0, 2.0};
-const ProfileParameters kGearPlaceDrive = {0.40, 2.0};
-const ProfileParameters kSlowDrive = {3.0, 2.0};
-const ProfileParameters kSlowTurn = {3.0, 3.0};
-const ProfileParameters kFirstTurn = {1.0, 1.5};
-const ProfileParameters kFirstGearStartTurn = {2.0, 3.0};
-const ProfileParameters kFirstGearTurn = {2.0, 5.0};
-const ProfileParameters kSecondGearTurn = {3.0, 5.0};
-const ProfileParameters kSmashTurn = {1.5, 5.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kGearBallBackDrive = MakeProfileParameters(3.0, 3.5);
+const ProfileParametersT kGearDrive = MakeProfileParameters(1.5, 2.0);
+const ProfileParametersT kGearFastDrive = MakeProfileParameters(2.0, 2.5);
+const ProfileParametersT kGearSlowDrive = MakeProfileParameters(1.0, 2.0);
+const ProfileParametersT kGearPlaceDrive = MakeProfileParameters(0.40, 2.0);
+const ProfileParametersT kSlowDrive = MakeProfileParameters(3.0, 2.0);
+const ProfileParametersT kSlowTurn = MakeProfileParameters(3.0, 3.0);
+const ProfileParametersT kFirstTurn = MakeProfileParameters(1.0, 1.5);
+const ProfileParametersT kFirstGearStartTurn = MakeProfileParameters(2.0, 3.0);
+const ProfileParametersT kFirstGearTurn = MakeProfileParameters(2.0, 5.0);
+const ProfileParametersT kSecondGearTurn = MakeProfileParameters(3.0, 5.0);
+const ProfileParametersT kSmashTurn = MakeProfileParameters(1.5, 5.0);
} // namespace
@@ -38,22 +45,21 @@
: frc971::autonomous::BaseAutonomousActor(
event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2017::control_loops::SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
- superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2017::control_loops::SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")) {}
+ ->MakeFetcher<::y2017::control_loops::superstructure::Status>(
+ "/superstructure")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2017::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
Reset();
- switch (params.mode) {
+ switch (params->mode()) {
case 503: {
// Middle gear auto.
// Red is positive.
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index f5d4c3f..f0bcfc8 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -7,9 +7,9 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace actors {
@@ -20,7 +20,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
@@ -38,9 +38,9 @@
SendSuperstructureGoal();
}
- ::aos::Fetcher<::y2017::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2017::control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2017::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2017::control_loops::superstructure::Goal>
superstructure_goal_sender_;
double intake_goal_ = 0.0;
@@ -82,7 +82,7 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- if (superstructure_status_fetcher_->hood.zeroed) {
+ if (superstructure_status_fetcher_->hood()->zeroed()) {
return;
}
}
@@ -91,40 +91,81 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->intake.distance = intake_goal_;
- new_superstructure_goal->intake.disable_intake = false;
- new_superstructure_goal->turret.angle = turret_goal_;
- new_superstructure_goal->turret.track = vision_track_;
- new_superstructure_goal->hood.angle = hood_goal_;
- new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->intake.profile_params.max_velocity =
- intake_max_velocity_;
- new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
- new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset = frc971::CreateProfileParameters(
+ *builder.fbb(), intake_max_velocity_, 5.0);
- new_superstructure_goal->intake.profile_params.max_acceleration = 5.0;
- new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
- new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
+ control_loops::superstructure::IntakeGoal::Builder intake_builder =
+ builder.MakeBuilder<control_loops::superstructure::IntakeGoal>();
+ intake_builder.add_distance(intake_goal_);
+ intake_builder.add_disable_intake(false);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ intake_builder.add_voltage_rollers(0.0);
+ intake_builder.add_disable_intake(false);
+ intake_builder.add_gear_servo(gear_servo_);
+ flatbuffers::Offset<control_loops::superstructure::IntakeGoal>
+ intake_offset = intake_builder.Finish();
- new_superstructure_goal->intake.voltage_rollers = 0.0;
- new_superstructure_goal->lights_on = true;
- new_superstructure_goal->intake.disable_intake = false;
- new_superstructure_goal->intake.gear_servo = gear_servo_;
- new_superstructure_goal->use_vision_for_shots = use_vision_for_shots_;
+ control_loops::superstructure::IndexerGoal::Builder indexer_builder =
+ builder.MakeBuilder<control_loops::superstructure::IndexerGoal>();
+ indexer_builder.add_angular_velocity(indexer_angular_velocity_);
+ indexer_builder.add_voltage_rollers(0.0);
+ flatbuffers::Offset<control_loops::superstructure::IndexerGoal>
+ indexer_offset = indexer_builder.Finish();
- new_superstructure_goal->indexer.angular_velocity =
- indexer_angular_velocity_;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset = frc971::CreateProfileParameters(
+ *builder.fbb(), 6.0, 15.0);
+ control_loops::superstructure::TurretGoal::Builder turret_builder =
+ builder.MakeBuilder<control_loops::superstructure::TurretGoal>();
+ turret_builder.add_angle(turret_goal_);
+ turret_builder.add_track(vision_track_);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<control_loops::superstructure::TurretGoal> turret_offset =
+ turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset = frc971::CreateProfileParameters(
+ *builder.fbb(), 5.0, 25.0);
+ control_loops::superstructure::HoodGoal::Builder hood_builder =
+ builder.MakeBuilder<control_loops::superstructure::HoodGoal>();
+ hood_builder.add_angle(hood_goal_);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<control_loops::superstructure::HoodGoal> hood_offset =
+ hood_builder.Finish();
+
+ control_loops::superstructure::ShooterGoal::Builder shooter_builder =
+ builder.MakeBuilder<control_loops::superstructure::ShooterGoal>();
+ shooter_builder.add_angular_velocity(shooter_velocity_);
+ flatbuffers::Offset<control_loops::superstructure::ShooterGoal>
+ shooter_offset = shooter_builder.Finish();
+
+ control_loops::superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+ goal_builder.add_lights_on(true);
+ goal_builder.add_use_vision_for_shots(use_vision_for_shots_);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ flatbuffers::Offset<control_loops::superstructure::Goal> goal_offset =
+ goal_builder.Finish();
+
+ control_loops::superstructure::Goal *goal =
+ GetMutableTemporaryPointer(*builder.fbb(), goal_offset);
if (indexer_angular_velocity_ < -0.1) {
- new_superstructure_goal->indexer.voltage_rollers = 12.0;
- new_superstructure_goal->intake.voltage_rollers = 6.0;
+ goal->mutable_indexer()->mutate_voltage_rollers(12.0);
+ goal->mutable_intake()->mutate_voltage_rollers(6.0);
} else {
- new_superstructure_goal->indexer.voltage_rollers = 0.0;
+ goal->mutable_indexer()->mutate_voltage_rollers(0.0);
}
- if (!new_superstructure_goal.Send()) {
+ if (!builder.Send(goal_offset)) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
diff --git a/y2017/actors/autonomous_actor_main.cc b/y2017/actors/autonomous_actor_main.cc
index 88779d8..d87ac84 100644
--- a/y2017/actors/autonomous_actor_main.cc
+++ b/y2017/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2017/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2017::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2017/control_loops/drivetrain/BUILD b/y2017/control_loops/drivetrain/BUILD
index 409fc93..46af643 100644
--- a/y2017/control_loops/drivetrain/BUILD
+++ b/y2017/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -78,7 +76,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
index a78d97f..c9da772 100644
--- a/y2017/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017/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 "y2017/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 5a00bfe..535475d 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -1,19 +1,49 @@
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",
],
)
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+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_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
cc_library(
name = "superstructure_lib",
srcs = [
@@ -23,10 +53,13 @@
"superstructure.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
":vision_distance_average",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//y2017:constants",
"//y2017/control_loops/superstructure/column",
"//y2017/control_loops/superstructure/hood",
@@ -40,11 +73,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2017: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",
@@ -64,9 +100,8 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -79,11 +114,12 @@
"vision_time_adjuster.h",
],
deps = [
- ":superstructure_queue",
"//aos/containers:ring_buffer",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2017/control_loops/drivetrain:polydrivetrain_plants",
- "//y2017/vision:vision_queue",
+ "//y2017/vision:vision_fbs",
],
)
@@ -92,6 +128,7 @@
srcs = [
"vision_time_adjuster_test.cc",
],
+ data = ["//y2017:config.json"],
deps = [
":vision_time_adjuster",
"//aos/events:simulated_event_loop",
@@ -109,7 +146,7 @@
deps = [
"//aos/containers:ring_buffer",
"//aos/time",
- "//y2017/vision:vision_queue",
+ "//y2017/vision:vision_fbs",
],
)
@@ -120,6 +157,7 @@
],
deps = [
":vision_distance_average",
+ "//aos:flatbuffers",
"//aos/testing:googletest",
"//aos/time",
],
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 694dd47..63dd98c 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -1,88 +1,89 @@
genrule(
- name = 'genrule_column',
- cmd = '$(location //y2017/control_loops/python:column) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:column',
- ],
- outs = [
- 'column_plant.h',
- 'column_plant.cc',
- 'column_integral_plant.h',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.h',
- 'stuck_column_integral_plant.cc',
- ],
+ name = "genrule_column",
+ outs = [
+ "column_plant.h",
+ "column_plant.cc",
+ "column_integral_plant.h",
+ "column_integral_plant.cc",
+ "stuck_column_integral_plant.h",
+ "stuck_column_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:column) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:column",
+ ],
)
cc_library(
- name = 'column_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'column_plant.cc',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.cc',
- ],
- hdrs = [
- 'column_plant.h',
- 'column_integral_plant.h',
- 'stuck_column_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "column_plants",
+ srcs = [
+ "column_integral_plant.cc",
+ "column_plant.cc",
+ "stuck_column_integral_plant.cc",
+ ],
+ hdrs = [
+ "column_integral_plant.h",
+ "column_plant.h",
+ "stuck_column_integral_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'column',
- visibility = ['//visibility:public'],
- srcs = [
- 'column.cc',
- ],
- hdrs = [
- 'column.h',
- ],
- deps = [
- ':column_plants',
- ':column_zeroing',
- '//aos/controls:control_loop',
- '//aos:math',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure/intake:intake',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017/control_loops/superstructure:vision_time_adjuster',
- '//y2017:constants',
- ],
+ name = "column",
+ srcs = [
+ "column.cc",
+ ],
+ hdrs = [
+ "column.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":column_plants",
+ ":column_zeroing",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "//y2017/control_loops/superstructure:vision_time_adjuster",
+ "//y2017/control_loops/superstructure/intake",
+ ],
)
cc_library(
- name = 'column_zeroing',
- srcs = [
- 'column_zeroing.cc',
- ],
- hdrs = [
- 'column_zeroing.h',
- ],
- deps = [
- '//frc971/control_loops:queues',
- '//frc971/zeroing:wrap',
- '//frc971/zeroing:zeroing',
- '//frc971:constants',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing",
+ srcs = [
+ "column_zeroing.cc",
+ ],
+ hdrs = [
+ "column_zeroing.h",
+ ],
+ deps = [
+ "//frc971:constants",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/zeroing",
+ "//frc971/zeroing:wrap",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
)
cc_test(
- name = 'column_zeroing_test',
- srcs = [
- 'column_zeroing_test.cc',
- ],
- deps = [
- ':column_zeroing',
- '//aos/testing:test_shm',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:team_number_test_environment',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing_test",
+ srcs = [
+ "column_zeroing_test.cc",
+ ],
+ deps = [
+ ":column_zeroing",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2017:constants",
+ ],
)
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index ab1ff72..b2e7c5c 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -118,17 +118,18 @@
}
turret_last_position_ = turret_position();
- Y_ << new_position.indexer.encoder, new_position.turret.encoder;
+ Y_ << new_position.indexer()->encoder(), new_position.turret()->encoder();
Y_ += offset_;
loop_->Correct(Y_);
- indexer_history_[indexer_history_position_] = new_position.indexer.encoder;
+ indexer_history_[indexer_history_position_] =
+ new_position.indexer()->encoder();
indexer_history_position_ = (indexer_history_position_ + 1) % kHistoryLength;
indexer_dt_velocity_ =
- (new_position.indexer.encoder - indexer_last_position_) /
+ (new_position.indexer()->encoder() - indexer_last_position_) /
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
- indexer_last_position_ = new_position.indexer.encoder;
+ indexer_last_position_ = new_position.indexer()->encoder();
stuck_indexer_detector_->Correct(Y_);
@@ -304,9 +305,11 @@
}
void ColumnProfiledSubsystem::AdjustProfile(
- const ::frc971::ProfileParameters &profile_parameters) {
- AdjustProfile(profile_parameters.max_velocity,
- profile_parameters.max_acceleration);
+ const ::frc971::ProfileParameters *profile_parameters) {
+ AdjustProfile(
+ profile_parameters == nullptr ? 0.0 : profile_parameters->max_velocity(),
+ profile_parameters == nullptr ? 0.0
+ : profile_parameters->max_acceleration());
}
void ColumnProfiledSubsystem::AdjustProfile(double max_angular_velocity,
@@ -348,20 +351,46 @@
stuck_indexer_detector_->mutable_X_hat(5, 0) = 0.0;
}
-void ColumnProfiledSubsystem::PopulateIndexerStatus(IndexerStatus *status) {
- status->avg_angular_velocity = indexer_average_angular_velocity_;
+void ColumnProfiledSubsystem::PopulateIndexerStatus(
+ IndexerStatus::Builder *status_builder) {
+ status_builder->add_avg_angular_velocity(indexer_average_angular_velocity_);
- status->angular_velocity = X_hat_current_(1, 0);
- status->ready = indexer_ready_;
+ status_builder->add_angular_velocity(X_hat_current_(1, 0));
+ status_builder->add_ready(indexer_ready_);
- status->voltage_error = X_hat_current_(4, 0);
- status->stuck_voltage_error = stuck_indexer_X_hat_current_(4, 0);
- status->position_error = indexer_position_error_;
- status->instantaneous_velocity = indexer_dt_velocity_;
+ status_builder->add_voltage_error(X_hat_current_(4, 0));
+ status_builder->add_stuck_voltage_error(stuck_indexer_X_hat_current_(4, 0));
+ status_builder->add_position_error(indexer_position_error_);
+ status_builder->add_instantaneous_velocity(indexer_dt_velocity_);
- status->stuck = IsIndexerStuck();
+ status_builder->add_stuck(IsIndexerStuck());
- status->stuck_voltage = IndexerStuckVoltage();
+ status_builder->add_stuck_voltage(IndexerStuckVoltage());
+}
+
+void ColumnProfiledSubsystem::PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State> estimator_state_offset) {
+ status_builder->add_zeroed(zeroed());
+
+ status_builder->add_position(X_hat(2, 0));
+ status_builder->add_velocity(X_hat(3, 0));
+ status_builder->add_goal_position(goal(2, 0));
+ status_builder->add_goal_velocity(goal(3, 0));
+ status_builder->add_unprofiled_goal_position(unprofiled_goal(2, 0));
+ status_builder->add_unprofiled_goal_velocity(unprofiled_goal(3, 0));
+ status_builder->add_voltage_error(X_hat(5, 0));
+ status_builder->add_calculated_velocity(
+ (turret_position() - turret_last_position_) /
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
+
+ status_builder->add_estimator_state(estimator_state_offset);
+
+ Eigen::Matrix<double, 6, 1> error = controller().error();
+ status_builder->add_position_power(controller().controller().K(0, 0) *
+ error(0, 0));
+ status_builder->add_velocity_power(controller().controller().K(0, 1) *
+ error(1, 0));
}
Column::Column(::aos::EventLoop *event_loop)
@@ -384,15 +413,15 @@
freeze_ = false;
}
-void Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake) {
+std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal,
+ const ColumnPosition *position,
+ const vision::VisionStatus *vision_status,
+ double *indexer_output, double *turret_output,
+ flatbuffers::FlatBufferBuilder *fbb, intake::Intake *intake) {
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -526,11 +555,12 @@
}
if (unsafe_turret_goal && unsafe_indexer_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params);
+ profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params());
profiled_subsystem_.set_unprofiled_goal(
- unsafe_indexer_goal->angular_velocity, unsafe_turret_goal->angle);
+ unsafe_indexer_goal->angular_velocity,
+ unsafe_turret_goal->angle());
- if (unsafe_turret_goal->track) {
+ if (unsafe_turret_goal->track()) {
if (vision_time_adjuster_.valid()) {
AOS_LOG(INFO, "Vision aligning to %f\n",
vision_time_adjuster_.goal());
@@ -623,23 +653,40 @@
// Save debug/internal state.
// TODO(austin): Save more.
- turret_status->zeroed = profiled_subsystem_.zeroed();
- profiled_subsystem_.PopulateTurretStatus(turret_status);
- turret_status->estopped = (state_ == State::ESTOP);
- turret_status->state = static_cast<int32_t>(state_);
- turret_status->turret_encoder_angle = profiled_subsystem_.turret_position();
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ column_estimator_state_offset =
+ profiled_subsystem_.EstimatorState(fbb, 0);
+
+ TurretProfiledSubsystemStatus::Builder turret_status_builder(*fbb);
+ profiled_subsystem_.PopulateTurretStatus(&turret_status_builder,
+ column_estimator_state_offset);
+ turret_status_builder.add_estopped((state_ == State::ESTOP));
+ turret_status_builder.add_state(static_cast<int32_t>(state_));
+ turret_status_builder.add_turret_encoder_angle(
+ profiled_subsystem_.turret_position());
if (vision_time_adjuster_.valid()) {
- turret_status->vision_angle = vision_time_adjuster_.goal();
- turret_status->raw_vision_angle =
- vision_time_adjuster_.most_recent_vision_reading();
- turret_status->vision_tracking = true;
+ turret_status_builder.add_vision_angle(vision_time_adjuster_.goal());
+ turret_status_builder.add_raw_vision_angle(
+ vision_time_adjuster_.most_recent_vision_reading());
+ turret_status_builder.add_vision_tracking(true);
} else {
- turret_status->vision_angle = ::std::numeric_limits<double>::quiet_NaN();
- turret_status->vision_tracking = false;
+ turret_status_builder.add_vision_angle(
+ ::std::numeric_limits<double>::quiet_NaN());
+ turret_status_builder.add_vision_tracking(false);
}
- profiled_subsystem_.PopulateIndexerStatus(indexer_status);
- indexer_status->state = static_cast<int32_t>(indexer_state_);
+ flatbuffers::Offset<TurretProfiledSubsystemStatus> turret_status_offset =
+ turret_status_builder.Finish();
+
+ IndexerStatus::Builder indexer_status_builder(*fbb);
+ profiled_subsystem_.PopulateIndexerStatus(&indexer_status_builder);
+
+ indexer_status_builder.add_state(static_cast<int32_t>(indexer_state_));
+
+ flatbuffers::Offset<IndexerStatus> indexer_status_offset =
+ indexer_status_builder.Finish();
+
+ return std::make_pair(indexer_status_offset, turret_status_offset);
}
} // namespace column
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 0382059..615565f 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -14,9 +14,10 @@
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,8 +42,10 @@
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- template <class StatusType>
- void PopulateTurretStatus(StatusType *status);
+ void PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ estimator_state_offset);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal_velocity, double goal);
@@ -52,7 +55,7 @@
void set_turret_unprofiled_goal(double unprofiled_goal);
void set_unprofiled_goal(double goal_velocity, double unprofiled_goal);
// Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
@@ -86,7 +89,7 @@
double IndexerStuckVoltage() const;
void PartialIndexerReset();
void PartialTurretReset();
- void PopulateIndexerStatus(IndexerStatus *status);
+ void PopulateIndexerStatus(IndexerStatus::Builder *status_builder);
void AddOffset(double indexer_offset_delta, double turret_offset_delta);
@@ -132,31 +135,6 @@
double turret_last_position_ = 0;
};
-template <typename StatusType>
-void ColumnProfiledSubsystem::PopulateTurretStatus(StatusType *status) {
- status->zeroed = zeroed();
- status->state = -1;
- // We don't know, so default to the bad case.
- status->estopped = true;
-
- status->position = X_hat(2, 0);
- status->velocity = X_hat(3, 0);
- status->goal_position = goal(2, 0);
- status->goal_velocity = goal(3, 0);
- status->unprofiled_goal_position = unprofiled_goal(2, 0);
- status->unprofiled_goal_velocity = unprofiled_goal(3, 0);
- status->voltage_error = X_hat(5, 0);
- status->calculated_velocity =
- (turret_position() - turret_last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
-
- status->estimator_state = EstimatorState(0);
-
- Eigen::Matrix<double, 6, 1> error = controller().error();
- status->position_power = controller().controller().K(0, 0) * error(0, 0);
- status->velocity_power = controller().controller().K(0, 1) * error(1, 0);
-}
-
class Column {
public:
Column(::aos::EventLoop *event_loop);
@@ -179,15 +157,16 @@
static constexpr double kTurretMin = -0.1;
static constexpr double kTurretMax = M_PI / 2.0 + 0.1;
- void Iterate(::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake);
+ std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+ Iterate(::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal, const ColumnPosition *position,
+ const vision::VisionStatus *vision_status, double *indexer_output,
+ double *turret_output, flatbuffers::FlatBufferBuilder *fbb,
+ // IndexerStatus *indexer_status,
+ // TurretProfiledSubsystemStatus *turret_status,
+ intake::Intake *intake);
void Reset();
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.cc b/y2017/control_loops/superstructure/column/column_zeroing.cc
index 30500ca..7d5937a 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing.cc
@@ -31,15 +31,16 @@
}
void ColumnZeroingEstimator::UpdateEstimate(const ColumnPosition &position) {
- indexer_.UpdateEstimate(position.indexer);
- turret_.UpdateEstimate(position.turret);
+ indexer_.UpdateEstimate(*position.indexer());
+ turret_.UpdateEstimate(*position.turret());
if (indexer_.zeroed() && turret_.zeroed()) {
indexer_offset_ = indexer_.offset();
// Compute the current turret position.
- const double current_turret = indexer_offset_ + position.indexer.encoder +
- turret_.offset() + position.turret.encoder;
+ const double current_turret =
+ indexer_offset_ + position.indexer()->encoder() + turret_.offset() +
+ position.turret()->encoder();
// Now, we can compute the turret position which is closest to 0 radians
// (within +- M_PI).
@@ -47,16 +48,16 @@
::frc971::zeroing::Wrap(0.0, current_turret, M_PI * 2.0);
// Now, compute the actual turret offset.
- turret_offset_ = adjusted_turret - position.turret.encoder -
- (indexer_offset_ + position.indexer.encoder);
+ turret_offset_ = adjusted_turret - position.turret()->encoder() -
+ (indexer_offset_ + position.indexer()->encoder());
offset_ready_ = true;
// If we are close enough to 0, we are zeroed. Otherwise, we don't know
// which revolution we are on and need more info. We will always report the
// turret position as within +- M_PI from 0 with the provided offset.
- if (::std::abs(position.indexer.encoder + position.turret.encoder +
- indexer_offset_ + turret_offset_) <
- turret_zeroed_distance_) {
+ if (::std::abs(position.indexer()->encoder() +
+ position.turret()->encoder() + indexer_offset_ +
+ turret_offset_) < turret_zeroed_distance_) {
zeroed_ = true;
}
@@ -65,17 +66,23 @@
}
}
-ColumnZeroingEstimator::State ColumnZeroingEstimator::GetEstimatorState()
- const {
- State r;
- r.error = error();
- r.zeroed = zeroed();
- r.indexer = indexer_.GetEstimatorState();
- r.turret = turret_.GetEstimatorState();
- return r;
+flatbuffers::Offset<ColumnZeroingEstimator::State>
+ColumnZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> indexer_offset =
+ indexer_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> turret_offset =
+ turret_.GetEstimatorState(fbb);
+
+ State::Builder state_builder(*fbb);
+ state_builder.add_indexer(indexer_offset);
+ state_builder.add_turret(turret_offset);
+ state_builder.add_error(error());
+ state_builder.add_zeroed(zeroed());
+ return state_builder.Finish();
}
-} // column
-} // superstructure
+} // namespace column
+} // namespace superstructure
} // control_loops
} // y2017
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
index f2ac94f..8c2cbf3 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.h
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -2,10 +2,10 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
#include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,7 +41,8 @@
double turret_offset() const { return turret_offset_; }
// Returns information about our current state.
- State GetEstimatorState() const;
+ flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const;
private:
// We are ensuring that two subsystems are zeroed, so here they are!
diff --git a/y2017/control_loops/superstructure/column/column_zeroing_test.cc b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
index 3cc9c2e..dab5139 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing_test.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
@@ -3,10 +3,13 @@
#include <random>
#include "aos/die.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "frc971/zeroing/zeroing.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
@@ -49,14 +52,28 @@
}
void MoveTo(double indexer, double turret) {
- ColumnPosition column_position;
+ flatbuffers::FlatBufferBuilder fbb;
indexer_sensor_.MoveTo(indexer);
turret_sensor_.MoveTo(turret - indexer);
- indexer_sensor_.GetSensorValues(&column_position.indexer);
- turret_sensor_.GetSensorValues(&column_position.turret);
+ HallEffectAndPosition::Builder indexer_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> indexer_offset =
+ indexer_sensor_.GetSensorValues(&indexer_builder);
- column_zeroing_estimator_.UpdateEstimate(column_position);
+ HallEffectAndPosition::Builder turret_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> turret_offset =
+ turret_sensor_.GetSensorValues(&turret_builder);
+
+ ColumnPosition::Builder column_position_builder(fbb);
+ column_position_builder.add_indexer(indexer_offset);
+ column_position_builder.add_turret(turret_offset);
+ fbb.Finish(column_position_builder.Finish());
+
+
+ aos::FlatbufferDetachedBuffer<ColumnPosition> column_position(fbb.Release());
+ LOG(INFO) << "Position: " << aos::FlatbufferToJson(column_position);
+
+ column_zeroing_estimator_.UpdateEstimate(column_position.message());
}
};
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 9760eac..c3f6ef0 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -1,46 +1,48 @@
genrule(
- name = 'genrule_hood',
- cmd = '$(location //y2017/control_loops/python:hood) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:hood',
- ],
- outs = [
- 'hood_plant.h',
- 'hood_plant.cc',
- 'hood_integral_plant.h',
- 'hood_integral_plant.cc',
- ],
+ name = "genrule_hood",
+ outs = [
+ "hood_plant.h",
+ "hood_plant.cc",
+ "hood_integral_plant.h",
+ "hood_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:hood) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:hood",
+ ],
)
cc_library(
- name = 'hood_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'hood_plant.cc',
- 'hood_integral_plant.cc',
- ],
- hdrs = [
- 'hood_plant.h',
- 'hood_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "hood_plants",
+ srcs = [
+ "hood_integral_plant.cc",
+ "hood_plant.cc",
+ ],
+ hdrs = [
+ "hood_integral_plant.h",
+ "hood_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'hood',
- visibility = ['//visibility:public'],
- srcs = [
- 'hood.cc',
- ],
- hdrs = [
- 'hood.h',
- ],
- deps = [
- ':hood_plants',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "hood",
+ srcs = [
+ "hood.cc",
+ ],
+ hdrs = [
+ "hood.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":hood_plants",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
)
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index 8588a16..3dcd806 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -61,10 +61,12 @@
last_position_ = 0;
}
-void Hood::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::IndexPosition *position, double *output,
- ::frc971::control_loops::IndexProfiledJointStatus *status) {
+flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus>
+Hood::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const double *unsafe_goal,
+ const frc971::ProfileParameters *unsafe_goal_profile_parameters,
+ const ::frc971::IndexPosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -151,8 +153,8 @@
// If we have a goal, go to it. Otherwise stay where we are.
if (unsafe_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
- profiled_subsystem_.set_unprofiled_goal(unsafe_goal->angle);
+ profiled_subsystem_.AdjustProfile(unsafe_goal_profile_parameters);
+ profiled_subsystem_.set_unprofiled_goal(*unsafe_goal);
}
// ESTOP if we hit the hard limits.
@@ -198,12 +200,14 @@
}
// Save debug/internal state.
+ frc971::control_loops::IndexProfiledJointStatus::Builder status_builder =
+ profiled_subsystem_.BuildStatus<
+ frc971::control_loops::IndexProfiledJointStatus::Builder>(fbb);
// TODO(austin): Save more.
- status->zeroed = profiled_subsystem_.zeroed();
+ status_builder.add_estopped((state_ == State::ESTOP));
+ status_builder.add_state(static_cast<int32_t>(state_));
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ return status_builder.Finish();
}
} // namespace hood
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 55d5344..7778152 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -2,8 +2,8 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
#include "frc971/control_loops/profiled_subsystem.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2017 {
namespace control_loops {
@@ -45,10 +45,12 @@
::std::chrono::milliseconds(15);
static constexpr double kNotMovingVoltage = 2.0;
- void Iterate(::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::IndexPosition *position, double *output,
- ::frc971::control_loops::IndexProfiledJointStatus *status);
+ flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus> Iterate(
+ ::aos::monotonic_clock::time_point monotonic_now,
+ const double *unsafe_goal,
+ const frc971::ProfileParameters *unsafe_goal_profile_parameters,
+ const ::frc971::IndexPosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 164a63b..43f4927 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -1,46 +1,46 @@
genrule(
- name = 'genrule_intake',
- cmd = '$(location //y2017/control_loops/python:intake) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:intake',
- ],
- outs = [
- 'intake_plant.h',
- 'intake_plant.cc',
- 'intake_integral_plant.h',
- 'intake_integral_plant.cc',
- ],
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "intake_integral_plant.h",
+ "intake_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:intake) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:intake",
+ ],
)
cc_library(
- name = 'intake_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'intake_plant.cc',
- 'intake_integral_plant.cc',
- ],
- hdrs = [
- 'intake_plant.h',
- 'intake_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "intake_plants",
+ srcs = [
+ "intake_integral_plant.cc",
+ "intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_integral_plant.h",
+ "intake_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'intake',
- visibility = ['//visibility:public'],
- srcs = [
- 'intake.cc',
- ],
- hdrs = [
- 'intake.h',
- ],
- deps = [
- ':intake_plants',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "intake",
+ srcs = [
+ "intake.cc",
+ ],
+ hdrs = [
+ "intake.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":intake_plants",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ ],
)
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index baa4693..ee631d7 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -29,10 +29,11 @@
disable_count_ = 0;
}
-void Intake::Iterate(
- const control_loops::IntakeGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
+flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+Intake::Iterate(const IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position,
+ double *output, flatbuffers::FlatBufferBuilder *fbb) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -84,9 +85,9 @@
}
if (unsafe_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
- profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance);
- if (unsafe_goal->disable_intake) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params());
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance());
+ if (unsafe_goal->disable_intake()) {
if (disable_count_ > 100) {
disable = true;
}
@@ -133,13 +134,16 @@
*output = profiled_subsystem_.voltage();
}
- // Save debug/internal state.
- // TODO(austin): Save more.
- status->zeroed = profiled_subsystem_.zeroed();
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::Builder
+ status_builder = profiled_subsystem_.BuildStatus<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::
+ Builder>(fbb);
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ // Save debug/internal state.
+ status_builder.add_estopped((state_ == State::ESTOP));
+ status_builder.add_state(static_cast<int32_t>(state_));
+
+ return status_builder.Finish();
}
} // namespace intake
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index db1442a..cba3318 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -3,7 +3,7 @@
#include "frc971/control_loops/profiled_subsystem.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2017 {
namespace control_loops {
@@ -32,10 +32,11 @@
static constexpr double kZeroingVoltage = 2.5;
static constexpr double kOperatingVoltage = 12.0;
- void Iterate(const control_loops::IntakeGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
- *status);
+ flatbuffers::Offset<
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ Iterate(const IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2017/control_loops/superstructure/shooter/BUILD b/y2017/control_loops/superstructure/shooter/BUILD
index cb7fa54..d83e34b 100644
--- a/y2017/control_loops/superstructure/shooter/BUILD
+++ b/y2017/control_loops/superstructure/shooter/BUILD
@@ -1,49 +1,53 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
genrule(
- name = 'genrule_shooter',
- cmd = '$(location //y2017/control_loops/python:shooter) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:shooter',
- ],
- outs = [
- 'shooter_plant.h',
- 'shooter_plant.cc',
- 'shooter_integral_plant.h',
- 'shooter_integral_plant.cc',
- ],
+ name = "genrule_shooter",
+ outs = [
+ "shooter_plant.h",
+ "shooter_plant.cc",
+ "shooter_integral_plant.h",
+ "shooter_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:shooter) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:shooter",
+ ],
)
cc_library(
- name = 'shooter_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'shooter_plant.cc',
- 'shooter_integral_plant.cc',
- ],
- hdrs = [
- 'shooter_plant.h',
- 'shooter_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:hybrid_state_feedback_loop',
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "shooter_plants",
+ srcs = [
+ "shooter_integral_plant.cc",
+ "shooter_plant.cc",
+ ],
+ hdrs = [
+ "shooter_integral_plant.h",
+ "shooter_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'shooter',
- visibility = ['//visibility:public'],
- srcs = [
- 'shooter.cc',
- ],
- hdrs = [
- 'shooter.h',
- ],
- deps = [
- ':shooter_plants',
- '//aos/controls:control_loop',
- '//third_party/eigen',
- '//y2017/control_loops/superstructure:superstructure_queue',
- ],
+ name = "shooter",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":shooter_plants",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 2b668ae..0ea4836 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -2,11 +2,7 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
-#include "y2017/control_loops/superstructure/shooter/shooter.h"
namespace y2017 {
namespace control_loops {
@@ -104,31 +100,35 @@
loop_->Update(disabled, dt);
}
-void ShooterController::SetStatus(ShooterStatus *status) {
- status->avg_angular_velocity = average_angular_velocity_;
+flatbuffers::Offset<ShooterStatus> ShooterController::BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ ShooterStatus::Builder status_builder(*fbb);
+ status_builder.add_avg_angular_velocity(average_angular_velocity_);
- status->filtered_velocity = X_hat_current_(1, 0);
- status->angular_velocity = X_hat_current_(2, 0);
- status->ready = ready_;
+ status_builder.add_filtered_velocity(X_hat_current_(1, 0));
+ status_builder.add_angular_velocity(X_hat_current_(2, 0));
+ status_builder.add_ready(ready_);
- status->voltage_error = X_hat_current_(3, 0);
- status->position_error = position_error_;
- status->instantaneous_velocity = dt_velocity_;
- status->fixed_instantaneous_velocity = fixed_dt_velocity_;
+ status_builder.add_voltage_error(X_hat_current_(3, 0));
+ status_builder.add_position_error(position_error_);
+ status_builder.add_instantaneous_velocity(dt_velocity_);
+ status_builder.add_fixed_instantaneous_velocity(fixed_dt_velocity_);
+
+ return status_builder.Finish();
}
void Shooter::Reset() { wheel_.Reset(); }
-void Shooter::Iterate(const control_loops::ShooterGoal *goal,
- const double *position,
- ::aos::monotonic_clock::time_point position_time,
- double *output, control_loops::ShooterStatus *status) {
+flatbuffers::Offset<ShooterStatus> Shooter::Iterate(
+ const ShooterGoalT *goal, const double position,
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
if (goal) {
// Update position/goal for our wheel.
wheel_.set_goal(goal->angular_velocity);
}
- wheel_.set_position(*position);
+ wheel_.set_position(position);
chrono::nanoseconds dt = ::aos::controls::kLoopFrequency;
if (last_time_ != ::aos::monotonic_clock::min_time) {
@@ -138,20 +138,22 @@
wheel_.Update(output == nullptr, dt);
- wheel_.SetStatus(status);
+ flatbuffers::Offset<ShooterStatus> status_offset = wheel_.BuildStatus(fbb);
- if (last_ready_ && !status->ready) {
+ if (last_ready_ && !wheel_.ready()) {
min_ = wheel_.dt_velocity();
- } else if (!status->ready) {
+ } else if (!wheel_.ready()) {
min_ = ::std::min(min_, wheel_.dt_velocity());
- } else if (!last_ready_ && status->ready) {
+ } else if (!last_ready_ && wheel_.ready()) {
AOS_LOG(INFO, "Shot min was [%f]\n", min_);
}
if (output) {
*output = wheel_.voltage();
}
- last_ready_ = status->ready;
+ last_ready_ = wheel_.ready();
+
+ return status_offset;
}
} // namespace shooter
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index e8f7789..19fc7dd 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -7,10 +7,11 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "third_party/eigen/Eigen/Dense"
+#include "Eigen/Dense"
#include "y2017/control_loops/superstructure/shooter/shooter_integral_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace control_loops {
@@ -27,7 +28,8 @@
void set_position(double current_position);
// Populates the status structure.
- void SetStatus(control_loops::ShooterStatus *status);
+ flatbuffers::Offset<ShooterStatus> BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Returns the control loop calculated voltage.
double voltage() const;
@@ -46,6 +48,8 @@
// Resets the kalman filter and any other internal state.
void Reset();
+ bool ready() { return ready_; }
+
private:
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -86,9 +90,10 @@
// Iterates the shooter control loop one cycle. position and status must
// never be nullptr. goal can be nullptr if no goal exists, and output should
// be nullptr if disabled.
- void Iterate(const control_loops::ShooterGoal *goal, const double *position,
- ::aos::monotonic_clock::time_point position_time, double *output,
- control_loops::ShooterStatus *status);
+ flatbuffers::Offset<ShooterStatus> Iterate(
+ const ShooterGoalT *goal, const double position,
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
// Sets the shooter up to reset the kalman filter next time Iterate is called.
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 3ad1747..29886e2 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -1,14 +1,13 @@
#include "y2017/control_loops/superstructure/superstructure.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -24,19 +23,18 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
vision_status_fetcher_(
- event_loop->MakeFetcher<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
+ event_loop->MakeFetcher<::y2017::vision::VisionStatus>("/vision")),
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")),
column_(event_loop) {
shot_interpolation_table_ =
::frc971::shooter_interpolation::InterpolationTable<ShotParams>({
- // { distance_to_target, { shot_angle, shot_power, indexer_velocity }},
+ // { distance_to_target, { shot_angle, shot_power, indexer_velocity
+ // }},
{1.21, {0.29, 301.0, -1.0 * M_PI}}, // table entry
{1.55, {0.305, 316.0, -1.1 * M_PI}}, // table entry
{1.82, {0.33, 325.0, -1.3 * M_PI}}, // table entry
@@ -51,10 +49,11 @@
}
void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
const ::aos::monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -71,41 +70,43 @@
}
// Create a copy of the goals so that we can modify them.
- HoodGoal hood_goal;
- ShooterGoal shooter_goal;
- IndexerGoal indexer_goal;
+ double hood_goal_angle = 0.0;
+ ShooterGoalT shooter_goal;
+ IndexerGoalT indexer_goal;
bool in_range = true;
+ double vision_distance = 0.0;
if (unsafe_goal != nullptr) {
- hood_goal = unsafe_goal->hood;
- shooter_goal = unsafe_goal->shooter;
- indexer_goal = unsafe_goal->indexer;
+ hood_goal_angle = unsafe_goal->hood()->angle();
+ shooter_goal.angular_velocity = unsafe_goal->shooter()->angular_velocity();
+ indexer_goal.angular_velocity = unsafe_goal->indexer()->angular_velocity();
+ indexer_goal.voltage_rollers = unsafe_goal->indexer()->voltage_rollers();
- if (!unsafe_goal->use_vision_for_shots) {
+ if (!unsafe_goal->use_vision_for_shots()) {
distance_average_.Reset();
}
distance_average_.Tick(monotonic_now, vision_status);
- status->vision_distance = distance_average_.Get();
+ vision_distance = distance_average_.Get();
// If we are moving too fast, disable shooting and clear the accumulator.
double robot_velocity = 0.0;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- robot_velocity = drivetrain_status_fetcher_->robot_speed;
+ robot_velocity = drivetrain_status_fetcher_->robot_speed();
}
if (::std::abs(robot_velocity) > 0.2) {
- if (unsafe_goal->use_vision_for_shots) {
+ if (unsafe_goal->use_vision_for_shots()) {
AOS_LOG(INFO, "Moving too fast, resetting\n");
}
distance_average_.Reset();
}
if (distance_average_.Valid()) {
- if (unsafe_goal->use_vision_for_shots) {
+ if (unsafe_goal->use_vision_for_shots()) {
ShotParams shot_params;
if (shot_interpolation_table_.GetInRange(
distance_average_.Get(), &shot_params)) {
- hood_goal.angle = shot_params.angle;
+ hood_goal_angle = shot_params.angle;
shooter_goal.angular_velocity = shot_params.power;
if (indexer_goal.angular_velocity != 0.0) {
indexer_goal.angular_velocity = shot_params.indexer_velocity;
@@ -116,25 +117,30 @@
}
AOS_LOG(
DEBUG, "VisionDistance %f, hood %f shooter %f, indexer %f * M_PI\n",
- status->vision_distance, hood_goal.angle,
+ vision_distance, hood_goal_angle,
shooter_goal.angular_velocity, indexer_goal.angular_velocity / M_PI);
} else {
- AOS_LOG(DEBUG, "VisionNotValid %f\n", status->vision_distance);
- if (unsafe_goal->use_vision_for_shots) {
+ AOS_LOG(DEBUG, "VisionNotValid %f\n", vision_distance);
+ if (unsafe_goal->use_vision_for_shots()) {
in_range = false;
indexer_goal.angular_velocity = 0.0;
}
}
}
- hood_.Iterate(monotonic_now, unsafe_goal != nullptr ? &hood_goal : nullptr,
- &(position->hood),
- output != nullptr ? &(output->voltage_hood) : nullptr,
- &(status->hood));
- shooter_.Iterate(unsafe_goal != nullptr ? &shooter_goal : nullptr,
- &(position->theta_shooter), position->sent_time,
- output != nullptr ? &(output->voltage_shooter) : nullptr,
- &(status->shooter));
+ flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus>
+ hood_offset = hood_.Iterate(
+ monotonic_now, unsafe_goal != nullptr ? &hood_goal_angle : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->hood()->profile_params()
+ : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.voltage_hood) : nullptr,
+ status->fbb());
+ flatbuffers::Offset<ShooterStatus> shooter_offset = shooter_.Iterate(
+ unsafe_goal != nullptr ? &shooter_goal : nullptr,
+ position->theta_shooter(), position_context().monotonic_sent_time,
+ output != nullptr ? &(output_struct.voltage_shooter) : nullptr,
+ status->fbb());
// Implement collision avoidance by passing down a freeze or range restricting
// signal to the column and intake objects.
@@ -145,8 +151,8 @@
// The turret is in a position (or wants to be in a position) where we
// need the intake out. Push it out.
const bool column_goal_not_safe =
- unsafe_goal->turret.angle > column::Column::kTurretMax ||
- unsafe_goal->turret.angle < column::Column::kTurretMin;
+ unsafe_goal->turret()->angle() > column::Column::kTurretMax ||
+ unsafe_goal->turret()->angle() < column::Column::kTurretMin;
const bool column_position_not_safe =
column_.turret_position() > column::Column::kTurretMax ||
column_.turret_position() < column::Column::kTurretMin;
@@ -178,55 +184,87 @@
AOS_LOG(ERROR, "Collisions ignored\n");
}
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake),
- output != nullptr ? &(output->voltage_intake) : nullptr,
- &(status->intake));
+ flatbuffers::Offset<
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_offset = intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake(),
+ output != nullptr ? &(output_struct.voltage_intake) : nullptr,
+ status->fbb());
- column_.Iterate(monotonic_now,
- unsafe_goal != nullptr ? &indexer_goal : nullptr,
- unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
- &(position->column), vision_status,
- output != nullptr ? &(output->voltage_indexer) : nullptr,
- output != nullptr ? &(output->voltage_turret) : nullptr,
- &(status->indexer), &(status->turret), &intake_);
+ std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+ indexer_and_turret_offsets = column_.Iterate(
+ monotonic_now, unsafe_goal != nullptr ? &indexer_goal : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+ position->column(), vision_status,
+ output != nullptr ? &(output_struct.voltage_indexer) : nullptr,
+ output != nullptr ? &(output_struct.voltage_turret) : nullptr,
+ status->fbb(), &intake_);
- status->estopped =
- status->intake.estopped | status->hood.estopped | status->turret.estopped;
+ const frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *temp_intake_status = GetTemporaryPointer(*status->fbb(), intake_offset);
+ const frc971::control_loops::IndexProfiledJointStatus *temp_hood_status =
+ GetTemporaryPointer(*status->fbb(), hood_offset);
+ const TurretProfiledSubsystemStatus *temp_turret_status =
+ GetTemporaryPointer(*status->fbb(), indexer_and_turret_offsets.second);
- status->zeroed =
- status->intake.zeroed && status->hood.zeroed && status->turret.zeroed;
+ const bool estopped = temp_intake_status->estopped() ||
+ temp_hood_status->estopped() ||
+ temp_turret_status->estopped();
+ const bool zeroed = temp_intake_status->zeroed() &&
+ temp_hood_status->zeroed() &&
+ temp_turret_status->zeroed();
+ const bool turret_vision_tracking = temp_turret_status->vision_tracking();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_intake(intake_offset);
+ status_builder.add_hood(hood_offset);
+ status_builder.add_shooter(shooter_offset);
+ status_builder.add_turret(indexer_and_turret_offsets.second);
+ status_builder.add_indexer(indexer_and_turret_offsets.first);
+
+ status_builder.add_estopped(estopped);
+ status_builder.add_zeroed(zeroed);
+
+ status_builder.add_vision_distance(vision_distance);
if (output && unsafe_goal) {
- output->gear_servo =
- ::std::min(1.0, ::std::max(0.0, unsafe_goal->intake.gear_servo));
+ output_struct.gear_servo =
+ ::std::min(1.0, ::std::max(0.0, unsafe_goal->intake()->gear_servo()));
- output->voltage_intake_rollers =
+ output_struct.voltage_intake_rollers =
::std::max(-kMaxIntakeRollerVoltage,
- ::std::min(unsafe_goal->intake.voltage_rollers,
+ ::std::min(unsafe_goal->intake()->voltage_rollers(),
kMaxIntakeRollerVoltage));
- output->voltage_indexer_rollers =
+ output_struct.voltage_indexer_rollers =
::std::max(-kMaxIndexerRollerVoltage,
- ::std::min(unsafe_goal->indexer.voltage_rollers,
+ ::std::min(unsafe_goal->indexer()->voltage_rollers(),
kMaxIndexerRollerVoltage));
// Set the lights on or off
- output->lights_on = unsafe_goal->lights_on;
+ output_struct.lights_on = unsafe_goal->lights_on();
- if (status->estopped) {
- output->red_light_on = true;
- output->green_light_on = false;
- output->blue_light_on = false;
- } else if (!status->zeroed) {
- output->red_light_on = false;
- output->green_light_on = false;
- output->blue_light_on = true;
- } else if (status->turret.vision_tracking && in_range) {
- output->red_light_on = false;
- output->green_light_on = true;
- output->blue_light_on = false;
+ if (estopped) {
+ output_struct.red_light_on = true;
+ output_struct.green_light_on = false;
+ output_struct.blue_light_on = false;
+ } else if (!zeroed) {
+ output_struct.red_light_on = false;
+ output_struct.green_light_on = false;
+ output_struct.blue_light_on = true;
+ } else if (turret_vision_tracking && in_range) {
+ output_struct.red_light_on = false;
+ output_struct.green_light_on = true;
+ output_struct.blue_light_on = false;
}
}
+
+ if (output) {
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
+
+ status->Send(status_builder.Finish());
}
} // namespace superstructure
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 660b861..a4c824e 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -4,13 +4,16 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_distance_average.h"
namespace y2017 {
@@ -18,11 +21,10 @@
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2017.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
const hood::Hood &hood() const { return hood_; }
const intake::Intake &intake() const { return intake_; }
@@ -35,15 +37,13 @@
}
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::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:
::aos::Fetcher<::y2017::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
hood::Hood hood_;
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 6ea2fa2..0000000
--- a/y2017/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,258 +0,0 @@
-package y2017.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/profiled_subsystem.q";
-// TODO(austin): Add this back in when the queue compiler supports diamond
-// inheritance.
-//import "frc971/control_loops/control_loops.q";
-
-struct IntakeGoal {
- // Zero for the intake is when the front tube is tangent with the front of the
- // frame. Positive is out.
-
- // Goal distance of the intake.
- double distance;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-
- // Voltage to send to the rollers. Positive is sucking in.
- double voltage_rollers;
-
- // If true, disable the intake so we can hang.
- bool disable_intake;
-
- // The gear servo value.
- double gear_servo;
-};
-
-struct IndexerGoal {
- // Indexer angular velocity goals in radians/second.
- double angular_velocity;
-
- // Roller voltage. Positive is sucking in.
- double voltage_rollers;
-};
-
-struct TurretGoal {
- // An angle of zero means the turrent faces toward the front of the
- // robot where the intake is located. The angle increases when the turret
- // turns clockwise (towards right from the front), and decreases when
- // the turrent turns counter-clockwise (towards left from the front).
- // These are from a top view above the robot.
- double angle;
-
- // If true, ignore the angle and track using vision. If we don't see
- // anything, we'll use the turret goal above.
- bool track;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-};
-
-struct HoodGoal {
- // Angle the hood is currently at. An angle of zero is at the lower hard
- // stop, angle increases as hood rises.
- double angle;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-};
-
-struct ShooterGoal {
- // Angular velocity goals in radians/second. Positive is shooting out of the
- // robot.
- double angular_velocity;
-};
-
-struct IndexerStatus {
- // The current average velocity in radians/second. Positive is moving balls up
- // towards the shooter. This is the angular velocity of the inner piece.
- double avg_angular_velocity;
-
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-
- // True if the indexer is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // True if the indexer is stuck.
- bool stuck;
- float stuck_voltage;
-
- // The state of the indexer state machine.
- int32_t state;
-
- // The estimated voltage error from the kalman filter in volts.
- double voltage_error;
- // The estimated voltage error from the stuck indexer kalman filter.
- double stuck_voltage_error;
-
- // The current velocity measured as delta x / delta t in radians/sec.
- double instantaneous_velocity;
-
- // The error between our measurement and expected measurement in radians.
- double position_error;
-};
-
-struct ShooterStatus {
- // The current average velocity in radians/second.
- double avg_angular_velocity;
-
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-
- // True if the shooter is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // The estimated voltage error from the kalman filter in volts.
- double voltage_error;
-
- // The current velocity measured as delta x / delta t in radians/sec.
- double instantaneous_velocity;
- double filtered_velocity;
- double fixed_instantaneous_velocity;
-
- // The error between our measurement and expected measurement in radians.
- double position_error;
-};
-
-struct ColumnPosition {
- // Indexer angle in radians relative to the base. Positive is according to
- // the right hand rule around +z.
- .frc971.HallEffectAndPosition indexer;
- // Turret angle in radians relative to the indexer. Positive is the same as
- // the indexer.
- .frc971.HallEffectAndPosition turret;
-};
-
-struct ColumnEstimatorState {
- bool error;
- bool zeroed;
- .frc971.HallEffectAndPositionEstimatorState indexer;
- .frc971.HallEffectAndPositionEstimatorState turret;
-};
-
-struct TurretProfiledSubsystemStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- ColumnEstimatorState estimator_state;
-
- double raw_vision_angle;
- double vision_angle;
- bool vision_tracking;
-
- double turret_encoder_angle;
-};
-
-// Published on ".y2017.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
- IndexerGoal indexer;
- TurretGoal turret;
- HoodGoal hood;
- ShooterGoal shooter;
- bool lights_on;
- bool use_vision_for_shots;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted. This is the or of all subsystem estops.
- bool estopped;
-
- // Each subsystems status.
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
- .frc971.control_loops.IndexProfiledJointStatus hood;
- ShooterStatus shooter;
-
- TurretProfiledSubsystemStatus turret;
- IndexerStatus indexer;
-
- float vision_distance;
- };
-
- message Position {
- // Position of the intake, zero when the intake is in, positive when it is
- // out.
- .frc971.PotAndAbsolutePosition intake;
-
- // The position of the column.
- ColumnPosition column;
-
- // The sensor readings for the hood. The units and sign are defined the
- // same as what's in the Goal message.
- .frc971.IndexPosition hood;
-
- // Shooter wheel angle in radians.
- double theta_shooter;
- };
-
- message Output {
- // Voltages for some of the subsystems.
- double voltage_intake;
- double voltage_indexer;
- double voltage_shooter;
-
- // Rollers on the intake.
- double voltage_intake_rollers;
- // Roller on the indexer
- double voltage_indexer_rollers;
-
- double voltage_turret;
- double voltage_hood;
-
- double gear_servo;
-
- // If true, the lights are on.
- bool lights_on;
-
- bool red_light_on;
- bool green_light_on;
- bool blue_light_on;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2017/control_loops/superstructure/superstructure_goal.fbs b/y2017/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..6681262
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,74 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table IntakeGoal {
+ // Zero for the intake is when the front tube is tangent with the front of the
+ // frame. Positive is out.
+
+ // Goal distance of the intake.
+ distance:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_rollers:double;
+
+ // If true, disable the intake so we can hang.
+ disable_intake:bool;
+
+ // The gear servo value.
+ gear_servo:double;
+}
+
+table IndexerGoal {
+ // Indexer angular velocity goals in radians/second.
+ angular_velocity:double;
+
+ // Roller voltage. Positive is sucking in.
+ voltage_rollers:double;
+}
+
+table TurretGoal {
+ // An angle of zero means the turrent faces toward the front of the
+ // robot where the intake is located. The angle increases when the turret
+ // turns clockwise (towards right from the front), and decreases when
+ // the turrent turns counter-clockwise (towards left from the front).
+ // These are from a top view above the robot.
+ angle:double;
+
+ // If true, ignore the angle and track using vision. If we don't see
+ // anything, we'll use the turret goal above.
+ track:bool;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+}
+
+table HoodGoal {
+ // Angle the hood is currently at. An angle of zero is at the lower hard
+ // stop, angle increases as hood rises.
+ angle:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+}
+
+table ShooterGoal {
+ // Angular velocity goals in radians/second. Positive is shooting out of the
+ // robot.
+ angular_velocity:double;
+}
+
+table Goal {
+ intake:IntakeGoal;
+ indexer:IndexerGoal;
+ turret:TurretGoal;
+ hood:HoodGoal;
+ shooter:ShooterGoal;
+ lights_on:bool;
+ use_vision_for_shots:bool;
+}
+
+root_type Goal;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 1e763b8..5298687 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,7 +6,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -121,14 +120,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2017.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
hood_plant_(new HoodPlant(
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
hood_encoder_(constants::Values::kHoodEncoderIndexDifference),
@@ -216,17 +212,42 @@
// 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();
- hood_encoder_.GetSensorValues(&position->hood);
- intake_pot_encoder_.GetSensorValues(&position->intake);
- position->theta_shooter = shooter_plant_->Y(0, 0);
+ frc971::IndexPosition::Builder hood_builder =
+ builder.MakeBuilder<frc971::IndexPosition>();
+ flatbuffers::Offset<frc971::IndexPosition> hood_offset =
+ hood_encoder_.GetSensorValues(&hood_builder);
- turret_encoder_.GetSensorValues(&position->column.turret);
- indexer_encoder_.GetSensorValues(&position->column.indexer);
+ frc971::PotAndAbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset =
+ intake_pot_encoder_.GetSensorValues(&intake_builder);
- position.Send();
+ frc971::HallEffectAndPosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::HallEffectAndPosition>();
+ flatbuffers::Offset<frc971::HallEffectAndPosition> turret_offset =
+ turret_encoder_.GetSensorValues(&turret_builder);
+
+ frc971::HallEffectAndPosition::Builder indexer_builder =
+ builder.MakeBuilder<frc971::HallEffectAndPosition>();
+ flatbuffers::Offset<frc971::HallEffectAndPosition> indexer_offset =
+ indexer_encoder_.GetSensorValues(&indexer_builder);
+
+ ColumnPosition::Builder column_builder =
+ builder.MakeBuilder<ColumnPosition>();
+ column_builder.add_indexer(indexer_offset);
+ column_builder.add_turret(turret_offset);
+ flatbuffers::Offset<ColumnPosition> column_offset = column_builder.Finish();
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_theta_shooter(shooter_plant_->Y(0, 0));
+ position_builder.add_column(column_offset);
+ position_builder.add_intake(intake_offset);
+ position_builder.add_hood(hood_offset);
+
+ builder.Send(position_builder.Finish());
}
double hood_position() const { return hood_plant_->X(0, 0); }
@@ -282,61 +303,61 @@
const double voltage_check_hood =
(static_cast<hood::Hood::State>(
- superstructure_status_fetcher_->hood.state) ==
+ superstructure_status_fetcher_->hood()->state()) ==
hood::Hood::State::RUNNING)
? superstructure::hood::Hood::kOperatingVoltage
: superstructure::hood::Hood::kZeroingVoltage;
const double voltage_check_indexer =
(static_cast<column::Column::State>(
- superstructure_status_fetcher_->turret.state) ==
+ superstructure_status_fetcher_->turret()->state()) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_turret =
(static_cast<column::Column::State>(
- superstructure_status_fetcher_->turret.state) ==
+ superstructure_status_fetcher_->turret()->state()) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_intake =
(static_cast<intake::Intake::State>(
- superstructure_status_fetcher_->intake.state) ==
+ superstructure_status_fetcher_->intake()->state()) ==
intake::Intake::State::RUNNING)
? superstructure::intake::Intake::kOperatingVoltage
: superstructure::intake::Intake::kZeroingVoltage;
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_hood),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_hood()),
voltage_check_hood);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
voltage_check_intake);
- EXPECT_LE(::std::abs(superstructure_output_fetcher_->voltage_indexer),
+ EXPECT_LE(::std::abs(superstructure_output_fetcher_->voltage_indexer()),
voltage_check_indexer)
<< ": check voltage " << voltage_check_indexer;
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_turret),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_turret()),
voltage_check_turret);
::Eigen::Matrix<double, 1, 1> hood_U;
- hood_U << superstructure_output_fetcher_->voltage_hood +
+ hood_U << superstructure_output_fetcher_->voltage_hood() +
hood_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> shooter_U;
- shooter_U << superstructure_output_fetcher_->voltage_shooter +
+ shooter_U << superstructure_output_fetcher_->voltage_shooter() +
shooter_plant_->voltage_offset();
::Eigen::Matrix<double, 2, 1> column_U;
- column_U << superstructure_output_fetcher_->voltage_indexer +
+ column_U << superstructure_output_fetcher_->voltage_indexer() +
column_plant_->indexer_voltage_offset(),
- superstructure_output_fetcher_->voltage_turret +
+ superstructure_output_fetcher_->voltage_turret() +
column_plant_->turret_voltage_offset();
hood_plant_->Update(hood_U);
@@ -465,9 +486,9 @@
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
::std::unique_ptr<HoodPlant> hood_plant_;
PositionSensorSimulator hood_encoder_;
@@ -499,23 +520,20 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2017/config.json"),
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2017.control_loops.superstructure_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")),
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -530,50 +548,49 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
- superstructure_status_fetcher_->hood.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->angle(),
+ superstructure_status_fetcher_->hood()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->angle(),
superstructure_plant_.hood_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
- superstructure_status_fetcher_->turret.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->angle(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->angle(),
superstructure_plant_.turret_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
- superstructure_status_fetcher_->intake.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->distance(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->distance(),
superstructure_plant_.intake_position(), 0.001);
// Check that the angular velocity, average angular velocity, and estimated
// angular velocity match when we are done for the shooter.
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
- superstructure_status_fetcher_->shooter.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
- superstructure_status_fetcher_->shooter.avg_angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
+ superstructure_status_fetcher_->shooter()->angular_velocity(), 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
+ superstructure_status_fetcher_->shooter()->avg_angular_velocity(),
0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
superstructure_plant_.shooter_velocity(), 0.1);
// Check that the angular velocity, average angular velocity, and estimated
// angular velocity match when we are done for the indexer.
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
- superstructure_status_fetcher_->indexer.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
- superstructure_status_fetcher_->indexer.avg_angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
+ superstructure_status_fetcher_->indexer()->angular_velocity(), 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
+ superstructure_status_fetcher_->indexer()->avg_angular_velocity(),
0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
superstructure_plant_.indexer_velocity(), 0.1);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Position>
- superstructure_position_fetcher_;
+ ::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_;
@@ -588,11 +605,36 @@
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 0.2;
- goal->turret.angle = 0.0;
- goal->intake.distance = 0.05;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(0.2);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(0.05);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -607,20 +649,48 @@
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 0.1;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 0.1;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(0.1);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = 0.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.1);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(0.1);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -638,11 +708,36 @@
// Zero it before we move.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->hood.angle = constants::Values::kHoodRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -652,17 +747,48 @@
// 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->intake.distance = constants::Values::kIntakeRange.lower;
- goal->intake.profile_params.max_velocity = 20.0;
- goal->intake.profile_params.max_acceleration = 0.1;
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->turret.profile_params.max_velocity = 20.0;
- goal->turret.profile_params.max_acceleration = 1.0;
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->hood.profile_params.max_velocity = 20.0;
- goal->hood.profile_params.max_acceleration = 1.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 1.0);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 1.0);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 0.1);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(23.0);
superstructure_plant_.set_peak_turret_velocity(23.0);
@@ -676,17 +802,48 @@
// Now do a high acceleration move with a low velocity limit.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->intake.profile_params.max_velocity = 0.1;
- goal->intake.profile_params.max_acceleration = 100;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 100;
- goal->hood.angle = constants::Values::kHoodRange.upper;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 100;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 100.0);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 100.0);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 0.1, 100.0);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(0.2);
@@ -707,50 +864,106 @@
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 100.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = 100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(100.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.upper,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.upper,
- superstructure_status_fetcher_->turret.position, 0.001);
+ superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = -100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = -100.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(-100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = -100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(-100.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(-100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -758,31 +971,59 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.lower,
- superstructure_status_fetcher_->turret.position, 0.001);
+ superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(column::Column::kIntakeZeroingMinDistance,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
// Now, center the turret so we can try ridiculous things without having the
// intake pushed out.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = -100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 0.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(-100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = -100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(-100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -790,12 +1031,12 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->turret.position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
}
// Tests that the hood, turret and intake loops zeroes when run for a while.
@@ -803,19 +1044,48 @@
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -844,11 +1114,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -867,11 +1162,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.upper;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -889,12 +1209,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.upper - 0.1;
- goal->turret.angle = constants::Values::kTurretRange.upper - 0.1;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.1;
- goal->indexer.angular_velocity = -5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper - 0.1);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper - 0.1);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.1);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(-5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -923,15 +1267,42 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower + 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -955,11 +1326,36 @@
constants::GetValues().hood.zeroing.measured_index_position - 0.001);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->turret.angle = 0.0;
- goal->intake.distance = constants::Values::kIntakeRange.lower;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -1000,49 +1396,118 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 300.0;
- goal->indexer.angular_velocity = 20.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(300.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(20.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->shooter.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->shooter()->ready());
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 0.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Make sure we don't apply voltage on spin-down.
RunFor(dt());
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter());
// Continue to stop.
RunFor(chrono::seconds(5));
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter());
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(SuperstructureTest, ShooterDisabled) {
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 200.0;
- goal->indexer.angular_velocity = 20.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(200.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(20.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
EXPECT_EQ(nullptr, superstructure_output_fetcher_.get());
@@ -1059,18 +1524,41 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer()->ready());
// Now, stick it.
const auto stuck_start_time = monotonic_now();
@@ -1080,7 +1568,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
@@ -1095,7 +1583,7 @@
superstructure_position_fetcher_.Fetch();
ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double indexer_position =
- superstructure_position_fetcher_->column.indexer.encoder;
+ superstructure_position_fetcher_->column()->indexer()->encoder();
// Now, unstick it.
superstructure_plant_.set_freeze_indexer(false);
@@ -1105,7 +1593,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1123,7 +1611,7 @@
superstructure_position_fetcher_.Fetch();
ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double unstuck_indexer_position =
- superstructure_position_fetcher_->column.indexer.encoder;
+ superstructure_position_fetcher_->column()->indexer()->encoder();
EXPECT_LT(unstuck_indexer_position, indexer_position - 0.1);
// Now, verify that everything works as expected.
@@ -1138,18 +1626,41 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer()->ready());
// Now, stick it.
const auto stuck_start_time = monotonic_now();
@@ -1159,7 +1670,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
@@ -1177,7 +1688,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1202,7 +1713,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
diff --git a/y2017/control_loops/superstructure/superstructure_main.cc b/y2017/control_loops/superstructure/superstructure_main.cc
index 1dbb342..eedba6f 100644
--- a/y2017/control_loops/superstructure/superstructure_main.cc
+++ b/y2017/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2017/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() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2017::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2017/control_loops/superstructure/superstructure_output.fbs b/y2017/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..5301380
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,27 @@
+namespace y2017.control_loops.superstructure;
+
+table Output {
+ // Voltages for some of the subsystems.
+ voltage_intake:double;
+ voltage_indexer:double;
+ voltage_shooter:double;
+
+ // Rollers on the intake.
+ voltage_intake_rollers:double;
+ // Roller on the indexer
+ voltage_indexer_rollers:double;
+
+ voltage_turret:double;
+ voltage_hood:double;
+
+ gear_servo:double;
+
+ // If true, the lights are on.
+ lights_on:bool;
+
+ red_light_on:bool;
+ green_light_on:bool;
+ blue_light_on:bool;
+}
+
+root_type Output;
diff --git a/y2017/control_loops/superstructure/superstructure_position.fbs b/y2017/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..7f95462
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,31 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table ColumnPosition {
+ // Indexer angle in radians relative to the base. Positive is according to
+ // the right hand rule around +z.
+ indexer:frc971.HallEffectAndPosition;
+ // Turret angle in radians relative to the indexer. Positive is the same as
+ // the indexer.
+ turret:frc971.HallEffectAndPosition;
+}
+
+
+table Position {
+ // Position of the intake, zero when the intake is in, positive when it is
+ // out.
+ intake:frc971.PotAndAbsolutePosition;
+
+ // The position of the column.
+ column:ColumnPosition;
+
+ // The sensor readings for the hood. The units and sign are defined the
+ // same as what's in the Goal message.
+ hood:frc971.IndexPosition;
+
+ // Shooter wheel angle in radians.
+ theta_shooter:double;
+}
+
+root_type Position;
diff --git a/y2017/control_loops/superstructure/superstructure_status.fbs b/y2017/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..217eb5b
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,129 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table IndexerStatus {
+ // The current average velocity in radians/second. Positive is moving balls up
+ // towards the shooter. This is the angular velocity of the inner piece.
+ avg_angular_velocity:double;
+
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+
+ // True if the indexer is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // True if the indexer is stuck.
+ stuck:bool;
+ stuck_voltage:float;
+
+ // The state of the indexer state machine.
+ state:int;
+
+ // The estimated voltage error from the kalman filter in volts.
+ voltage_error:double;
+ // The estimated voltage error from the stuck indexer kalman filter.
+ stuck_voltage_error:double;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ instantaneous_velocity:double;
+
+ // The error between our measurement and expected measurement in radians.
+ position_error:double;
+}
+
+table ShooterStatus {
+ // The current average velocity in radians/second.
+ avg_angular_velocity:double;
+
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // The estimated voltage error from the kalman filter in volts.
+ voltage_error:double;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ instantaneous_velocity:double;
+ filtered_velocity:double;
+ fixed_instantaneous_velocity:double;
+
+ // The error between our measurement and expected measurement in radians.
+ position_error:double;
+}
+
+table ColumnEstimatorState {
+ error:bool;
+ zeroed:bool;
+ indexer:frc971.HallEffectAndPositionEstimatorState;
+ turret:frc971.HallEffectAndPositionEstimatorState;
+}
+
+table TurretProfiledSubsystemStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:ColumnEstimatorState;
+
+ raw_vision_angle:double;
+ vision_angle:double;
+ vision_tracking:bool;
+
+ turret_encoder_angle:double;
+}
+
+table Status {
+ // Are all the subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+ // Each subsystems status.
+ intake:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ hood:frc971.control_loops.IndexProfiledJointStatus;
+ shooter:ShooterStatus;
+
+ turret:TurretProfiledSubsystemStatus;
+ indexer:IndexerStatus;
+
+ vision_distance:float;
+}
+
+root_type Status;
diff --git a/y2017/control_loops/superstructure/vision_distance_average.h b/y2017/control_loops/superstructure/vision_distance_average.h
index 74e775d..7af2a9b 100644
--- a/y2017/control_loops/superstructure/vision_distance_average.h
+++ b/y2017/control_loops/superstructure/vision_distance_average.h
@@ -6,7 +6,7 @@
#include "aos/containers/ring_buffer.h"
#include "aos/time/time.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -28,8 +28,8 @@
cached_value_ = ComputeValue();
}
- if (vision_status != nullptr && vision_status->image_valid) {
- data_.Push({monotonic_now, vision_status->distance});
+ if (vision_status != nullptr && vision_status->image_valid()) {
+ data_.Push({monotonic_now, vision_status->distance()});
cached_value_ = ComputeValue();
}
}
diff --git a/y2017/control_loops/superstructure/vision_distance_average_test.cc b/y2017/control_loops/superstructure/vision_distance_average_test.cc
index 70f2f64..bc0fc9a 100644
--- a/y2017/control_loops/superstructure/vision_distance_average_test.cc
+++ b/y2017/control_loops/superstructure/vision_distance_average_test.cc
@@ -1,5 +1,6 @@
#include "y2017/control_loops/superstructure/vision_distance_average.h"
+#include "aos/flatbuffers.h"
#include "gtest/gtest.h"
namespace y2017 {
@@ -18,18 +19,28 @@
}
void TickInvalid() {
- vision::VisionStatus status;
- status.image_valid = false;
+ flatbuffers::FlatBufferBuilder fbb;
- average_.Tick(tick_time(), &status);
+ vision::VisionStatus::Builder status_builder(fbb);
+ status_builder.add_image_valid(false);
+ fbb.Finish(status_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<vision::VisionStatus> status(fbb.Release());
+
+ average_.Tick(tick_time(), &status.message());
}
void TickValid(double distance) {
- vision::VisionStatus status;
- status.image_valid = true;
- status.distance = distance;
+ flatbuffers::FlatBufferBuilder fbb;
- average_.Tick(tick_time(), &status);
+ vision::VisionStatus::Builder status_builder(fbb);
+ status_builder.add_image_valid(true);
+ status_builder.add_distance(distance);
+ fbb.Finish(status_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<vision::VisionStatus> status(fbb.Release());
+
+ average_.Tick(tick_time(), &status.message());
}
void TickNullptr() {
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index a67c5f2..7db653e 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -2,9 +2,8 @@
#include <chrono>
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
namespace control_loops {
@@ -104,9 +103,8 @@
VisionTimeAdjuster::VisionTimeAdjuster(::aos::EventLoop *event_loop)
: 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")) {}
void VisionTimeAdjuster::Tick(monotonic_clock::time_point monotonic_now,
double turret_position,
@@ -117,18 +115,19 @@
// If we have new drivetrain data, we store it.
if (drivetrain_status_fetcher_.Fetch()) {
const auto &position = drivetrain_status_fetcher_.get();
- DrivetrainAngle new_position{.time = position->sent_time,
- .left = position->estimated_left_position,
- .right = position->estimated_right_position};
+ DrivetrainAngle new_position{
+ .time = drivetrain_status_fetcher_.context().monotonic_sent_time,
+ .left = position->estimated_left_position(),
+ .right = position->estimated_right_position()};
drivetrain_data_.Push(new_position);
most_recent_drivetrain_angle_ = ComputeDrivetrainPosition(new_position);
}
// If we have new vision data, compute the newest absolute angle at which the
// target is.
- if (vision_status != nullptr && vision_status->image_valid) {
+ if (vision_status != nullptr && vision_status->image_valid()) {
monotonic_clock::time_point last_target_time(
- monotonic_clock::duration(vision_status->target_time));
+ monotonic_clock::duration(vision_status->target_time()));
double column_angle = 0;
double drivetrain_angle = 0;
@@ -143,9 +142,9 @@
AOS_LOG(INFO, "Accepting Vision angle of %f, age %f\n",
most_recent_vision_angle_,
::aos::time::DurationInSeconds(monotonic_now - last_target_time));
- most_recent_vision_reading_ = vision_status->angle;
+ most_recent_vision_reading_ = vision_status->angle();
most_recent_vision_angle_ =
- vision_status->angle + column_angle + drivetrain_angle;
+ vision_status->angle() + column_angle + drivetrain_angle;
most_recent_vision_time_ = monotonic_now;
}
}
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index b45f827..3c873f7 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -4,10 +4,10 @@
#include <stdint.h>
#include "aos/containers/ring_buffer.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2017/vision/vision.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -51,7 +51,7 @@
private:
// Fetcher to grab the latest drivetrain message.
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
// Buffer space to store the most recent drivetrain and turret messages from
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
index 4f341de..7e9d82d 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -2,12 +2,13 @@
#include "gtest/gtest.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/configuration.h"
+#include "aos/events/simulated_event_loop.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -17,19 +18,20 @@
public:
VisionTimeAdjusterTest()
: ::testing::Test(),
- simulation_event_loop_(simulated_event_loop_factory_.MakeEventLoop()),
+ configuration_(aos::configuration::ReadConfig("y2017/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ simulation_event_loop_(event_loop_factory_.MakeEventLoop()),
drivetrain_status_sender_(
simulation_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeSender<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
vision_status_sender_(
simulation_event_loop_->MakeSender<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
- vision_time_adjuster_event_loop_(
- simulated_event_loop_factory_.MakeEventLoop()),
- vision_status_fetcher_(vision_time_adjuster_event_loop_
- ->MakeFetcher<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
+ "/vision")),
+ vision_time_adjuster_event_loop_(event_loop_factory_.MakeEventLoop()),
+ vision_status_fetcher_(
+ vision_time_adjuster_event_loop_
+ ->MakeFetcher<::y2017::vision::VisionStatus>("/vision")),
adjuster_(vision_time_adjuster_event_loop_.get()) {
::aos::testing::EnableTestLogging();
static_assert(kVisionDelay % kTimeTick == ::std::chrono::milliseconds(0),
@@ -75,11 +77,11 @@
VisionTimeAdjuster *adjuster() { return &adjuster_; }
private:
- void TickTime() { simulated_event_loop_factory_.RunFor(kTimeTick); }
+ void TickTime() { event_loop_factory_.RunFor(kTimeTick); }
void SendMessages() {
SendDrivetrainPosition();
- if (simulated_event_loop_factory_.monotonic_now().time_since_epoch() %
+ if (event_loop_factory_.monotonic_now().time_since_epoch() %
kVisionTick ==
kVisionDelay) {
SendVisionTarget();
@@ -89,25 +91,33 @@
}
void SendDrivetrainPosition() {
- auto message = drivetrain_status_sender_.MakeMessage();
- message->estimated_left_position = drivetrain_left_;
- message->estimated_right_position = drivetrain_right_;
- ASSERT_TRUE(message.Send());
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::Status::Builder status_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Status>();
+ status_builder.add_estimated_left_position(drivetrain_left_);
+ status_builder.add_estimated_right_position(drivetrain_right_);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
}
void SendVisionTarget() {
- auto message = vision_status_sender_.MakeMessage();
- message->target_time =
+ auto builder = vision_status_sender_.MakeBuilder();
+
+ vision::VisionStatus::Builder vision_status_builder =
+ builder.MakeBuilder<vision::VisionStatus>();
+ vision_status_builder.add_target_time(
(simulation_event_loop_->monotonic_now() - kVisionDelay)
.time_since_epoch()
- .count();
- message->distance = vision_distance_;
+ .count());
+ vision_status_builder.add_distance(vision_distance_);
ASSERT_EQ(turret_history_.capacity(), turret_history_.size());
ASSERT_EQ(drivetrain_history_.capacity(), drivetrain_history_.size());
- message->angle =
- vision_angle_ - turret_history_[0] - drivetrain_history_[0];
- message->image_valid = true;
- ASSERT_TRUE(message.Send());
+ vision_status_builder.add_angle(vision_angle_ - turret_history_[0] -
+ drivetrain_history_[0]);
+ vision_status_builder.add_image_valid(true);
+
+ ASSERT_TRUE(builder.Send(vision_status_builder.Finish()));
}
double GetDriveTrainAngle() const {
@@ -115,9 +125,10 @@
(drivetrain::kRobotRadius * 2.0);
}
- ::aos::SimulatedEventLoopFactory simulated_event_loop_factory_;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ ::aos::SimulatedEventLoopFactory event_loop_factory_;
::std::unique_ptr<::aos::EventLoop> simulation_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Status>
drivetrain_status_sender_;
::aos::Sender<::y2017::vision::VisionStatus> vision_status_sender_;
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 3bbbeb2..e42085a 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -4,19 +4,18 @@
#include <unistd.h>
#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
#include "aos/logging/logging.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "aos/input/drivetrain_input.h"
-#include "aos/input/action_joystick_input.h"
-#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
@@ -28,6 +27,8 @@
namespace input {
namespace joysticks {
+namespace superstructure = control_loops::superstructure;
+
const ButtonLocation kGearSlotBack(2, 11);
const ButtonLocation kIntakeDown(3, 9);
@@ -54,13 +55,13 @@
::y2017::control_loops::drivetrain::GetDrivetrainConfig(),
DrivetrainInputReader::InputType::kSteeringWheel, {}),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2017::control_loops::SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
+ event_loop
+ ->MakeFetcher<::y2017::control_loops::superstructure::Status>(
+ "/superstructure")),
superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2017::control_loops::SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")) {}
+ ->MakeSender<::y2017::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
@@ -150,109 +151,155 @@
fire_ = data.IsPressed(kFire) && shooter_velocity_ != 0.0;
if (data.IsPressed(kVisionAlign)) {
- fire_ = fire_ && superstructure_status_fetcher_->turret.vision_tracking;
+ fire_ = fire_ && superstructure_status_fetcher_->turret()->vision_tracking();
}
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
if (data.IsPressed(kHang)) {
intake_goal_ = 0.23;
}
- new_superstructure_goal->intake.distance = intake_goal_;
- new_superstructure_goal->intake.disable_intake = false;
- new_superstructure_goal->turret.angle = turret_goal_;
- new_superstructure_goal->turret.track = vision_track;
- new_superstructure_goal->hood.angle = hood_goal_;
- new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+ bool intake_disable_intake = false;
+ double intake_gear_servo = 0.0;
if (data.IsPressed(kIntakeUp)) {
- new_superstructure_goal->intake.gear_servo = 0.30;
+ intake_gear_servo = 0.30;
} else {
// Clamp the gear
- new_superstructure_goal->intake.gear_servo = 0.66;
+ intake_gear_servo = 0.66;
}
- new_superstructure_goal->intake.profile_params.max_velocity = 0.50;
- new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+ double intake_voltage_rollers = 0.0;
- new_superstructure_goal->intake.profile_params.max_acceleration = 3.0;
- if (vision_track) {
- new_superstructure_goal->turret.profile_params.max_acceleration = 35.0;
- new_superstructure_goal->turret.profile_params.max_velocity = 10.0;
- } else {
- new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
- new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
- }
- new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
-
- new_superstructure_goal->intake.voltage_rollers = 0.0;
- new_superstructure_goal->lights_on = lights_on;
-
- if (data.IsPressed(kVisionAlign) && vision_distance_shot_) {
- new_superstructure_goal->use_vision_for_shots = true;
- } else {
- new_superstructure_goal->use_vision_for_shots = false;
- }
-
- if (superstructure_status_fetcher_->intake.position >
- superstructure_status_fetcher_->intake.unprofiled_goal_position +
+ if (superstructure_status_fetcher_->intake()->position() >
+ superstructure_status_fetcher_->intake()->unprofiled_goal_position() +
0.01) {
intake_accumulator_ = 10;
}
if (intake_accumulator_ > 0) {
--intake_accumulator_;
- if (!superstructure_status_fetcher_->intake.estopped) {
- new_superstructure_goal->intake.voltage_rollers = 10.0;
+ if (!superstructure_status_fetcher_->intake()->estopped()) {
+ intake_voltage_rollers = 10.0;
}
}
if (data.IsPressed(kHang)) {
- new_superstructure_goal->intake.voltage_rollers = -10.0;
- new_superstructure_goal->intake.disable_intake = true;
+ intake_voltage_rollers = -10.0;
+ intake_disable_intake = true;
} else if (data.IsPressed(kIntakeIn) || data.IsPressed(kFire)) {
if (robot_velocity() > 2.0) {
- new_superstructure_goal->intake.voltage_rollers = 12.0;
+ intake_voltage_rollers = 12.0;
} else {
- new_superstructure_goal->intake.voltage_rollers = 11.0;
+ intake_voltage_rollers = 11.0;
}
} else if (data.IsPressed(kIntakeOut)) {
- new_superstructure_goal->intake.voltage_rollers = -8.0;
+ intake_voltage_rollers = -8.0;
}
if (intake_goal_ < 0.1) {
- new_superstructure_goal->intake.voltage_rollers =
- ::std::min(8.0, new_superstructure_goal->intake.voltage_rollers);
+ intake_voltage_rollers = ::std::min(8.0, intake_voltage_rollers);
}
+ double indexer_voltage_rollers = 0.0;
+ double indexer_angular_velocity = 0.0;
if (data.IsPressed(kReverseIndexer)) {
- new_superstructure_goal->indexer.voltage_rollers = -12.0;
- new_superstructure_goal->indexer.angular_velocity = 4.0;
- new_superstructure_goal->indexer.angular_velocity = 1.0;
+ indexer_voltage_rollers = -12.0;
+ indexer_angular_velocity = 1.0;
} else if (fire_) {
- new_superstructure_goal->indexer.voltage_rollers = 12.0;
+ indexer_voltage_rollers = 12.0;
switch (last_shot_distance_) {
case ShotDistance::VISION_SHOT:
- new_superstructure_goal->indexer.angular_velocity = -1.25 * M_PI;
+ indexer_angular_velocity = -1.25 * M_PI;
break;
case ShotDistance::MIDDLE_SHOT:
case ShotDistance::FAR_SHOT:
- new_superstructure_goal->indexer.angular_velocity = -2.25 * M_PI;
+ indexer_angular_velocity = -2.25 * M_PI;
break;
}
} else {
- new_superstructure_goal->indexer.voltage_rollers = 0.0;
- new_superstructure_goal->indexer.angular_velocity = 0.0;
+ indexer_voltage_rollers = 0.0;
+ indexer_angular_velocity = 0.0;
}
- AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
+ superstructure::IndexerGoal::Builder indexer_goal_builder =
+ builder.MakeBuilder<superstructure::IndexerGoal>();
+ indexer_goal_builder.add_angular_velocity(indexer_angular_velocity);
+ indexer_goal_builder.add_voltage_rollers(indexer_voltage_rollers);
+
+ flatbuffers::Offset<superstructure::IndexerGoal> indexer_goal_offset =
+ indexer_goal_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset;
+ if (vision_track) {
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 10.0, 35.0);
+ } else {
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 6.0, 15.0);
+ }
+ superstructure::TurretGoal::Builder turret_goal_builder =
+ builder.MakeBuilder<superstructure::TurretGoal>();
+ turret_goal_builder.add_angle(turret_goal_);
+ turret_goal_builder.add_track(vision_track);
+ turret_goal_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<superstructure::TurretGoal> turret_goal_offset =
+ turret_goal_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 0.5, 3.0);
+
+ superstructure::IntakeGoal::Builder intake_goal_builder =
+ builder.MakeBuilder<superstructure::IntakeGoal>();
+ intake_goal_builder.add_voltage_rollers(intake_voltage_rollers);
+ intake_goal_builder.add_distance(intake_goal_);
+ intake_goal_builder.add_disable_intake(intake_disable_intake);
+ intake_goal_builder.add_gear_servo(intake_gear_servo);
+ intake_goal_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<superstructure::IntakeGoal> intake_goal_offset =
+ intake_goal_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 5.0, 25.0);
+
+ superstructure::HoodGoal::Builder hood_goal_builder =
+ builder.MakeBuilder<superstructure::HoodGoal>();
+ hood_goal_builder.add_angle(hood_goal_);
+ hood_goal_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<superstructure::HoodGoal> hood_goal_offset =
+ hood_goal_builder.Finish();
+
+ superstructure::ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<superstructure::ShooterGoal>();
+ shooter_goal_builder.add_angular_velocity(shooter_velocity_);
+ flatbuffers::Offset<superstructure::ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+ goal_builder.add_lights_on(lights_on);
+ if (data.IsPressed(kVisionAlign) && vision_distance_shot_) {
+ goal_builder.add_use_vision_for_shots(true);
+ } else {
+ goal_builder.add_use_vision_for_shots(false);
+ }
+
+ goal_builder.add_intake(intake_goal_offset);
+ goal_builder.add_indexer(indexer_goal_offset);
+ goal_builder.add_turret(turret_goal_offset);
+ goal_builder.add_hood(hood_goal_offset);
+ goal_builder.add_shooter(shooter_goal_offset);
+
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
private:
- ::aos::Fetcher<::y2017::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2017::control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2017::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2017::control_loops::superstructure::Goal>
superstructure_goal_sender_;
ShotDistance last_shot_distance_ = ShotDistance::VISION_SHOT;
@@ -276,7 +323,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());
::y2017::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index 83447e3..1997870 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -1,14 +1,15 @@
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
package(default_visibility = ["//visibility:public"])
-queue_library(
- name = "vision_queue",
+flatbuffer_cc_library(
+ name = "vision_fbs",
srcs = [
- "vision.q",
+ "vision.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -58,12 +59,11 @@
visibility = ["//visibility:public"],
deps = [
":target_finder",
- ":vision_queue",
+ ":vision_fbs",
":vision_result",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/mutex",
"//aos/time",
"//aos/vision/events:udp",
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
index 6b86972..e96fba9 100644
--- a/y2017/vision/target_receiver.cc
+++ b/y2017/vision/target_receiver.cc
@@ -1,13 +1,12 @@
#include <netdb.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/vision/events/udp.h"
#include "y2017/vision/target_finder.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
#include "y2017/vision/vision_result.pb.h"
namespace y2017 {
@@ -20,11 +19,13 @@
char raw_data[65507];
// TODO(parker): Have this pull in a config from somewhere.
TargetFinder finder;
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::aos::Sender<::y2017::vision::VisionStatus> vision_status_sender =
- event_loop.MakeSender<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status");
+ event_loop.MakeSender<::y2017::vision::VisionStatus>("/vision");
while (true) {
// TODO(austin): Don't malloc.
@@ -42,22 +43,27 @@
continue;
}
- auto new_vision_status = vision_status_sender.MakeMessage();
- new_vision_status->image_valid = target.has_target();
- if (new_vision_status->image_valid) {
- new_vision_status->target_time =
+ auto builder = vision_status_sender.MakeBuilder();
+ VisionStatus::Builder vision_status_builder =
+ builder.MakeBuilder<VisionStatus>();
+ vision_status_builder.add_image_valid(target.has_target());
+ if (target.has_target()) {
+ vision_status_builder.add_target_time (
std::chrono::duration_cast<std::chrono::nanoseconds>(
target_time.time_since_epoch())
- .count();
+ .count());
+ double distance = 0.0;
+ double angle = 0.0;
finder.GetAngleDist(
aos::vision::Vector<2>(target.target().x(), target.target().y()),
/* TODO: Insert down estimate here in radians: */ 0.0,
- &new_vision_status->distance, &new_vision_status->angle);
+ &distance, &angle);
+ vision_status_builder.add_distance(distance);
+ vision_status_builder.add_angle(angle);
}
- AOS_LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
+ if (!builder.Send(vision_status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send vision information\n");
}
}
diff --git a/y2017/vision/vision.fbs b/y2017/vision/vision.fbs
new file mode 100644
index 0000000..f1bc013
--- /dev/null
+++ b/y2017/vision/vision.fbs
@@ -0,0 +1,16 @@
+namespace y2017.vision;
+
+// Published on ".y2017.vision.vision_status"
+table VisionStatus {
+ image_valid:bool;
+
+ // Distance to the target in meters.
+ distance:double;
+ // The angle in radians of the bottom of the target.
+ angle:double;
+
+ // Capture time of the angle using the clock behind monotonic_clock::now().
+ target_time:long;
+}
+
+root_type VisionStatus;
diff --git a/y2017/vision/vision.q b/y2017/vision/vision.q
deleted file mode 100644
index 6e4a02a..0000000
--- a/y2017/vision/vision.q
+++ /dev/null
@@ -1,14 +0,0 @@
-package y2017.vision;
-
-// Published on ".y2017.vision.vision_status"
-message VisionStatus {
- bool image_valid;
-
- // Distance to the target in meters.
- double distance;
- // The angle in radians of the bottom of the target.
- double angle;
-
- // Capture time of the angle using the clock behind monotonic_clock::now().
- int64_t target_time;
-};
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index e55e350..d08909f 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -22,21 +22,21 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -46,19 +46,20 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2017::control_loops::SuperstructureQueue;
+namespace superstructure = ::y2017::control_loops::superstructure;
using ::y2017::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -128,14 +129,14 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -207,18 +208,20 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_right_encoder(
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ builder.Send(position_builder.Finish());
}
}
@@ -226,49 +229,77 @@
const auto values = constants::GetValues();
{
- auto superstructure_message = superstructure_position_sender_.MakeMessage();
- CopyPosition(intake_encoder_, &superstructure_message->intake,
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePositionT intake;
+ CopyPosition(intake_encoder_, &intake,
Values::kIntakeEncoderCountsPerRevolution,
Values::kIntakeEncoderRatio, intake_pot_translate, true,
values.intake.pot_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &intake);
- CopyPosition(indexer_counter_, &superstructure_message->column.indexer,
+ frc971::HallEffectAndPositionT indexer;
+ CopyPosition(indexer_counter_, &indexer,
Values::kIndexerEncoderCountsPerRevolution,
Values::kIndexerEncoderRatio, true);
+ flatbuffers::Offset<frc971::HallEffectAndPosition> indexer_offset =
+ frc971::HallEffectAndPosition::Pack(*builder.fbb(), &indexer);
- superstructure_message->theta_shooter =
- encoder_translate(shooter_encoder_->GetRaw(),
- Values::kShooterEncoderCountsPerRevolution,
- Values::kShooterEncoderRatio);
-
- CopyPosition(hood_encoder_, &superstructure_message->hood,
+ frc971::IndexPositionT hood;
+ CopyPosition(hood_encoder_, &hood,
Values::kHoodEncoderCountsPerRevolution,
Values::kHoodEncoderRatio, true);
+ flatbuffers::Offset<frc971::IndexPosition> hood_offset =
+ frc971::IndexPosition::Pack(*builder.fbb(), &hood);
- CopyPosition(turret_counter_, &superstructure_message->column.turret,
+ frc971::HallEffectAndPositionT turret;
+ CopyPosition(turret_counter_, &turret,
Values::kTurretEncoderCountsPerRevolution,
Values::kTurretEncoderRatio, false);
+ flatbuffers::Offset<frc971::HallEffectAndPosition> turret_offset =
+ frc971::HallEffectAndPosition::Pack(*builder.fbb(), &turret);
- superstructure_message.Send();
+ superstructure::ColumnPosition::Builder column_builder =
+ builder.MakeBuilder<superstructure::ColumnPosition>();
+ column_builder.add_indexer(indexer_offset);
+ column_builder.add_turret(turret_offset);
+ flatbuffers::Offset<superstructure::ColumnPosition> column_offset =
+ column_builder.Finish();
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_column(column_offset);
+ position_builder.add_hood(hood_offset);
+ position_builder.add_intake(intake_offset);
+ position_builder.add_theta_shooter(
+ encoder_translate(shooter_encoder_->GetRaw(),
+ Values::kShooterEncoderCountsPerRevolution,
+ Values::kShooterEncoderRatio));
+
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+ ::frc971::autonomous::AutonomousMode::Builder auto_builder =
+ builder.MakeBuilder<::frc971::autonomous::AutonomousMode>();
+
+ int mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
+ mode |= 1 << i;
}
}
- AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
+ auto_builder.add_mode(mode);
+ builder.Send(auto_builder.Finish());
}
}
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
DigitalGlitchFilter hall_filter_;
@@ -293,8 +324,8 @@
public:
SolenoidWriter(::aos::EventLoop *event_loop)
: superstructure_output_fetcher_(
- event_loop->MakeFetcher<SuperstructureQueue::Output>(
- ".y2017.control_loops.superstructure_queue.output")) {
+ event_loop->MakeFetcher<superstructure::Output>(
+ "/superstructure")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -321,11 +352,10 @@
{
superstructure_output_fetcher_.Fetch();
if (superstructure_output_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_output_fetcher_);
- lights_->Set(superstructure_output_fetcher_->lights_on);
- rgb_lights_->Set(superstructure_output_fetcher_->red_light_on |
- superstructure_output_fetcher_->green_light_on |
- superstructure_output_fetcher_->blue_light_on);
+ lights_->Set(superstructure_output_fetcher_->lights_on());
+ rgb_lights_->Set(superstructure_output_fetcher_->red_light_on() |
+ superstructure_output_fetcher_->green_light_on() |
+ superstructure_output_fetcher_->blue_light_on());
}
}
@@ -337,16 +367,16 @@
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> lights_, rgb_lights_;
- ::aos::Fetcher<::y2017::control_loops::SuperstructureQueue::Output>
+ ::aos::Fetcher<::y2017::control_loops::superstructure::Output>
superstructure_output_fetcher_;
};
class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
- event_loop, ".y2017.control_loops.superstructure_queue.output") {}
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure") {}
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
@@ -386,27 +416,26 @@
}
private:
- virtual void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- intake_victor_->SetSpeed(::aos::Clip(output.voltage_intake,
+ virtual void Write(const superstructure::Output &output) override {
+ intake_victor_->SetSpeed(::aos::Clip(output.voltage_intake(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- intake_rollers_victor_->SetSpeed(output.voltage_intake_rollers / 12.0);
- indexer_victor_->SetSpeed(-output.voltage_indexer / 12.0);
- indexer_roller_victor_->SetSpeed(output.voltage_indexer_rollers / 12.0);
- turret_victor_->SetSpeed(::aos::Clip(-output.voltage_turret,
+ intake_rollers_victor_->SetSpeed(output.voltage_intake_rollers() / 12.0);
+ indexer_victor_->SetSpeed(-output.voltage_indexer() / 12.0);
+ indexer_roller_victor_->SetSpeed(output.voltage_indexer_rollers() / 12.0);
+ turret_victor_->SetSpeed(::aos::Clip(-output.voltage_turret(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- hood_victor_->SetSpeed(
- ::aos::Clip(output.voltage_hood, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- shooter_victor_->SetSpeed(output.voltage_shooter / 12.0);
+ hood_victor_->SetSpeed(::aos::Clip(output.voltage_hood(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ shooter_victor_->SetSpeed(output.voltage_shooter() / 12.0);
- red_light_->Set(output.red_light_on);
- green_light_->Set(output.green_light_on);
- blue_light_->Set(output.blue_light_on);
+ red_light_->Set(output.red_light_on());
+ green_light_->Set(output.green_light_on());
+ blue_light_->Set(output.blue_light_on());
- gear_servo_->SetPosition(output.gear_servo);
+ gear_servo_->SetPosition(output.gear_servo());
}
virtual void Stop() override {
@@ -443,19 +472,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
@@ -484,7 +516,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 5.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
auto imu_trigger = make_unique<DigitalInput>(3);
::frc971::wpilib::ADIS16448 imu(&imu_event_loop, SPI::Port::kOnboardCS1,
imu_trigger.get());
@@ -493,7 +525,7 @@
imu.set_reset(imu_reset.get());
AddLoop(&imu_event_loop);
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)), true);
@@ -529,7 +561,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_lights(solenoid_writer.pcm()->MakeSolenoid(0));
solenoid_writer.set_rgb_light(solenoid_writer.pcm()->MakeSolenoid(1));
diff --git a/y2017/y2017.json b/y2017/y2017.json
new file mode 100644
index 0000000..90aa77c
--- /dev/null
+++ b/y2017/y2017.json
@@ -0,0 +1,34 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2017.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2017.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2017.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2017.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/vision",
+ "type": "y2017.vision.VisionStatus",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}
diff --git a/y2018/BUILD b/y2018/BUILD
index d7cc2fd..94fd844 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -1,6 +1,7 @@
load("//frc971:downloader.bzl", "robot_downloader")
-load("//aos/build:queues.bzl", "queue_library")
+load("//aos:config.bzl", "aos_config")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
start_binaries = [
@@ -30,11 +31,12 @@
"//aos/time",
"//aos/util:log_interval",
"//aos/vision/events:udp",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018/control_loops/drivetrain:drivetrain_base",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
"//y2018/control_loops/superstructure/arm:generated_graph",
],
)
@@ -67,21 +69,21 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- ":status_light",
+ ":status_light_fbs",
"//aos:init",
"//aos:make_unique",
"//aos:math",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
@@ -89,7 +91,7 @@
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
@@ -97,17 +99,37 @@
"//third_party:phoenix",
"//third_party:wpilib",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
- "//y2018/vision:vision_queue",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/vision:vision_fbs",
],
)
-queue_library(
- name = "status_light",
+flatbuffer_cc_library(
+ name = "status_light_fbs",
srcs = [
- "status_light.q",
+ "status_light.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+aos_config(
+ name = "config",
+ src = "y2018.json",
+ flatbuffers = [
+ ":status_light_fbs",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
+ "//y2018/vision:vision_fbs",
],
visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
)
cc_proto_library(
diff --git a/y2018/actors/BUILD b/y2018/actors/BUILD
index 7d97f66..7a95419 100644
--- a/y2018/actors/BUILD
+++ b/y2018/actors/BUILD
@@ -8,14 +8,14 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018/control_loops/drivetrain:drivetrain_base",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
"//y2018/control_loops/superstructure/arm:generated_graph",
],
)
@@ -29,7 +29,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index dd94083..89082df 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -8,12 +8,11 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
namespace y2018 {
namespace actors {
-using ::frc971::ProfileParameters;
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -23,25 +22,34 @@
namespace arm = ::y2018::control_loops::superstructure::arm;
-const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
-const ProfileParameters kDrive = {5.0, 2.5};
-const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
-const ProfileParameters kSlowDrive = {1.5, 2.5};
-const ProfileParameters kScaleTurnDrive = {3.0, 2.5};
-const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
-const ProfileParameters kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
-const ProfileParameters kTurn = {4.0, 2.0};
-const ProfileParameters kSweepingTurn = {5.0, 7.0};
-const ProfileParameters kFarScaleSweepingTurn = kSweepingTurn;
-const ProfileParameters kFastTurn = {5.0, 7.0};
-const ProfileParameters kReallyFastTurn = {1.5, 9.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
-const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
-const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
+const ProfileParametersT kFinalSwitchDrive = MakeProfileParameters(0.5, 1.5);
+const ProfileParametersT kDrive = MakeProfileParameters(5.0, 2.5);
+const ProfileParametersT kThirdBoxDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kSlowDrive = MakeProfileParameters(1.5, 2.5);
+const ProfileParametersT kScaleTurnDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kFarSwitchTurnDrive = MakeProfileParameters(2.0, 2.5);
+const ProfileParametersT kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
+const ProfileParametersT kTurn = MakeProfileParameters(4.0, 2.0);
+const ProfileParametersT kSweepingTurn = MakeProfileParameters(5.0, 7.0);
+const ProfileParametersT kFarScaleSweepingTurn = kSweepingTurn;
+const ProfileParametersT kFastTurn = MakeProfileParameters(5.0, 7.0);
+const ProfileParametersT kReallyFastTurn = MakeProfileParameters(1.5, 9.0);
-const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.0};
+const ProfileParametersT kThirdBoxSlowBackup = MakeProfileParameters(0.35, 1.5);
+const ProfileParametersT kThirdBoxSlowTurn = MakeProfileParameters(1.5, 4.0);
-const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
+const ProfileParametersT kThirdBoxPlaceDrive = MakeProfileParameters(4.0, 2.0);
+
+const ProfileParametersT kFinalFrontFarSwitchDrive =
+ MakeProfileParameters(2.0, 2.0);
} // namespace
@@ -49,24 +57,23 @@
: frc971::autonomous::BaseAutonomousActor(
event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
superstructure_goal_sender_(
- event_loop
- ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
- ".frc971.control_loops.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2018::control_loops::superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Status>(
- ".frc971.control_loops.superstructure_queue.status")) {}
+ event_loop
+ ->MakeFetcher<::y2018::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
Reset();
// Switch
/*
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
case 2: {
if (FarSwitch(start_time, true)) return true;
@@ -79,7 +86,7 @@
}
*/
// Scale
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
case 1: {
if (FarScale(start_time)) return true;
@@ -91,7 +98,7 @@
} break;
}
/*
- switch (params.mode) {
+ switch (params->mode()) {
case 1: {
if (FarScale(start_time)) return true;
//if (CloseSwitch(start_time)) return true;
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index 3ef7677..014459a 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -6,12 +6,13 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace actors {
@@ -21,7 +22,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
@@ -40,9 +41,9 @@
SendSuperstructureGoal();
}
- ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2018::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2018::control_loops::superstructure::Status>
superstructure_status_fetcher_;
double roller_voltage_ = 0.0;
@@ -82,19 +83,29 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->intake.roller_voltage = roller_voltage_;
- new_superstructure_goal->intake.left_intake_angle = left_intake_angle_;
- new_superstructure_goal->intake.right_intake_angle = right_intake_angle_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ control_loops::superstructure::IntakeGoal::Builder intake_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::IntakeGoal>();
+ intake_goal_builder.add_roller_voltage(roller_voltage_);
+ intake_goal_builder.add_left_intake_angle(left_intake_angle_);
+ intake_goal_builder.add_right_intake_angle(right_intake_angle_);
- new_superstructure_goal->arm_goal_position = arm_goal_position_;
- new_superstructure_goal->grab_box = grab_box_;
- new_superstructure_goal->open_claw = open_claw_;
- new_superstructure_goal->close_claw = close_claw_;
- new_superstructure_goal->deploy_fork = deploy_fork_;
- new_superstructure_goal->trajectory_override = false;
+ flatbuffers::Offset<control_loops::superstructure::IntakeGoal>
+ intake_offset = intake_goal_builder.Finish();
- if (!new_superstructure_goal.Send()) {
+ control_loops::superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_offset);
+
+ superstructure_builder.add_arm_goal_position(arm_goal_position_);
+ superstructure_builder.add_grab_box(grab_box_);
+ superstructure_builder.add_open_claw(open_claw_);
+ superstructure_builder.add_close_claw(close_claw_);
+ superstructure_builder.add_deploy_fork(deploy_fork_);
+ superstructure_builder.add_trajectory_override(false);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -129,17 +140,17 @@
superstructure_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal);
+ drivetrain_status_fetcher_->profiled_left_position_goal());
const double right_profile_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal);
+ drivetrain_status_fetcher_->profiled_right_position_goal());
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double profile_distance_to_go =
(left_profile_error + right_profile_error) / 2.0;
@@ -147,12 +158,12 @@
const double distance_to_go = (left_error + right_error) / 2.0;
// Check superstructure first.
- if (superstructure_status_fetcher_->arm.current_node ==
+ if (superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go <
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
arm_threshold) {
AOS_LOG(INFO, "Arm finished first: %f, drivetrain %f distance\n",
- superstructure_status_fetcher_->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm()->path_distance_to_go(),
::std::abs(distance_to_go));
return true;
}
@@ -163,7 +174,7 @@
::std::abs(distance_to_go) < drive_threshold + kPositionTolerance) {
AOS_LOG(INFO,
"Drivetrain finished first: arm %f, drivetrain %f distance\n",
- superstructure_status_fetcher_->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm()->path_distance_to_go(),
::std::abs(distance_to_go));
return true;
}
@@ -183,9 +194,9 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- if (superstructure_status_fetcher_->arm.current_node ==
+ if (superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go <
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
threshold) {
return true;
}
@@ -205,7 +216,7 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- if (superstructure_status_fetcher_->arm.grab_state == 4) {
+ if (superstructure_status_fetcher_->arm()->grab_state() == 4) {
return true;
}
}
diff --git a/y2018/actors/autonomous_actor_main.cc b/y2018/actors/autonomous_actor_main.cc
index 6193d48..76fae2e 100644
--- a/y2018/actors/autonomous_actor_main.cc
+++ b/y2018/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2018/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2018::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2018/control_loops/drivetrain/BUILD b/y2018/control_loops/drivetrain/BUILD
index 1824196..4d6a6a1 100644
--- a/y2018/control_loops/drivetrain/BUILD
+++ b/y2018/control_loops/drivetrain/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index 1bb1dc0..e5c68c5 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/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 "y2018/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,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());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index af158de..5690b08 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -146,8 +146,8 @@
"arm_bounds.h",
],
deps = [
- "//third_party/eigen",
"@cgal_repo//:cgal",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -163,7 +163,6 @@
],
)
-
py_binary(
name = "graph_edit",
srcs = [
@@ -175,9 +174,9 @@
restricted_to = ["//tools:k8"],
srcs_version = "PY3",
deps = [
+ ":python_init",
"//frc971/control_loops/python:basic_window",
"//frc971/control_loops/python:color",
- ":python_init",
"@python_gtk",
],
)
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 5dabd1b..4e97571 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,18 +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:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
cc_library(
name = "superstructure_lib",
srcs = [
@@ -22,16 +52,19 @@
"superstructure.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//y2018:constants",
- "//y2018:status_light",
+ "//y2018:status_light_fbs",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
- "//y2018/vision:vision_queue",
+ "//y2018/vision:vision_fbs",
],
)
@@ -41,12 +74,15 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2018:config.json"],
shard_count = 5,
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",
@@ -63,7 +99,6 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index e8af2b9..bf3c306 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -12,7 +12,7 @@
"//aos/logging",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -27,7 +27,7 @@
":ekf",
":trajectory",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -42,8 +42,8 @@
visibility = ["//visibility:public"],
deps = [
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -77,9 +77,9 @@
":ekf",
":generated_graph",
":trajectory",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -95,7 +95,7 @@
deps = [
":dynamics",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -129,10 +129,10 @@
":generated_graph",
":graph",
":trajectory",
- "//aos/logging:queue_logging",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 2ae1996..1f904e2 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -4,7 +4,6 @@
#include <iostream>
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -44,15 +43,15 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status) {
+flatbuffers::Offset<superstructure::ArmStatus> Arm::Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw, bool close_claw,
+ const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -86,11 +85,11 @@
claw_closed_count_ = 50;
}
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
- proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
- distal_zeroing_estimator_.UpdateEstimate(position->distal);
+ proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
+ distal_zeroing_estimator_.UpdateEstimate(*position->distal());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -128,8 +127,8 @@
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
@@ -377,17 +376,29 @@
follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
- status->goal_theta0 = follower_.theta(0);
- status->goal_theta1 = follower_.theta(1);
- status->goal_omega0 = follower_.omega(0);
- status->goal_omega1 = follower_.omega(1);
- status->theta0 = arm_ekf_.X_hat(0);
- status->theta1 = arm_ekf_.X_hat(2);
- status->omega0 = arm_ekf_.X_hat(1);
- status->omega1 = arm_ekf_.X_hat(3);
- status->voltage_error0 = arm_ekf_.X_hat(4);
- status->voltage_error1 = arm_ekf_.X_hat(5);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ proximal_estimator_state_offset =
+ proximal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ distal_estimator_state_offset =
+ distal_zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::ArmStatus::Builder status_builder(*fbb);
+ status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
+ status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+
+ status_builder.add_goal_theta0(follower_.theta(0));
+ status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_omega0(follower_.omega(0));
+ status_builder.add_goal_omega1(follower_.omega(1));
+
+ status_builder.add_theta0(arm_ekf_.X_hat(0));
+ status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_omega0(arm_ekf_.X_hat(1));
+ status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
+ status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
if (!disable) {
*proximal_output = ::std::max(
@@ -402,22 +413,17 @@
*claw_closed = claw_closed_;
}
- status->proximal_estimator_state =
- proximal_zeroing_estimator_.GetEstimatorState();
- status->distal_estimator_state =
- distal_zeroing_estimator_.GetEstimatorState();
+ status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
+ status_builder.add_current_node(current_node_);
- status->path_distance_to_go = follower_.path_distance_to_go();
- status->current_node = current_node_;
-
- status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- status->grab_state = static_cast<int32_t>(grab_state_);
- status->failed_solutions = follower_.failed_solutions();
+ status_builder.add_zeroed(zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state(static_cast<int32_t>(state_));
+ status_builder.add_grab_state(static_cast<int32_t>(grab_state_));
+ status_builder.add_failed_solutions(follower_.failed_solutions());
arm_ekf_.Predict(follower_.U(), kDt());
+ return status_builder.Finish();
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 8cd6b39..7ceb001 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -1,6 +1,7 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -8,7 +9,8 @@
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -34,15 +36,15 @@
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
- void Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status);
+ flatbuffers::Offset<superstructure::ArmStatus> Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+ bool close_claw, const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -68,6 +70,12 @@
State state() const { return state_; }
GrabState grab_state() const { return grab_state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const {
+ return (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ }
+
// Returns the maximum position for the intake. This is used to override the
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index ca1289e..dde7c63 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -41,10 +41,11 @@
":intake_plants",
"//aos:math",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 6c07a49..dffebbc 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -3,9 +3,7 @@
#include <chrono>
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
@@ -80,15 +78,15 @@
loop_->Update(disabled);
}
-void IntakeController::SetStatus(IntakeSideStatus *status,
+void IntakeController::SetStatus(IntakeSideStatus::Builder *status,
const double *unsafe_goal) {
- status->goal_position = goal_angle(unsafe_goal);
- status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
- status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
- status->motor_position = loop_->X_hat(2);
- status->motor_velocity = loop_->X_hat(3);
- status->delayed_voltage = loop_->X_hat(4);
+ status->add_goal_position(goal_angle(unsafe_goal));
+ status->add_goal_velocity(loop_->R(1, 0));
+ status->add_spring_position(loop_->X_hat(0) - loop_->X_hat(2));
+ status->add_spring_velocity(loop_->X_hat(1) - loop_->X_hat(3));
+ status->add_motor_position(loop_->X_hat(2));
+ status->add_motor_velocity(loop_->X_hat(3));
+ status->add_delayed_voltage(loop_->X_hat(4));
}
IntakeSide::IntakeSide(
@@ -98,11 +96,12 @@
void IntakeSide::Reset() { state_ = State::UNINITIALIZED; }
-void IntakeSide::Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status) {
- zeroing_estimator_.UpdateEstimate(position->motor_position);
+flatbuffers::Offset<superstructure::IntakeSideStatus> IntakeSide::Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ zeroing_estimator_.UpdateEstimate(*position->motor_position());
switch (state_) {
case State::UNINITIALIZED:
@@ -132,8 +131,8 @@
state_ = State::UNINITIALIZED;
}
// ESTOP if we hit the hard limits.
- if ((status->motor_position) > controller_.intake_range_.upper ||
- (status->motor_position) < controller_.intake_range_.lower) {
+ if ((controller_.motor_position()) > controller_.intake_range().upper_hard ||
+ (controller_.motor_position()) < controller_.intake_range().lower_hard) {
AOS_LOG(ERROR, "Hit hard limits\n");
state_ = State::ESTOP;
}
@@ -145,8 +144,8 @@
}
const bool disable = (output == nullptr) || state_ != State::RUNNING;
- controller_.set_position(position->spring_angle,
- position->motor_position.encoder);
+ controller_.set_position(position->spring_angle(),
+ position->motor_position()->encoder());
controller_.Update(disable, unsafe_goal);
@@ -154,16 +153,25 @@
output->voltage_elastic = controller_.voltage();
}
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ estimator_state = zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::IntakeSideStatus::Builder status_builder(*fbb);
// Save debug/internal state.
- status->estimator_state = zeroing_estimator_.GetEstimatorState();
- controller_.SetStatus(status, unsafe_goal);
- status->calculated_velocity =
- (status->estimator_state.position - intake_last_position_) /
- controller_.kDt;
- status->zeroed = zeroing_estimator_.zeroed();
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- intake_last_position_ = status->estimator_state.position;
+ status_builder.add_estimator_state(estimator_state);
+
+ controller_.SetStatus(&status_builder, unsafe_goal);
+ status_builder.add_calculated_velocity(
+ (zeroing_estimator_.offset() + position->motor_position()->encoder() -
+ intake_last_position_) /
+ controller_.kDt);
+ status_builder.add_zeroed(zeroing_estimator_.zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state ( static_cast<int32_t>(state_));
+ intake_last_position_ =
+ zeroing_estimator_.offset() + position->motor_position()->encoder();
+
+ return status_builder.Finish();
}
} // namespace intake
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index c3d9772..7f0ec9f 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -3,12 +3,13 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -23,13 +24,14 @@
void set_position(double spring_angle, double output_position);
// Populates the status structure.
- void SetStatus(control_loops::IntakeSideStatus *status,
+ void SetStatus(superstructure::IntakeSideStatus::Builder *status,
const double *unsafe_goal);
// Returns the control loop calculated voltage.
double voltage() const;
double output_position() const { return loop_->X_hat(0); }
+ double motor_position() const { return loop_->X_hat(2); }
// Executes the control loop for a cycle.
void Update(bool disabled, const double *unsafe_goal);
@@ -40,12 +42,6 @@
// Sets the goal angle from unsafe_goal.
double goal_angle(const double *unsafe_goal);
- // The control loop.
- ::std::unique_ptr<
- StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
- StateFeedbackObserver<5, 1, 2>>>
- loop_;
-
constexpr static double kDt =
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
@@ -53,12 +49,22 @@
// possible otherwise zero.
void UpdateOffset(double offset);
+ const ::frc971::constants::Range intake_range() const {
+ return intake_range_;
+ }
+
+ private:
+ // The control loop.
+ ::std::unique_ptr<
+ StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+ StateFeedbackObserver<5, 1, 2>>>
+ loop_;
+
const ::frc971::constants::Range intake_range_;
// Stores the current zeroing estimator offset.
double offset_ = 0.0;
- private:
bool reset_ = true;
// The current sensor measurement.
@@ -75,10 +81,11 @@
// The operating voltage.
static constexpr double kOperatingVoltage() { return 12.0; }
- void Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status);
+ flatbuffers::Offset<superstructure::IntakeSideStatus> Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -91,6 +98,9 @@
State state() const { return state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const { return zeroing_estimator_.zeroed(); }
+
bool clear_of_box() const { return controller_.output_position() < -0.1; }
double output_position() const { return controller_.output_position(); }
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index a2dcaf6..70af274 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -2,14 +2,13 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
@@ -26,25 +25,23 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
+ event_loop->MakeSender<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ "/superstructure")),
drivetrain_output_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
-void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -55,7 +52,7 @@
}
const double clipped_box_distance =
- ::std::min(1.0, ::std::max(0.0, position->box_distance));
+ ::std::min(1.0, ::std::max(0.0, position->box_distance()));
const double box_velocity =
(clipped_box_distance - last_box_distance_) / 0.005;
@@ -64,31 +61,34 @@
filtered_box_velocity_ =
box_velocity * kFilteredBoxVelocityAlpha +
(1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
- status->filtered_box_velocity = filtered_box_velocity_;
constexpr double kCenteringAngleGain = 0.0;
const double left_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.left_intake_angle)) +
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->left_intake_angle())) +
last_intake_center_error_ * kCenteringAngleGain;
const double right_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.right_intake_angle)) -
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->right_intake_angle())) -
last_intake_center_error_ * kCenteringAngleGain;
- intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
- &(position->left_intake),
- output != nullptr ? &(output->left_intake) : nullptr,
- &(status->left_intake));
+ IntakeVoltageT left_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> left_status_offset =
+ intake_left_.Iterate(
+ unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
+ position->left_intake(),
+ output != nullptr ? &(left_intake_output) : nullptr, status->fbb());
- intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
- &(position->right_intake),
- output != nullptr ? &(output->right_intake) : nullptr,
- &(status->right_intake));
+ IntakeVoltageT right_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> right_status_offset =
+ intake_right_.Iterate(
+ unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
+ position->right_intake(),
+ output != nullptr ? &(right_intake_output) : nullptr, status->fbb());
const double intake_center_error =
intake_right_.output_position() - intake_left_.output_position();
@@ -97,63 +97,81 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
- bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw() : false;
if (unsafe_goal) {
- if (unsafe_goal->open_threshold != 0.0) {
- if (arm_.current_node() != unsafe_goal->arm_goal_position ||
- arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ if (unsafe_goal->open_threshold() != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position() ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold()) {
open_claw = false;
}
}
}
- arm_.Iterate(
- monotonic_now,
- unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
- unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
- &(position->arm), position->claw_beambreak_triggered,
- position->box_back_beambreak_triggered, intake_clear_of_box,
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
- unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
- output != nullptr ? &(output->voltage_proximal) : nullptr,
- output != nullptr ? &(output->voltage_distal) : nullptr,
- output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
+ const uint32_t arm_goal_position =
+ unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u;
+
+ double voltage_proximal_output = 0.0;
+ double voltage_distal_output = 0.0;
+ bool release_arm_brake_output = false;
+ bool claw_grabbed_output = false;
+ flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset =
+ arm_.Iterate(
+ monotonic_now,
+ unsafe_goal != nullptr ? &(arm_goal_position) : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box() : false, open_claw,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw() : false,
+ position->arm(), position->claw_beambreak_triggered(),
+ position->box_back_beambreak_triggered(), intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch() > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
+ output != nullptr ? &voltage_proximal_output : nullptr,
+ output != nullptr ? &voltage_distal_output : nullptr,
+ output != nullptr ? &release_arm_brake_output : nullptr,
+ output != nullptr ? &claw_grabbed_output : nullptr, status->fbb());
+
+
+ bool hook_release_output = false;
+ bool forks_release_output = false;
+ double voltage_winch_output = 0.0;
if (output) {
if (unsafe_goal) {
- output->hook_release = unsafe_goal->hook_release;
- output->voltage_winch = unsafe_goal->voltage_winch;
- output->forks_release = unsafe_goal->deploy_fork;
- } else {
- output->voltage_winch = 0.0;
- output->hook_release = false;
- output->forks_release = false;
+ hook_release_output = unsafe_goal->hook_release();
+ voltage_winch_output = unsafe_goal->voltage_winch();
+ forks_release_output = unsafe_goal->deploy_fork();
}
}
- status->estopped = status->left_intake.estopped ||
- status->right_intake.estopped || status->arm.estopped;
+ Status::Builder status_builder = status->MakeBuilder<Status>();
- status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
- status->arm.zeroed;
+ status_builder.add_left_intake(left_status_offset);
+ status_builder.add_right_intake(right_status_offset);
+ status_builder.add_arm(arm_status_offset);
+
+ status_builder.add_filtered_box_velocity(filtered_box_velocity_);
+ const bool estopped =
+ intake_left_.estopped() || intake_right_.estopped() || arm_.estopped();
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_zeroed(intake_left_.zeroed() && intake_right_.zeroed() &&
+ arm_.zeroed());
if (output && unsafe_goal) {
- double roller_voltage = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ double roller_voltage =
+ ::std::max(-kMaxIntakeRollerVoltage,
+ ::std::min(unsafe_goal->intake()->roller_voltage(),
+ kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 14;
- if (unsafe_goal->intake.roller_voltage < 0.0 ||
- unsafe_goal->disable_box_correct) {
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ if (unsafe_goal->intake()->roller_voltage() < 0.0 ||
+ unsafe_goal->disable_box_correct()) {
+ left_intake_output.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
stuck_count_ = 0;
} else {
- const bool stuck = position->box_distance < 0.20 &&
- filtered_box_velocity_ > -0.05 &&
- !position->box_back_beambreak_triggered;
+ const bool stuck = position->box_distance() < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered();
// Make sure we don't declare ourselves re-stuck too quickly. We want to
// wait 400 ms before triggering the stuck condition again.
if (!stuck) {
@@ -171,11 +189,11 @@
rotation_state_ = RotationState::STUCK;
++stuck_count_;
last_stuck_time_ = monotonic_now;
- } else if (position->left_intake.beam_break) {
+ } else if (position->left_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
- } else if (position->right_intake.beam_break) {
+ } else if (position->right_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
@@ -190,7 +208,7 @@
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->right_intake.beam_break) {
+ if (position->right_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -200,7 +218,7 @@
}
break;
case RotationState::ROTATING_RIGHT:
- if (position->left_intake.beam_break) {
+ if (position->left_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -214,7 +232,7 @@
constexpr double kHoldVoltage = 1.0;
constexpr double kStuckVoltage = 10.0;
- if (position->box_back_beambreak_triggered &&
+ if (position->box_back_beambreak_triggered() &&
roller_voltage > kHoldVoltage) {
roller_voltage = kHoldVoltage;
}
@@ -226,31 +244,31 @@
centering_gain = 0.0;
}
}
- output->left_intake.voltage_rollers =
+ left_intake_output.voltage_rollers =
roller_voltage - intake_center_error * centering_gain;
- output->right_intake.voltage_rollers =
+ right_intake_output.voltage_rollers =
roller_voltage + intake_center_error * centering_gain;
} break;
case RotationState::STUCK: {
if (roller_voltage > kHoldVoltage) {
- output->left_intake.voltage_rollers = -kStuckVoltage;
- output->right_intake.voltage_rollers = -kStuckVoltage;
+ left_intake_output.voltage_rollers = -kStuckVoltage;
+ right_intake_output.voltage_rollers = -kStuckVoltage;
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->left_intake.beam_break) {
- output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ if (position->left_intake()->beam_break()) {
+ left_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ left_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
- output->right_intake.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = roller_voltage;
- if (position->right_intake.beam_break) {
- output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ left_intake_output.voltage_rollers = roller_voltage;
+ if (position->right_intake()->beam_break()) {
+ right_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ right_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
break;
}
@@ -260,29 +278,31 @@
rotation_count_ = 0;
stuck_count_ = 0;
}
- status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ status_builder.add_rotation_state(static_cast<uint32_t>(rotation_state_));
drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
- if (status->estopped) {
+ if (estopped) {
SendColors(0.5, 0.0, 0.0);
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
SendColors(0.5, 0.5, 0.0);
} else if (rotation_state_ == RotationState::ROTATING_LEFT ||
rotation_state_ == RotationState::ROTATING_RIGHT) {
SendColors(0.5, 0.20, 0.0);
} else if (rotation_state_ == RotationState::STUCK) {
SendColors(0.5, 0.0, 0.5);
- } else if (position->box_back_beambreak_triggered) {
+ } else if (position->box_back_beambreak_triggered()) {
SendColors(0.0, 0.0, 0.5);
- } else if (position->box_distance < 0.2) {
+ } else if (position->box_distance() < 0.2) {
SendColors(0.0, 0.5, 0.0);
} else if (drivetrain_output_fetcher_.get() &&
- ::std::max(::std::abs(drivetrain_output_fetcher_->left_voltage),
- ::std::abs(drivetrain_output_fetcher_->right_voltage)) >
+ ::std::max(
+ ::std::abs(drivetrain_output_fetcher_->left_voltage()),
+ ::std::abs(drivetrain_output_fetcher_->right_voltage())) >
11.5) {
SendColors(0.5, 0.0, 0.5);
} else {
@@ -290,15 +310,41 @@
}
last_box_distance_ = clipped_box_distance;
+
+ if (output) {
+ flatbuffers::Offset<IntakeVoltage> left_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &left_intake_output);
+ flatbuffers::Offset<IntakeVoltage> right_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &right_intake_output);
+
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_left_intake(left_intake_offset);
+ output_builder.add_right_intake(right_intake_offset);
+ output_builder.add_voltage_proximal(voltage_proximal_output);
+ output_builder.add_voltage_distal(voltage_distal_output);
+ output_builder.add_release_arm_brake(release_arm_brake_output);
+ output_builder.add_claw_grabbed(claw_grabbed_output);
+
+ output_builder.add_hook_release(hook_release_output);
+ output_builder.add_forks_release(forks_release_output);
+ output_builder.add_voltage_winch(voltage_winch_output);
+
+ output->Send(output_builder.Finish());
+ }
+
+ status->Send(status_builder.Finish());
}
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();
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
- if (!new_status_light.Send()) {
+ 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/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 81b6d9d..78851df 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -4,36 +4,37 @@
#include <memory>
#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 "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2018.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
const intake::IntakeSide &intake_left() const { return intake_left_; }
const intake::IntakeSide &intake_right() const { return intake_right_; }
const arm::Arm &arm() const { return arm_; }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::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:
// Sends the status light message for the 3 colors provided.
@@ -41,7 +42,7 @@
::aos::Sender<::y2018::StatusLight> status_light_sender_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
intake::IntakeSide intake_left_;
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 049a406..0000000
--- a/y2018/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,222 +0,0 @@
-package y2018.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct IntakeSideStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Estimated position of the spring.
- float spring_position;
- // Estimated velocity of the spring in units/second.
- float spring_velocity;
-
- // Estimated position of the joint.
- float motor_position;
- // Estimated velocity of the joint in units/second.
- float motor_velocity;
-
- // Goal position of the joint.
- float goal_position;
- // Goal velocity of the joint in units/second.
- float goal_velocity;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // The voltage given last cycle;
- float delayed_voltage;
-
- // State of the estimator.
- .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IntakeGoal {
- double roller_voltage;
-
- // Goal angle in radians of the intake.
- // Zero radians is where the intake is pointing straight out, with positive
- // radians inward towards the cube.
- double left_intake_angle;
- double right_intake_angle;
-};
-
-struct IntakeElasticSensors {
- // Position of the motor end of the series elastic in radians.
- .frc971.PotAndAbsolutePosition motor_position;
-
- // Displacement of the spring in radians.
- double spring_angle;
-
- // False if the beam break sensor isn't triggered, true if the beam breaker is
- // triggered.
- bool beam_break;
-};
-
-struct ArmStatus {
- // State of the estimators.
- .frc971.PotAndAbsoluteEncoderEstimatorState proximal_estimator_state;
- .frc971.PotAndAbsoluteEncoderEstimatorState distal_estimator_state;
-
- // The node we are currently going to.
- uint32_t current_node;
- // Distance (in radians) to the end of the path.
- float path_distance_to_go;
- // Goal position and velocity (radians)
- float goal_theta0;
- float goal_theta1;
- float goal_omega0;
- float goal_omega1;
-
- // Current position and velocity (radians)
- float theta0;
- float theta1;
-
- float omega0;
- float omega1;
-
- // Estimated voltage error for the two joints.
- float voltage_error0;
- float voltage_error1;
-
- // True if we are zeroed.
- bool zeroed;
-
- // True if the arm is zeroed.
- bool estopped;
-
- // The current state machine state.
- uint32_t state;
-
- uint32_t grab_state;
-
- // The number of times the LQR solver failed.
- uint32_t failed_solutions;
-};
-
-struct ArmPosition {
- // Values of the encoder and potentiometer at the base of the proximal
- // (connected to drivebase) arm in radians.
- .frc971.PotAndAbsolutePosition proximal;
-
- // Values of the encoder and potentiometer at the base of the distal
- // (connected to proximal) arm in radians.
- .frc971.PotAndAbsolutePosition distal;
-};
-
-struct IntakeVoltage {
- // Voltage of the motors on the series elastic on one side (left or right) of
- // the intake.
- double voltage_elastic;
-
- // Voltage of the rollers on one side (left or right) of the intake.
- double voltage_rollers;
-};
-
-// Published on ".y2018.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identiy a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2018/control_loops/superstructure/superstructure_goal.fbs b/y2018/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..b618c1b
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,39 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeGoal {
+ roller_voltage:double;
+
+ // Goal angle in radians of the intake.
+ // Zero radians is where the intake is pointing straight out, with positive
+ // radians inward towards the cube.
+ left_intake_angle:double;
+ right_intake_angle:double;
+}
+
+table Goal {
+ intake:IntakeGoal;
+
+ // Used to identiy a position in the planned set of positions on the arm.
+ arm_goal_position:uint;
+ // If true, start the grab box sequence.
+ grab_box:bool;
+
+ open_claw:bool;
+ close_claw:bool;
+
+ deploy_fork:bool;
+
+ hook_release:bool;
+
+ voltage_winch:double;
+
+ open_threshold:double;
+
+ disable_box_correct:bool;
+
+ trajectory_override:bool;
+}
+
+root_type Goal;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 162db33..988655f 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,8 +6,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -15,8 +13,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -71,9 +69,18 @@
zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(IntakeElasticSensors *position) {
- pot_encoder_.GetSensorValues(&position->motor_position);
- position->spring_angle = plant_.Y(0);
+ flatbuffers::Offset<IntakeElasticSensors> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder motor_position_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> motor_position_offset =
+ pot_encoder_.GetSensorValues(&motor_position_builder);
+
+ IntakeElasticSensors::Builder intake_elastic_sensors_builder(*fbb);
+
+ intake_elastic_sensors_builder.add_motor_position(motor_position_offset);
+ intake_elastic_sensors_builder.add_spring_angle(plant_.Y(0));
+
+ return intake_elastic_sensors_builder.Finish();
}
double spring_position() const { return plant_.X(0); }
@@ -85,14 +92,14 @@
plant_.set_voltage_offset(voltage_offset);
}
- void Simulate(const IntakeVoltage &intake_voltage) {
+ void Simulate(const IntakeVoltage *intake_voltage) {
const double voltage_check =
superstructure::intake::IntakeSide::kOperatingVoltage();
- AOS_CHECK_LE(::std::abs(intake_voltage.voltage_elastic), voltage_check);
+ AOS_CHECK_LE(::std::abs(intake_voltage->voltage_elastic()), voltage_check);
::Eigen::Matrix<double, 1, 1> U;
- U << intake_voltage.voltage_elastic + plant_.voltage_offset();
+ U << intake_voltage->voltage_elastic() + plant_.voltage_offset();
plant_.Update(U);
@@ -141,9 +148,21 @@
distal_zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(ArmPosition *position) {
- proximal_pot_encoder_.GetSensorValues(&position->proximal);
- distal_pot_encoder_.GetSensorValues(&position->distal);
+ flatbuffers::Offset<ArmPosition> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder proximal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
+ proximal_pot_encoder_.GetSensorValues(&proximal_builder);
+
+ frc971::PotAndAbsolutePosition::Builder distal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
+ distal_pot_encoder_.GetSensorValues(&distal_builder);
+
+ ArmPosition::Builder arm_position_builder(*fbb);
+ arm_position_builder.add_proximal(proximal_offset);
+ arm_position_builder.add_distal(distal_offset);
+
+ return arm_position_builder.Finish();
}
double proximal_position() const { return X_(0, 0); }
@@ -184,7 +203,6 @@
PositionSensorSimulator distal_pot_encoder_;
};
-
class SuperstructureSimulation {
public:
SuperstructureSimulation(::aos::EventLoop *event_loop)
@@ -198,14 +216,14 @@
arm_(constants::GetValues().arm_proximal.zeroing,
constants::GetValues().arm_distal.zeroing),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure.position")),
+ event_loop_->MakeSender<superstructure::Position>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")) {
+ event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")) {
// Start the intake out in the middle by default.
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
@@ -235,13 +253,20 @@
}
void SendPositionMessage() {
- auto position = superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
- left_intake_.GetSensorValues(&position->left_intake);
- right_intake_.GetSensorValues(&position->right_intake);
- arm_.GetSensorValues(&position->arm);
- AOS_LOG_STRUCT(INFO, "sim position", *position);
- position.Send();
+ flatbuffers::Offset<IntakeElasticSensors> left_intake_offset =
+ left_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<IntakeElasticSensors> right_intake_offset =
+ right_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<ArmPosition> arm_offset =
+ arm_.GetSensorValues(builder.fbb());
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_left_intake(left_intake_offset);
+ position_builder.add_right_intake(right_intake_offset);
+ position_builder.add_arm(arm_offset);
+ EXPECT_TRUE(builder.Send(position_builder.Finish()));
}
// Sets the difference between the commanded and applied powers.
@@ -263,13 +288,13 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- left_intake_.Simulate(superstructure_output_fetcher_->left_intake);
- right_intake_.Simulate(superstructure_output_fetcher_->right_intake);
+ left_intake_.Simulate(superstructure_output_fetcher_->left_intake());
+ right_intake_.Simulate(superstructure_output_fetcher_->right_intake());
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
- << superstructure_output_fetcher_->voltage_proximal,
- superstructure_output_fetcher_->voltage_distal)
+ << superstructure_output_fetcher_->voltage_proximal(),
+ superstructure_output_fetcher_->voltage_distal())
.finished(),
- superstructure_output_fetcher_->release_arm_brake);
+ superstructure_output_fetcher_->release_arm_brake());
}
private:
@@ -280,9 +305,9 @@
IntakeSideSimulation right_intake_;
ArmSimulation arm_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
bool first_ = true;
};
@@ -290,23 +315,24 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(::std::chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2018/config.json"),
+ ::std::chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeFetcher<superstructure::Goal>(
+ "/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeSender<superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ test_event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")),
+ test_event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
- superstructure_(superstructure_event_loop_.get(),
- ".y2018.control_loops.superstructure"),
+ superstructure_(superstructure_event_loop_.get(), "/superstructure"),
superstructure_plant_event_loop_(MakeEventLoop()),
superstructure_plant_(superstructure_plant_event_loop_.get()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
@@ -319,28 +345,30 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// Left side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->left_intake_angle(),
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->left_intake_angle(),
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
- superstructure_status_fetcher_->right_intake.spring_position +
- superstructure_status_fetcher_->right_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->right_intake_angle(),
+ superstructure_status_fetcher_->right_intake()->spring_position() +
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->right_intake_angle(),
superstructure_plant_.right_intake().spring_position(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -360,8 +388,10 @@
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_right().state());
- EXPECT_EQ(superstructure_output_fetcher_->left_intake.voltage_elastic, 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->right_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->left_intake()->voltage_elastic(),
+ 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->right_intake()->voltage_elastic(),
+ 0.0);
}
// Tests that the intake loop can reach a goal.
@@ -369,14 +399,20 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -393,14 +429,20 @@
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -415,43 +457,58 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 5.0 * M_PI;
- goal->intake.right_intake_angle = 5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
- EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
+ EXPECT_NEAR(
+ constants::Values::kIntakeRange().upper,
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->right_intake.motor_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = -5.0 * M_PI;
- goal->intake.right_intake_angle = -5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(-5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -460,13 +517,17 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->left_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->right_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
}
@@ -475,19 +536,40 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().lower_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().lower);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().lower);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 1.0;
- goal->intake.right_intake_angle = 1.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(1.0);
+ intake_goal_builder.add_right_intake_angle(1.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset =
+ intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -499,12 +581,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -517,14 +609,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->intake.right_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -558,12 +658,20 @@
RunFor(chrono::seconds(5));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = -0.8;
- goal->intake.right_intake_angle = -0.8;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-0.8);
+ intake_goal_builder.add_right_intake_angle(-0.8);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
EXPECT_EQ(arm::Arm::State::RUNNING, superstructure_.arm().state());
@@ -573,12 +681,20 @@
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -586,12 +702,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -604,12 +728,20 @@
superstructure_plant_.InitializeArmPosition(arm::ReadyAboveBoxPoint());
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::BackLowBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::BackLowBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -617,12 +749,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
index 789f13f..3200ead 100644
--- a/y2018/control_loops/superstructure/superstructure_main.cc
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2018/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() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2018::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2018/control_loops/superstructure/superstructure_output.fbs b/y2018/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..f4d62da
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,40 @@
+namespace y2018.control_loops.superstructure;
+
+table IntakeVoltage {
+ // Voltage of the motors on the series elastic on one side (left or right) of
+ // the intake.
+ voltage_elastic:double;
+
+ // Voltage of the rollers on one side (left or right) of the intake.
+ voltage_rollers:double;
+}
+
+table Output {
+ // Voltage sent to the parts on the left side of the intake.
+ left_intake:IntakeVoltage;
+
+ // Voltage sent to the parts on the right side of the intake.
+ right_intake:IntakeVoltage;
+
+ // Voltage sent to the motors on the proximal joint of the arm.
+ voltage_proximal:double;
+
+ // Voltage sent to the motors on the distal joint of the arm.
+ voltage_distal:double;
+
+ // Voltage sent to the hanger. Positive pulls the robot up.
+ voltage_winch:double;
+
+ // Clamped (when true) or unclamped (when false) status sent to the
+ // pneumatic claw on the arm.
+ claw_grabbed:bool;
+
+ // If true, release the arm brakes.
+ release_arm_brake:bool;
+ // If true, release the hook
+ hook_release:bool;
+ // If true, release the forks
+ forks_release:bool;
+}
+
+root_type Output;
diff --git a/y2018/control_loops/superstructure/superstructure_position.fbs b/y2018/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..0323b78
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,50 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeElasticSensors {
+ // Position of the motor end of the series elastic in radians.
+ motor_position:frc971.PotAndAbsolutePosition;
+
+ // Displacement of the spring in radians.
+ spring_angle:double;
+
+ // False if the beam break sensor isn't triggered, true if the beam breaker is
+ // triggered.
+ beam_break:bool;
+}
+
+table ArmPosition {
+ // Values of the encoder and potentiometer at the base of the proximal
+ // (connected to drivebase) arm in radians.
+ proximal:frc971.PotAndAbsolutePosition;
+
+ // Values of the encoder and potentiometer at the base of the distal
+ // (connected to proximal) arm in radians.
+ distal:frc971.PotAndAbsolutePosition;
+}
+
+
+table Position {
+ // Values of the series elastic encoders on the left side of the robot from
+ // the rear perspective in radians.
+ left_intake:IntakeElasticSensors;
+
+ // Values of the series elastic encoders on the right side of the robot from
+ // the rear perspective in radians.
+ right_intake:IntakeElasticSensors;
+
+ arm:ArmPosition;
+
+ // Value of the beam breaker sensor. This value is true if the beam is
+ // broken, false if the beam isn't broken.
+ claw_beambreak_triggered:bool;
+ // Value of the beambreak sensor detecting when the box has hit the frame
+ // cutout.
+ box_back_beambreak_triggered:bool;
+
+ // Distance to the box in meters.
+ box_distance:double;
+}
+
+root_type Position;
diff --git a/y2018/control_loops/superstructure/superstructure_status.fbs b/y2018/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..af2d1ab
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,98 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeSideStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Estimated position of the spring.
+ spring_position:float;
+ // Estimated velocity of the spring in units/second.
+ spring_velocity:float;
+
+ // Estimated position of the joint.
+ motor_position:float;
+ // Estimated velocity of the joint in units/second.
+ motor_velocity:float;
+
+ // Goal position of the joint.
+ goal_position:float;
+ // Goal velocity of the joint in units/second.
+ goal_velocity:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // The voltage given last cycle;
+ delayed_voltage:float;
+
+ // State of the estimator.
+ estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table ArmStatus {
+ // State of the estimators.
+ proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+ distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+
+ // The node we are currently going to.
+ current_node:uint;
+ // Distance (in radians) to the end of the path.
+ path_distance_to_go:float;
+ // Goal position and velocity (radians)
+ goal_theta0:float;
+ goal_theta1:float;
+ goal_omega0:float;
+ goal_omega1:float;
+
+ // Current position and velocity (radians)
+ theta0:float;
+ theta1:float;
+
+ omega0:float;
+ omega1:float;
+
+ // Estimated voltage error for the two joints.
+ voltage_error0:float;
+ voltage_error1:float;
+
+ // True if we are zeroed.
+ zeroed:bool;
+
+ // True if the arm is zeroed.
+ estopped:bool;
+
+ // The current state machine state.
+ state:uint;
+
+ grab_state:uint;
+
+ // The number of times the LQR solver failed.
+ failed_solutions:uint;
+}
+
+table Status {
+ // Are all the subsystems zeroed?
+ zeroed:bool;
+
+ // If true, any of the subsystems have aborted.
+ estopped:bool;
+
+ // Status of both intake sides.
+ left_intake:IntakeSideStatus;
+ right_intake:IntakeSideStatus;
+
+ arm:ArmStatus;
+
+ filtered_box_velocity:double;
+ rotation_state:uint;
+}
+
+root_type Status;
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 54c5f8d..c9778b8 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -6,22 +6,22 @@
#include <google/protobuf/stubs/stringprintf.h>
#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "aos/input/drivetrain_input.h"
-#include "aos/input/action_joystick_input.h"
-#include "aos/init.h"
#include "aos/vision/events/udp.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
#include "y2018/vision.pb.h"
using ::aos::monotonic_clock;
@@ -95,15 +95,15 @@
{}),
superstructure_position_fetcher_(
event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Position>(
+ ::y2018::control_loops::superstructure::Position>(
".frc971.control_loops.superstructure_queue.position")),
superstructure_status_fetcher_(
event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Status>(
+ ::y2018::control_loops::superstructure::Status>(
".frc971.control_loops.superstructure_queue.status")),
superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
+ ->MakeSender<::y2018::control_loops::superstructure::Goal>(
".frc971.control_loops.superstructure_queue.goal")) {
const uint16_t team = ::aos::network::GetTeamNumber();
@@ -120,10 +120,10 @@
return;
}
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->intake.left_intake_angle = intake_goal_;
- new_superstructure_goal->intake.right_intake_angle = intake_goal_;
+ double roller_voltage = 0.0;
+ bool trajectory_override = false;
if (data.PosEdge(kIntakeIn) || data.PosEdge(kSmallBox) ||
data.PosEdge(kIntakeClosed)) {
@@ -132,22 +132,22 @@
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage = 8.0;
+ roller_voltage = 8.0;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
- new_superstructure_goal->intake.roller_voltage = -12.0;
+ roller_voltage = -12.0;
} else {
// We don't want the rollers on.
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ roller_voltage = 0.0;
}
if (data.IsPressed(kSmallBox)) {
// Deploy the intake.
- if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered()) {
intake_goal_ = 0.30;
} else {
- if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
- superstructure_position_fetcher_->box_distance < 0.15) {
+ if (roller_voltage > 0.1 &&
+ superstructure_position_fetcher_->box_distance() < 0.15) {
intake_goal_ = 0.18;
} else {
intake_goal_ = -0.60;
@@ -155,22 +155,22 @@
}
} else if (data.IsPressed(kIntakeClosed)) {
// Deploy the intake.
- if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered()) {
intake_goal_ = 0.30;
} else {
- if (new_superstructure_goal->intake.roller_voltage > 0.1) {
- if (superstructure_position_fetcher_->box_distance < 0.10) {
- new_superstructure_goal->intake.roller_voltage -= 3.0;
+ if (roller_voltage > 0.1) {
+ if (superstructure_position_fetcher_->box_distance() < 0.10) {
+ roller_voltage -= 3.0;
intake_goal_ = 0.22;
- } else if (superstructure_position_fetcher_->box_distance < 0.17) {
+ } else if (superstructure_position_fetcher_->box_distance() < 0.17) {
intake_goal_ = 0.13;
- } else if (superstructure_position_fetcher_->box_distance < 0.25) {
+ } else if (superstructure_position_fetcher_->box_distance() < 0.25) {
intake_goal_ = 0.05;
} else {
intake_goal_ = -0.10;
}
if (robot_velocity() < -0.1 &&
- superstructure_position_fetcher_->box_distance > 0.15) {
+ superstructure_position_fetcher_->box_distance() > 0.15) {
intake_goal_ += 0.10;
}
} else {
@@ -182,18 +182,19 @@
intake_goal_ = -3.20;
}
- if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
+ if (roller_voltage > 0.1 &&
intake_goal_ > 0.0) {
- if (superstructure_position_fetcher_->box_distance < 0.10) {
- new_superstructure_goal->intake.roller_voltage -= 3.0;
+ if (superstructure_position_fetcher_->box_distance() < 0.10) {
+ roller_voltage -= 3.0;
}
- new_superstructure_goal->intake.roller_voltage += 3.0;
+ roller_voltage += 3.0;
}
// If we are disabled, stay at the node closest to where we start. This
// should remove long motions when enabled.
if (!data.GetControlBit(ControlBit::kEnabled) || never_disabled_) {
- arm_goal_position_ = superstructure_status_fetcher_->arm.current_node;
+ arm_goal_position_ =
+ superstructure_status_fetcher_->arm()->current_node();
never_disabled_ = false;
}
@@ -202,9 +203,9 @@
grab_box = true;
}
const bool near_goal =
- superstructure_status_fetcher_->arm.current_node ==
+ superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go < 1e-3;
+ superstructure_status_fetcher_->arm()->path_distance_to_go() < 1e-3;
if (data.IsPressed(kArmStepDown) && near_goal) {
uint32_t *front_point = ::std::find(
front_points_.begin(), front_points_.end(), arm_goal_position_);
@@ -291,50 +292,65 @@
if (data.IsPressed(kEmergencyDown)) {
arm_goal_position_ = arm::NeutralIndex();
- new_superstructure_goal->trajectory_override = true;
+ trajectory_override = true;
} else if (data.IsPressed(kEmergencyUp)) {
arm_goal_position_ = arm::UpIndex();
- new_superstructure_goal->trajectory_override = true;
+ trajectory_override = true;
} else {
- new_superstructure_goal->trajectory_override = false;
+ trajectory_override = false;
}
- new_superstructure_goal->deploy_fork =
+ const bool deploy_fork =
data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
- if (new_superstructure_goal->deploy_fork) {
+ if (deploy_fork) {
intake_goal_ = -2.0;
}
+ control_loops::superstructure::IntakeGoal::Builder intake_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::IntakeGoal>();
+
+ intake_goal_builder.add_left_intake_angle(intake_goal_);
+ intake_goal_builder.add_right_intake_angle(intake_goal_);
+ intake_goal_builder.add_roller_voltage(roller_voltage);
+
+ flatbuffers::Offset<control_loops::superstructure::IntakeGoal>
+ intake_goal_offset = intake_goal_builder.Finish();
+
+ control_loops::superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_goal_offset);
+ superstructure_builder.add_grab_box(grab_box);
+ superstructure_builder.add_trajectory_override(trajectory_override);
+ superstructure_builder.add_deploy_fork(deploy_fork);
+
if (data.IsPressed(kWinch)) {
AOS_LOG(INFO, "Winching\n");
- new_superstructure_goal->voltage_winch = 12.0;
+ superstructure_builder.add_voltage_winch(12.0);
} else {
- new_superstructure_goal->voltage_winch = 0.0;
+ superstructure_builder.add_voltage_winch(0.0);
}
- new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
+ superstructure_builder.add_hook_release(data.IsPressed(kArmBelowHang));
- new_superstructure_goal->arm_goal_position = arm_goal_position_;
+ superstructure_builder.add_arm_goal_position(arm_goal_position_);
if (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)) {
- new_superstructure_goal->open_threshold = 0.35;
+ superstructure_builder.add_open_threshold(0.35);
} else {
- new_superstructure_goal->open_threshold = 0.0;
+ superstructure_builder.add_open_threshold(0.0);
}
if ((data.IsPressed(kClawOpen) && data.IsPressed(kDriverClawOpen)) ||
data.PosEdge(kArmPickupBoxFromIntake) ||
(data.IsPressed(kClawOpen) &&
(data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)))) {
- new_superstructure_goal->open_claw = true;
+ superstructure_builder.add_open_claw(true);
} else {
- new_superstructure_goal->open_claw = false;
+ superstructure_builder.add_open_claw(false);
}
- new_superstructure_goal->grab_box = grab_box;
-
- AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
@@ -347,11 +363,11 @@
return mode();
}
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Position>
+ ::aos::Fetcher<control_loops::superstructure::Position>
superstructure_position_fetcher_;
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<control_loops::superstructure::Goal>
superstructure_goal_sender_;
// Current goals to send to the robot.
@@ -375,7 +391,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());
::y2018::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2018/status_light.fbs b/y2018/status_light.fbs
new file mode 100644
index 0000000..e51decb
--- /dev/null
+++ b/y2018/status_light.fbs
@@ -0,0 +1,10 @@
+namespace y2018;
+
+table StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ red:float;
+ green:float;
+ blue:float;
+}
+
+root_type StatusLight;
diff --git a/y2018/status_light.q b/y2018/status_light.q
deleted file mode 100644
index 90d4eec..0000000
--- a/y2018/status_light.q
+++ /dev/null
@@ -1,8 +0,0 @@
-package y2018;
-
-message StatusLight {
- // How bright to make each one. 0 is off, 1 is full on.
- float red;
- float green;
- float blue;
-};
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index 8e5e950..7bcd402 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -1,4 +1,4 @@
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_binary(
name = "image_streamer",
@@ -17,11 +17,12 @@
],
)
-queue_library(
- name = "vision_queue",
+flatbuffer_cc_library(
+ name = "vision_fbs",
srcs = [
- "vision.q",
+ "vision.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -32,11 +33,10 @@
],
visibility = ["//visibility:public"],
deps = [
- ":vision_queue",
+ ":vision_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
"//aos/vision/events:udp",
"//y2018:vision_proto",
diff --git a/y2018/vision/vision.fbs b/y2018/vision/vision.fbs
new file mode 100644
index 0000000..14f8a45
--- /dev/null
+++ b/y2018/vision/vision.fbs
@@ -0,0 +1,9 @@
+namespace y2018.vision;
+
+// Published on ".y2018.vision.vision_status"
+table VisionStatus {
+ high_frame_count:uint;
+ low_frame_count:uint;
+}
+
+root_type VisionStatus;
diff --git a/y2018/vision/vision.q b/y2018/vision/vision.q
deleted file mode 100644
index fb59d69..0000000
--- a/y2018/vision/vision.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2018.vision;
-
-// Published on ".y2018.vision.vision_status"
-message VisionStatus {
- uint32_t high_frame_count;
- uint32_t low_frame_count;
-};
diff --git a/y2018/vision/vision_status.cc b/y2018/vision/vision_status.cc
index 2f358c5..fdd6ffe 100644
--- a/y2018/vision/vision_status.cc
+++ b/y2018/vision/vision_status.cc
@@ -1,13 +1,12 @@
#include <netdb.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/vision/events/udp.h"
#include "y2018/vision.pb.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace vision {
@@ -15,23 +14,28 @@
using aos::monotonic_clock;
int Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
::aos::events::RXUdpSocket video_rx(5001);
char data[65507];
::y2018::VisionStatus status;
- ::aos::ShmEventLoop event_loop;
- ::aos::Sender<::y2018::vision::VisionStatus> vision_status_sender_ =
- event_loop.MakeSender<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::aos::Sender<VisionStatus> vision_status_sender_ =
+ event_loop.MakeSender<VisionStatus>("/superstructure");
while (true) {
const ssize_t rx_size = video_rx.Recv(data, sizeof(data));
if (rx_size > 0) {
status.ParseFromArray(data, rx_size);
- auto new_vision_status = vision_status_sender_.MakeMessage();
- new_vision_status->high_frame_count = status.high_frame_count();
- new_vision_status->low_frame_count = status.low_frame_count();
- AOS_LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
+
+ auto builder = vision_status_sender_.MakeBuilder();
+ VisionStatus::Builder vision_status_builder =
+ builder.MakeBuilder<VisionStatus>();
+ vision_status_builder.add_high_frame_count(status.high_frame_count());
+ vision_status_builder.add_low_frame_count(status.low_frame_count());
+ if (!builder.Send(vision_status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send vision information\n");
}
}
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index b93d2b0..6fc4828 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -21,19 +21,17 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -42,25 +40,26 @@
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2018::control_loops::SuperstructureQueue;
-using ::y2018::constants::Values;
-using ::aos::monotonic_clock;
-namespace chrono = ::std::chrono;
using aos::make_unique;
+using ::aos::monotonic_clock;
+using ::y2018::constants::Values;
+namespace chrono = ::std::chrono;
+namespace superstructure = ::y2018::control_loops::superstructure;
namespace y2018 {
namespace wpilib {
@@ -146,12 +145,12 @@
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure_queue.position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -272,24 +271,27 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->left_shifter_position =
- drivetrain_shifter_pot_translate(
- left_drivetrain_shifter_->GetVoltage());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed =
- -drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
- drivetrain_message->right_shifter_position =
+ drivetrain_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed (
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ drivetrain_builder.add_left_shifter_position (
drivetrain_shifter_pot_translate(
- right_drivetrain_shifter_->GetVoltage());
+ left_drivetrain_shifter_->GetVoltage()));
- drivetrain_message.Send();
+ drivetrain_builder.add_right_encoder (
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed (
+ -drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod()));
+ drivetrain_builder.add_right_shifter_position (
+ drivetrain_shifter_pot_translate(
+ right_drivetrain_shifter_->GetVoltage()));
+
+ builder.Send(drivetrain_builder.Finish());
}
}
@@ -297,57 +299,111 @@
const auto values = constants::GetValues();
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
+ auto builder =
+ superstructure_position_sender_.MakeBuilder();
- CopyPosition(proximal_encoder_, &superstructure_message->arm.proximal,
+ // Proximal arm
+ frc971::PotAndAbsolutePositionT arm_proximal;
+ CopyPosition(proximal_encoder_, &arm_proximal,
Values::kProximalEncoderCountsPerRevolution(),
Values::kProximalEncoderRatio(), proximal_pot_translate,
true, values.arm_proximal.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_proximal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_proximal);
- CopyPosition(distal_encoder_, &superstructure_message->arm.distal,
+ // Distal arm
+ frc971::PotAndAbsolutePositionT arm_distal;
+ CopyPosition(distal_encoder_, &arm_distal,
Values::kDistalEncoderCountsPerRevolution(),
Values::kDistalEncoderRatio(), distal_pot_translate, true,
values.arm_distal.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_distal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_distal);
+ superstructure::ArmPosition::Builder arm_position_builder =
+ builder.MakeBuilder<superstructure::ArmPosition>();
+ arm_position_builder.add_proximal(arm_proximal_offset);
+ arm_position_builder.add_distal(arm_distal_offset);
+
+ flatbuffers::Offset<superstructure::ArmPosition> arm_position_offset =
+ arm_position_builder.Finish();
+
+ // Left intake
+ frc971::PotAndAbsolutePositionT left_intake_motor_position;
CopyPosition(left_intake_encoder_,
- &superstructure_message->left_intake.motor_position,
+ &left_intake_motor_position,
Values::kIntakeMotorEncoderCountsPerRevolution(),
Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
false, values.left_intake.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition>
+ left_intake_motor_position_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(),
+ &left_intake_motor_position);
+ // Right intake
+ frc971::PotAndAbsolutePositionT right_intake_motor_position;
CopyPosition(right_intake_encoder_,
- &superstructure_message->right_intake.motor_position,
+ &right_intake_motor_position,
Values::kIntakeMotorEncoderCountsPerRevolution(),
Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
true, values.right_intake.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition>
+ right_intake_motor_position_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(),
+ &right_intake_motor_position);
- superstructure_message->left_intake.spring_angle =
+ superstructure::IntakeElasticSensors::Builder
+ left_intake_sensors_builder =
+ builder.MakeBuilder<superstructure::IntakeElasticSensors>();
+
+ left_intake_sensors_builder.add_motor_position(
+ left_intake_motor_position_offset);
+ left_intake_sensors_builder.add_spring_angle(
intake_spring_translate(left_intake_spring_angle_->GetVoltage()) +
- values.left_intake.spring_offset;
- superstructure_message->left_intake.beam_break =
- !left_intake_cube_detector_->Get();
+ values.left_intake.spring_offset);
+ left_intake_sensors_builder.add_beam_break(
+ !left_intake_cube_detector_->Get());
- superstructure_message->right_intake.spring_angle =
+ flatbuffers::Offset<superstructure::IntakeElasticSensors>
+ left_intake_offset = left_intake_sensors_builder.Finish();
+
+ superstructure::IntakeElasticSensors::Builder
+ right_intake_sensors_builder =
+ builder.MakeBuilder<superstructure::IntakeElasticSensors>();
+
+ right_intake_sensors_builder.add_motor_position(
+ right_intake_motor_position_offset);
+ right_intake_sensors_builder.add_spring_angle(
-intake_spring_translate(right_intake_spring_angle_->GetVoltage()) +
- values.right_intake.spring_offset;
- superstructure_message->right_intake.beam_break =
- !right_intake_cube_detector_->Get();
+ values.right_intake.spring_offset);
+ right_intake_sensors_builder.add_beam_break(
+ !right_intake_cube_detector_->Get());
- superstructure_message->claw_beambreak_triggered = !claw_beambreak_->Get();
- superstructure_message->box_back_beambreak_triggered =
- !box_back_beambreak_->Get();
+ flatbuffers::Offset<control_loops::superstructure::IntakeElasticSensors>
+ right_intake_offset = right_intake_sensors_builder.Finish();
- superstructure_message->box_distance =
- lidar_lite_.last_width() / 0.00001 / 100.0 / 2;
+ superstructure::Position::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Position>();
- superstructure_message.Send();
+ superstructure_builder.add_left_intake(left_intake_offset);
+ superstructure_builder.add_right_intake(right_intake_offset);
+ superstructure_builder.add_arm(arm_position_offset);
+
+ superstructure_builder.add_claw_beambreak_triggered(
+ !claw_beambreak_->Get());
+ superstructure_builder.add_box_back_beambreak_triggered(
+ !box_back_beambreak_->Get());
+
+ superstructure_builder.add_box_distance(lidar_lite_.last_width() /
+ 0.00001 / 100.0 / 2);
+
+ builder.Send(superstructure_builder.Finish());
}
}
private:
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::std::unique_ptr<frc::AnalogInput> left_drivetrain_shifter_,
@@ -378,16 +434,16 @@
: event_loop_(event_loop),
drivetrain_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
superstructure_fetcher_(
- event_loop->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure_queue.output")),
- status_light_fetcher_(event_loop->MakeFetcher<::y2018::StatusLight>(
- ".y2018.status_light")),
+ event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
+ status_light_fetcher_(
+ event_loop->MakeFetcher<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
- event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ event_loop->MakeFetcher<::y2018::vision::VisionStatus>("/vision")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")),
pcm_(pcm) {
event_loop_->set_name("Solenoids");
event_loop_->SetRuntimeRealtimePriority(27);
@@ -449,40 +505,40 @@
{
drivetrain_fetcher_.Fetch();
if (drivetrain_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
- left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high);
+ left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high());
+ right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high());
}
}
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- claw_->Set(!superstructure_fetcher_->claw_grabbed);
- arm_brakes_->Set(superstructure_fetcher_->release_arm_brake);
- hook_->Set(superstructure_fetcher_->hook_release);
- forks_->Set(superstructure_fetcher_->forks_release);
+ claw_->Set(!superstructure_fetcher_->claw_grabbed());
+ arm_brakes_->Set(superstructure_fetcher_->release_arm_brake());
+ hook_->Set(superstructure_fetcher_->hook_release());
+ forks_->Set(superstructure_fetcher_->forks_release());
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
+ StatusLightT color;
if (!status_light_fetcher_.get() ||
- monotonic_now >
- status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
- StatusLight color;
+ monotonic_now > status_light_fetcher_.context().monotonic_sent_time +
+ chrono::milliseconds(100)) {
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
@@ -493,7 +549,8 @@
color.red = 0.5;
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
color.red = 0.5;
color.green = 0.5;
}
@@ -501,16 +558,13 @@
if (light_flash_ > 20) {
light_flash_ = 0;
}
-
- AOS_LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
} else {
- AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_);
- SetColor(*status_light_fetcher_);
+ status_light_fetcher_->UnPackTo(&color);
}
+ SetColor(color);
}
- void SetColor(const StatusLight &status_light) {
+ void SetColor(const StatusLightT &status_light) {
// Save CAN bandwidth and CPU at the cost of RT. Only change the light when
// it actually changes. This is pretty low priority anyways.
static int time_since_last_send = 0;
@@ -541,12 +595,14 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_fetcher_;
::aos::Fetcher<::y2018::StatusLight> status_light_fetcher_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
+
::frc971::wpilib::BufferedPcm *pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
@@ -567,11 +623,11 @@
};
class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
- event_loop, ".y2018.control_loops.superstructure_queue.output") {}
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure") {}
void set_proximal_victor(::std::unique_ptr<::frc::VictorSP> t) {
proximal_victor_ = ::std::move(t);
@@ -600,40 +656,38 @@
}
private:
- virtual void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
-
+ virtual void Write(const superstructure::Output &output) override {
left_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(-output.left_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake()->voltage_elastic(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(output.right_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake()->voltage_elastic(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
left_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(-output.left_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake()->voltage_rollers(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(output.right_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake()->voltage_rollers(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
- proximal_victor_->SetSpeed(::aos::Clip(-output.voltage_proximal,
+ proximal_victor_->SetSpeed(::aos::Clip(-output.voltage_proximal(),
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- distal_victor_->SetSpeed(::aos::Clip(output.voltage_distal,
+ distal_victor_->SetSpeed(::aos::Clip(output.voltage_distal(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- hanger_victor_->SetSpeed(
- ::aos::Clip(-output.voltage_winch, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ hanger_victor_->SetSpeed(::aos::Clip(-output.voltage_winch(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
}
virtual void Stop() override {
@@ -663,19 +717,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_left_drivetrain_shifter_potentiometer(
@@ -721,7 +778,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
auto imu_trigger = make_unique<frc::DigitalInput>(5);
::frc971::wpilib::ADIS16448 imu(&imu_event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
@@ -735,7 +792,7 @@
// variety so all the Victors are written as SPs.
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
@@ -764,7 +821,7 @@
// Thread 6.
// This is a separate event loop because we want to run it at much lower
// priority.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::frc971::wpilib::BufferedPcm pcm;
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, &pcm);
solenoid_writer.set_left_drivetrain_shifter(pcm.MakeSolenoid(0));
diff --git a/y2018/y2018.json b/y2018/y2018.json
new file mode 100644
index 0000000..ebb6c75
--- /dev/null
+++ b/y2018/y2018.json
@@ -0,0 +1,44 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.StatusLight",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.vision.VisionStatus",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}
diff --git a/y2019/BUILD b/y2019/BUILD
index 04855d5..5c9a3a6 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,6 +1,7 @@
load("//frc971:downloader.bzl", "robot_downloader")
-load("//aos/build:queues.bzl", "queue_library")
+load("//aos:config.bzl", "aos_config")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
dirs = [
@@ -56,25 +57,24 @@
"//aos:make_unique",
"//aos:math",
"//aos/controls:control_loop",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
@@ -82,8 +82,9 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
- "//y2019/control_loops/drivetrain:camera_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/superstructure:superstructure_output_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
"//y2019/jevois:spi",
],
)
@@ -132,23 +133,45 @@
"//aos/time",
"//aos/util:log_interval",
"//aos/vision/events:udp",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
"//y2019/control_loops/drivetrain:drivetrain_base",
- "//y2019/control_loops/drivetrain:target_selector_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
"@com_google_protobuf//:protobuf",
],
)
-queue_library(
+flatbuffer_cc_library(
name = "status_light",
srcs = [
- "status_light.q",
+ "status_light.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+aos_config(
+ name = "config",
+ src = "y2019.json",
+ flatbuffers = [
+ ":status_light",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_output_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
)
cc_proto_library(
diff --git a/y2019/actors/BUILD b/y2019/actors/BUILD
index a622dbf..3aa2a22 100644
--- a/y2019/actors/BUILD
+++ b/y2019/actors/BUILD
@@ -25,16 +25,17 @@
"autonomous_actor.h",
],
deps = [
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
"//y2019/control_loops/drivetrain:drivetrain_base",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
)
@@ -46,7 +47,7 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
+ "//frc971/autonomous:auto_fbs",
],
)
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index 4e8e60d..6166ca8 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -1,397 +1,792 @@
#include "y2019/actors/auto_splines.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
namespace y2019 {
namespace actors {
-void MaybeFlipSpline(::frc971::MultiSpline *spline, bool is_left) {
+void MaybeFlipSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
+ bool is_left) {
+
+ flatbuffers::Vector<float> *spline_y =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+
if (!is_left) {
- for (size_t i = 0; i < spline->spline_y.size(); i++) {
- spline->spline_y[i] *= -1.0;
+ for (size_t i = 0; i < spline_y->size(); i++) {
+ spline_y->Mutate(i, -spline_y->Get(i));
}
}
}
// Path off of level 2 to the far side of the rocket with a panel
-::frc971::MultiSpline AutonomousSplines::HABToFarRocket(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 4.0;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 2;
- spline.spline_x = {{1.14763818102, 1.66, 3.10, 4.05, 4.45, 5.11, 5.77, 6.71,
- 7.27, 7.19, 6.57}};
- spline.spline_y = {{1.30261224364, 1.30217320136, 1.39, 1.47, 1.56346705393,
- 1.69, 1.81, 1.97, 2.18, 2.84, 3.33}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.14763818102, 1.66, 3.10, 4.05,
+ 4.45, 5.11, 5.77, 6.71, 7.27, 7.19,
+ 6.57});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.30261224364, 1.30217320136, 1.39,
+ 1.47, 1.56346705393, 1.69, 1.81,
+ 1.97, 2.18, 2.84, 3.33});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(2);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the far side of the rocket to the loading station to pickup
-::frc971::MultiSpline AutonomousSplines::FarRocketToHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(3.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 4.5;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{6.6, 7.511, 6.332, 4.590, 1.561, 0.179}};
- spline.spline_y = {{3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {6.6, 7.511, 6.332, 4.590, 1.561, 0.179});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the human player station to the far side of the rocket with a panel
-::frc971::MultiSpline AutonomousSplines::HPToFarRocket(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint velocity_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(3.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(3.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 2.0;
- velocity_constraint.start_distance = 7.0;
- velocity_constraint.end_distance = 15.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(7.0);
+ velocity_constraint_builder.add_end_distance(15.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{0.895115737979, 2.9155615909, 5.02361983866,
- 6.40346237218, 7.1260656844, 7.83907559509}};
- spline.spline_y = {{3.43030859063, 3.44230565037, 2.8824369646, 2.81000389973,
- 3.08853311072, 2.6933085577}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{lateral_constraint, velocity_constraint,
- voltage_constraint, longitudinal_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.895115737979, 2.9155615909,
+ 5.02361983866, 6.40346237218,
+ 7.1260656844, 7.83907559509});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.43030859063, 3.44230565037,
+ 2.8824369646, 2.81000389973,
+ 3.08853311072, 2.6933085577});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the far side of the rocket to close to the loading station
-::frc971::MultiSpline AutonomousSplines::FarRocketToNearHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
-
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
// TODO(theo): Add some real constraints.
- constraints.constraint_type = 0;
- constraints.value = 0;
- constraints.start_distance = 0;
- constraints.end_distance = 0;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({6.51652191988, 6.83156293562,
+ 5.74513904409, 2.2337653586,
+ 1.94766705864, 0.727526876557});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.2465107468, 2.88277456846,
+ 1.93458779243, 3.44064777429,
+ 3.44377880106, 3.43326367284});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.spline_count = 1;
- spline.spline_x = {{6.51652191988, 6.83156293562, 5.74513904409, 2.2337653586,
- 1.94766705864, 0.727526876557}};
- spline.spline_y = {{3.2465107468, 2.88277456846, 1.93458779243, 3.44064777429,
- 3.44377880106, 3.43326367284}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- spline.constraints = {{constraints}};
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ return multispline_builder.Finish();
}
// Path from level 2 to 2nd cargo ship bay with a hatch panel
-::frc971::MultiSpline AutonomousSplines::HABToSecondCargoShipBay(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToSecondCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- constraints.constraint_type = 4;
- constraints.value = 1.6;
- constraints.start_distance = 4.0;
- constraints.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(10.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(1.6);
+ velocity_constraint_builder.add_start_distance(4.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, voltage_constraint_offset,
+ velocity_constraint_offset});
+
constexpr double kLess = 0.06;
- spline.spline_x = {{1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
- 7.12318661548 - kLess, 7.22595029399 - kLess}};
- spline.spline_y = {{1.5, 1.48, 2.05178220103,
- 2.56666687655, 1.79340280288, 1.16170693058}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
+ 7.12318661548 - kLess, 7.22595029399 - kLess});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.5, 1.48, 2.05178220103,
+ 2.56666687655, 1.79340280288,
+ 1.16170693058});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {{constraints, longitudinal_constraint, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from 2nd cargo ship bay to loading station
-::frc971::MultiSpline AutonomousSplines::SecondCargoShipBayToHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SecondCargoShipBayToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- constraints.constraint_type = 4;
- constraints.value = 4.0;
- constraints.start_distance = 0;
- constraints.end_distance = 10;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{7.22595029399, 7.1892447864, 6.5373977907, 5.55997590982,
- 1.22953437637, 0.32521840905}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {voltage_constraint_offset, velocity_constraint_offset});
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({7.22595029399, 7.1892447864,
+ 6.5373977907, 5.55997590982,
+ 1.22953437637, 0.32521840905});
constexpr double kYShift = 0.1;
- spline.spline_y = {{1.2, 1.44543230529, 2.00646674662,
- 3.43762336271 - kYShift, 3.44125430793 - kYShift,
- 3.4360348159 - kYShift}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {1.2, 1.44543230529, 2.00646674662, 3.43762336271 - kYShift,
+ 3.44125430793 - kYShift, 3.4360348159 - kYShift});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {{constraints, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from loading station to 3rd cargo ship bay with a hatch panel
-::frc971::MultiSpline AutonomousSplines::HPToThirdCargoShipBay(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint velocity_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint2;
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 3.5;
- velocity_constraint.start_distance = 0;
- velocity_constraint.end_distance = 10;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToThirdCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint2_offset;
- velocity_constraint2.constraint_type = 4;
- velocity_constraint2.value = 2.0;
- velocity_constraint2.start_distance = 6;
- velocity_constraint2.end_distance = 10;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(10.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(3.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ {
+ frc971::Constraint::Builder velocity_constraint2_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint2_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint2_builder.add_value(2.0);
+ velocity_constraint2_builder.add_start_distance(6.0);
+ velocity_constraint2_builder.add_end_distance(10.0);
+ velocity_constraint2_offset = velocity_constraint2_builder.Finish();
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {voltage_constraint_offset, velocity_constraint_offset,
+ velocity_constraint2_offset});
+
constexpr double kEndMove = 0.25;
- spline.spline_x = {{0.75, 1.112, 5.576, 7.497 - kEndMove, 7.675 - kEndMove,
- 7.768 - kEndMove}};
- spline.spline_y = {{3.431, 3.434, 2.712, 2.874, 1.786, 1.168}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.75, 1.112, 5.576, 7.497 - kEndMove,
+ 7.675 - kEndMove, 7.768 - kEndMove});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {3.431, 3.434, 2.712, 2.874, 1.786, 1.168});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {
- {velocity_constraint, voltage_constraint, velocity_constraint2}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from 3rd cargo ship bay to near the loading station
-::frc971::MultiSpline AutonomousSplines::ThirdCargoShipBayToNearHP(
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::ThirdCargoShipBayToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint velocity_constraint;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 0;
- velocity_constraint.end_distance = 10;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{7.75823205276, 7.58356294646, 5.95536035287,
- 2.12377989323, 1.29347361128, 0.598613577531}};
- spline.spline_y = {{1.16791407107, 1.94564064915, 2.54565614767,
- 3.43728005786, 3.43775494434, 3.43119598027}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {velocity_constraint_offset});
- spline.constraints = {{velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({7.75823205276, 7.58356294646,
+ 5.95536035287, 2.12377989323,
+ 1.29347361128, 0.598613577531});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.16791407107, 1.94564064915,
+ 2.54565614767, 3.43728005786,
+ 3.43775494434, 3.43119598027});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::HabToFarRocketTest(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HabToFarRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 1.7;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 0.8;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(1.7);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(0.8);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{1.14763818102, 2.53944573074, 3.74586892131,
- 5.22352745444, 6.70255737219, 7.35784750785}};
- spline.spline_y = {{1.30261224364, 1.28295363394, 1.27450357714,
- 2.89953366429, 3.10734391012, 2.90125929705}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.14763818102, 2.53944573074,
+ 3.74586892131, 5.22352745444,
+ 6.70255737219, 7.35784750785});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.30261224364, 1.28295363394,
+ 1.27450357714, 2.89953366429,
+ 3.10734391012, 2.90125929705});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::FarRocketToHPTest() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHPTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.5;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.5);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 9.5;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(9.5);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
- spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
- return spline;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({6.53, 7.8, 7.8, 4.0, 2.0, 0.4});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.47, 3.0, 1.5, 3.0, 3.4, 3.4});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::HPToNearRocketTest() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToNearRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 2.7;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(2.7);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
- spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
- return spline;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.5, 2.0, 3.0, 4.0, 4.5, 5.12});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.4, 3.4, 3.4, 3.0, 3.0, 3.43});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::BasicSSpline() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 6.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(6.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset});
+
const float startx = 0.4;
const float starty = 3.4;
- spline.spline_x = {{0.0f + startx, 0.6f + startx, 0.6f + startx,
- 0.4f + startx, 0.4f + startx, 1.0f + startx}};
- spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f,
- starty - 0.7f, starty - 1.0f, starty - 1.0f}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+ 0.6f + startx, 0.4f + startx,
+ 0.4f + startx, 1.0f + startx});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+ starty - 0.3f, starty - 0.7f,
+ starty - 1.0f, starty - 1.0f});
- spline.constraints = {
- {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::StraightLine() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint contraints;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
- contraints.constraint_type = 0;
- contraints.value = 0.0;
- contraints.start_distance = 0.0;
- contraints.end_distance = 0.0;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- spline.spline_count = 1;
- spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}};
- spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}};
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
- spline.constraints = {{contraints}};
-
- return spline;
+ return multispline_builder.Finish();
}
} // namespace actors
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index a55c2f1..5d79ba9 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -1,7 +1,9 @@
#ifndef Y2019_ACTORS_AUTO_SPLINES_H_
#define Y2019_ACTORS_AUTO_SPLINES_H_
-#include "frc971/control_loops/control_loops.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
/*
The cooridinate system for the autonomous splines is the same as the spline
@@ -17,38 +19,60 @@
// Splines for 2 Panels on the far side of the Rocket
// Path off of level 2 to the far side of the rocket with a panel
- static ::frc971::MultiSpline HABToFarRocket(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HABToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to the loading station to pickup
- static ::frc971::MultiSpline FarRocketToHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to the loading station to pickup
- static ::frc971::MultiSpline HPToFarRocket(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HPToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to close to the loading station
- static ::frc971::MultiSpline FarRocketToNearHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Splines for 2 Panels on the far reaches of the cargo ship
// Path from level 2 to 2nd cargo ship bay with a hatch panel
- static ::frc971::MultiSpline HABToSecondCargoShipBay(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HABToSecondCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from 2nd cargo ship bay to loading station
- static ::frc971::MultiSpline SecondCargoShipBayToHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> SecondCargoShipBayToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from loading station to 3rd cargo ship bay with a hatch panel
- static ::frc971::MultiSpline HPToThirdCargoShipBay(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HPToThirdCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from 3rd cargo ship bay to near the loading station
- static ::frc971::MultiSpline ThirdCargoShipBayToNearHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> ThirdCargoShipBayToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Testing Splines
- static ::frc971::MultiSpline HPToNearRocketTest();
- static ::frc971::MultiSpline HabToFarRocketTest(bool is_left);
- static ::frc971::MultiSpline FarRocketToHPTest();
+ static flatbuffers::Offset<frc971::MultiSpline> HPToNearRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ static flatbuffers::Offset<frc971::MultiSpline> HabToFarRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHPTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
- static ::frc971::MultiSpline BasicSSpline();
- static ::frc971::MultiSpline StraightLine();
+ static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
};
} // namespace actors
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index a5a71aa..fbde0be 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -8,14 +8,16 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2019/actors/auto_splines.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace y2019 {
namespace actors {
+
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
@@ -24,15 +26,14 @@
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ "/drivetrain")),
superstructure_goal_sender_(
- event_loop->MakeSender<::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_status_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")) {}
+ event_loop->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::y2019::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
AOS_LOG(INFO, "Waiting until x > %f\n", x);
@@ -46,8 +47,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (drivetrain_status_fetcher_->x > x) {
- AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
+ if (drivetrain_status_fetcher_->x() > x) {
+ AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x());
return true;
}
}
@@ -65,8 +66,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (::std::abs(drivetrain_status_fetcher_->y) < y) {
- AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
+ if (::std::abs(drivetrain_status_fetcher_->y()) < y) {
+ AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y());
return true;
}
}
@@ -90,13 +91,16 @@
SendSuperstructureGoal();
{
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
// Start on the left l2.
- localizer_resetter->x = 1.0;
- localizer_resetter->y = 1.35 * turn_scalar;
- localizer_resetter->theta = M_PI;
- localizer_resetter->theta_uncertainty = 0.00001;
- if (!localizer_resetter.Send()) {
+ localizer_control_builder.add_x(1.0);
+ localizer_control_builder.add_y(1.35 * turn_scalar);
+ localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -105,20 +109,28 @@
// Otherwise our drivetrain reset will do a 180 right at the start.
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
InitializeEncoders();
ResetDrivetrain();
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
ResetDrivetrain();
InitializeEncoders();
}
-const ProfileParameters kJumpDrive = {2.0, 3.0};
-const ProfileParameters kDrive = {4.0, 3.0};
-const ProfileParameters kTurn = {5.0, 15.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kJumpDrive = MakeProfileParameters(2.0, 3.0);
+const ProfileParametersT kDrive = MakeProfileParameters(4.0, 3.0);
+const ProfileParametersT kTurn = MakeProfileParameters(5.0, 15.0);
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};
@@ -130,14 +142,23 @@
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
+template <typename Functor>
+std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+BindIsLeft(Functor f, bool is_left) {
+ return
+ [is_left, f](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder) { return f(builder, is_left); };
+}
+
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
- const bool is_left = params.mode == 0;
+ const bool is_left = params->mode() == 0;
{
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
- params.mode, is_left ? "left" : "right");
+ params->mode(), is_left ? "left" : "right");
}
const double turn_scalar = is_left ? 1.0 : -1.0;
@@ -147,7 +168,7 @@
Mode mode = Mode::kCargoship;
if (mode == Mode::kRocket) {
SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HabToFarRocketTest(is_left),
+ PlanSpline(BindIsLeft(AutonomousSplines::HabToFarRocketTest, is_left),
SplineDirection::kBackward);
// Grab the disk, jump, wait until we have vacuum, then raise the elevator
@@ -176,15 +197,18 @@
// END SPLINE 1
if (!spline1.WaitForSplineDistanceRemaining(0.2)) return true;
- LineFollowAtVelocity(1.3, 4);
+ LineFollowAtVelocity(1.3,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1200))) return true;
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
- LineFollowAtVelocity(-1.0, 4);
- SplineHandle spline2 = PlanSpline(AutonomousSplines::FarRocketToHP(is_left),
- SplineDirection::kBackward);
+ LineFollowAtVelocity(-1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
+ SplineHandle spline2 =
+ PlanSpline(BindIsLeft(AutonomousSplines::FarRocketToHP, is_left),
+ SplineDirection::kBackward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
if (!spline2.WaitForPlan()) return true;
@@ -204,8 +228,9 @@
// As soon as we pick up Panel 2 go score on the back rocket
if (!WaitForGamePiece()) return true;
LineFollowAtVelocity(1.5);
- SplineHandle spline3 = PlanSpline(AutonomousSplines::HPToFarRocket(is_left),
- SplineDirection::kForward);
+ SplineHandle spline3 =
+ PlanSpline(BindIsLeft(AutonomousSplines::HPToFarRocket, is_left),
+ SplineDirection::kForward);
if (!WaitForDriveXGreater(0.50)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -214,7 +239,8 @@
set_elevator_wrist_goal(kPanelBackwardMiddlePos);
SendSuperstructureGoal();
if (!WaitForDriveXGreater(7.1)) return true;
- LineFollowAtVelocity(-1.5, 4);
+ LineFollowAtVelocity(-1.5,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
set_elevator_wrist_goal(kPanelBackwardUpperPos);
SendSuperstructureGoal();
@@ -222,13 +248,14 @@
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
- LineFollowAtVelocity(1.0, 4);
+ LineFollowAtVelocity(1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
} else if (mode == Mode::kCargoship) {
- SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HABToSecondCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline1 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HABToSecondCargoShipBay, is_left),
+ SplineDirection::kBackward);
set_elevator_goal(0.01);
set_wrist_goal(-M_PI / 2.0);
set_intake_goal(-1.2);
@@ -254,7 +281,8 @@
if (!spline1.WaitForSplineDistanceRemaining(0.8)) return true;
// Line follow in to the first disc.
- LineFollowAtVelocity(-0.9, 2);
+ LineFollowAtVelocity(-0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
if (!WaitForDriveYCloseToZero(1.2)) return true;
set_suction_goal(false, 1);
@@ -265,10 +293,11 @@
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
- LineFollowAtVelocity(0.9, 2);
- SplineHandle spline2 =
- PlanSpline(AutonomousSplines::SecondCargoShipBayToHP(is_left),
- SplineDirection::kForward);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
+ SplineHandle spline2 = PlanSpline(
+ BindIsLeft(AutonomousSplines::SecondCargoShipBayToHP, is_left),
+ SplineDirection::kForward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
if (!spline2.WaitForPlan()) return true;
AOS_LOG(INFO, "Planned\n");
@@ -288,9 +317,9 @@
AOS_LOG(INFO, "Got gamepiece %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
LineFollowAtVelocity(-4.0);
- SplineHandle spline3 =
- PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline3 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HPToThirdCargoShipBay, is_left),
+ SplineDirection::kBackward);
if (!WaitForDriveXGreater(0.55)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -301,7 +330,8 @@
if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
// Line follow in to the second disc.
- LineFollowAtVelocity(-0.7, 3);
+ LineFollowAtVelocity(-0.7,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
AOS_LOG(INFO, "Drawing in disc 2 %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.2)) return true;
@@ -315,7 +345,8 @@
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
AOS_LOG(INFO, "Backing up %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- LineFollowAtVelocity(0.9, 3);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
} else {
// Grab the disk, wait until we have vacuum, then jump
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 8c61596..af2a60f 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -7,16 +7,18 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace actors {
-using ::frc971::ProfileParameters;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2019::control_loops::superstructure;
struct ElevatorWristPosition {
double elevator;
@@ -28,7 +30,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset(bool is_left);
@@ -75,25 +77,83 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->elevator.unsafe_goal = elevator_goal_;
- new_superstructure_goal->wrist.unsafe_goal = wrist_goal_;
- new_superstructure_goal->intake.unsafe_goal = intake_goal_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->suction.grab_piece = suction_on_;
- new_superstructure_goal->suction.gamepiece_mode = suction_gamepiece_;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset;
- new_superstructure_goal->elevator.profile_params.max_velocity =
- elevator_max_velocity_;
- new_superstructure_goal->elevator.profile_params.max_acceleration =
- elevator_max_acceleration_;
+ {
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(elevator_max_velocity_);
+ profile_params_builder.add_max_acceleration(elevator_max_acceleration_);
- new_superstructure_goal->wrist.profile_params.max_velocity =
- wrist_max_velocity_;
- new_superstructure_goal->wrist.profile_params.max_acceleration =
- wrist_max_acceleration_;
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
- if (!new_superstructure_goal.Send()) {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder elevator_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ elevator_builder.add_unsafe_goal(elevator_goal_);
+ elevator_builder.add_profile_params(profile_params_offset);
+
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset;
+
+ {
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(wrist_max_velocity_);
+ profile_params_builder.add_max_acceleration(wrist_max_acceleration_);
+
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
+
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder wrist_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ wrist_builder.add_unsafe_goal(wrist_goal_);
+ wrist_builder.add_profile_params(profile_params_offset);
+
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ intake_builder.add_unsafe_goal(intake_goal_);
+
+ intake_offset = intake_builder.Finish();
+ }
+
+ flatbuffers::Offset<superstructure::SuctionGoal> suction_offset;
+
+ {
+ superstructure::SuctionGoal::Builder suction_builder =
+ builder.MakeBuilder<superstructure::SuctionGoal>();
+
+ suction_builder.add_grab_piece(suction_on_);
+ suction_builder.add_gamepiece_mode(suction_gamepiece_);
+
+ suction_offset = suction_builder.Finish();
+ }
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_elevator(elevator_offset);
+ superstructure_builder.add_wrist(wrist_offset);
+ superstructure_builder.add_intake(intake_offset);
+ superstructure_builder.add_suction(suction_offset);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -102,7 +162,7 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- return superstructure_status_fetcher_->has_piece;
+ return superstructure_status_fetcher_->has_piece();
}
return false;
}
@@ -145,12 +205,12 @@
if (superstructure_status_fetcher_.get()) {
const bool elevator_at_goal =
::std::abs(elevator_goal_ -
- superstructure_status_fetcher_->elevator.position) <
+ superstructure_status_fetcher_->elevator()->position()) <
kElevatorTolerance;
const bool wrist_at_goal =
::std::abs(wrist_goal_ -
- superstructure_status_fetcher_->wrist.position) <
+ superstructure_status_fetcher_->wrist()->position()) <
kWristTolerance;
return elevator_at_goal && wrist_at_goal;
@@ -183,11 +243,9 @@
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- ::aos::Sender<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2019::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Status>
superstructure_status_fetcher_;
};
diff --git a/y2019/actors/autonomous_actor_main.cc b/y2019/actors/autonomous_actor_main.cc
index 79b52da..aa176f9 100644
--- a/y2019/actors/autonomous_actor_main.cc
+++ b/y2019/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2019/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
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;
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 599bcba..0363ced 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -66,7 +66,7 @@
deps = [
"//aos/containers:sized_array",
"//aos/time",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -79,7 +79,7 @@
deps = [
"//aos/containers:sized_array",
"//aos/time:time_mcu",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9450b13..da43495 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -11,27 +11,30 @@
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/logging/logging.h"
-#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "aos/vision/events/udp.h"
#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
-#include "y2019/status_light.q.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2019/status_light_generated.h"
#include "y2019/vision.pb.h"
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::ControlBit;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::POVLocation;
-using ::aos::events::ProtoTXUdpSocket;
+using aos::events::ProtoTXUdpSocket;
+using aos::input::driver_station::ButtonLocation;
+using aos::input::driver_station::ControlBit;
+using aos::input::driver_station::JoystickAxis;
+using aos::input::driver_station::POVLocation;
+using frc971::CreateProfileParameters;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
@@ -39,6 +42,8 @@
namespace input {
namespace joysticks {
+namespace superstructure = y2019::control_loops::superstructure;
+
using google::protobuf::StringPrintf;
struct ElevatorWristPosition {
@@ -143,36 +148,31 @@
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
+ "/drivetrain")),
localizer_control_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ event_loop->MakeSender<LocalizerControl>("/drivetrain")),
camera_log_sender_(
- event_loop->MakeSender<::y2019::CameraLog>(".y2019.camera_log")),
- superstructure_goal_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_goal_sender_(event_loop->MakeSender<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2019::CameraLog>("/camera")),
+ superstructure_goal_fetcher_(
+ event_loop->MakeFetcher<superstructure::Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_position_fetcher_(
- event_loop->MakeFetcher<::y2019::control_loops::superstructure::
- SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ event_loop->MakeFetcher<superstructure::Position>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<::y2019::control_loops::superstructure::
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "status")) {
+ event_loop->MakeFetcher<superstructure::Status>(
+ "/superstructure")) {
const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_goal_fetcher_.Fetch();
if (superstructure_goal_fetcher_.get()) {
- grab_piece_ = superstructure_goal_fetcher_->suction.grab_piece;
- switch_ball_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
+ grab_piece_ = superstructure_goal_fetcher_->has_suction()
+ ? superstructure_goal_fetcher_->suction()->grab_piece()
+ : false;
+ switch_ball_ =
+ superstructure_goal_fetcher_->has_suction()
+ ? (superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0)
+ : true;
}
video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
@@ -195,106 +195,172 @@
return;
}
- if (!superstructure_status_fetcher_->has_piece) {
+ CHECK(superstructure_status_fetcher_->has_stilts());
+
+ if (!superstructure_status_fetcher_->has_piece()) {
last_not_has_piece_ = monotonic_now;
}
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto main_superstructure_goal_builder =
+ superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
{
- auto target_hint = target_selector_hint_sender_.MakeMessage();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), -1.2);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ superstructure::Goal::Builder superstructure_goal_builder =
+ main_superstructure_goal_builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_goal_builder.add_elevator(elevator_offset);
+ superstructure_goal_builder.add_intake(intake_offset);
+ superstructure_goal_builder.add_wrist(wrist_offset);
+ superstructure_goal_builder.add_stilts(stilts_offset);
+ superstructure_goal_builder.add_roller_voltage(0.0);
+
+ superstructure_goal_offset = superstructure_goal_builder.Finish();
+ }
+ superstructure::Goal *mutable_superstructure_goal =
+ GetMutableTemporaryPointer(*main_superstructure_goal_builder.fbb(),
+ superstructure_goal_offset);
+
+ {
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ control_loops::drivetrain::TargetSelectorHint::Builder
+ target_selector_hint_builder =
+ builder
+ .MakeBuilder<control_loops::drivetrain::TargetSelectorHint>();
if (data.IsPressed(kNearCargoHint)) {
- target_hint->suggested_target = 1;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NEAR_SHIP);
} else if (data.IsPressed(kMidCargoHint)) {
- target_hint->suggested_target = 2;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
} else if (data.IsPressed(kFarCargoHint)) {
- target_hint->suggested_target = 3;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
} else {
const double cargo_joy_y = data.GetAxis(kCargoSelectorY);
const double cargo_joy_x = data.GetAxis(kCargoSelectorX);
if (cargo_joy_y > 0.5) {
- target_hint->suggested_target = 1;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NEAR_SHIP);
} else if (cargo_joy_y < -0.5) {
- target_hint->suggested_target = 3;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
} else if (::std::abs(cargo_joy_x) > 0.5) {
- target_hint->suggested_target = 2;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
} else {
- target_hint->suggested_target = 0;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NONE);
}
}
- if (!target_hint.Send()) {
+ if (!builder.Send(target_selector_hint_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send target selector hint.\n");
}
}
if (data.PosEdge(kResetLocalizerLeft)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.6;
- localizer_resetter->y = 3.4;
- localizer_resetter->keep_current_theta = true;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.6);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_keep_current_theta(true);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRight)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
- // Start at the left feeder station.
- localizer_resetter->x = 0.6;
- localizer_resetter->y = -3.4;
- localizer_resetter->keep_current_theta = true;
+ auto builder = localizer_control_sender_.MakeBuilder();
+ // Start at the right feeder station.
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.6);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_keep_current_theta(true);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerLeftForwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = 3.4;
- localizer_resetter->theta = 0.0;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_theta(0.0);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerLeftBackwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = 3.4;
- localizer_resetter->theta = M_PI;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_theta(M_PI);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRightForwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the right feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = -3.4;
- localizer_resetter->theta = 0.0;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_theta(0.0);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRightBackwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the right feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = -3.4;
- localizer_resetter->theta = M_PI;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_theta(M_PI);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -302,7 +368,7 @@
if (data.PosEdge(kRelease) &&
monotonic_now >
last_release_button_press_ + ::std::chrono::milliseconds(500)) {
- if (superstructure_status_fetcher_->has_piece) {
+ if (superstructure_status_fetcher_->has_piece()) {
release_mode_ = ReleaseButtonMode::kRelease;
} else {
release_mode_ = ReleaseButtonMode::kBallIntake;
@@ -313,7 +379,8 @@
last_release_button_press_ = monotonic_now;
}
- AOS_LOG(INFO, "has_piece: %d\n", superstructure_status_fetcher_->has_piece);
+ AOS_LOG(INFO, "has_piece: %d\n",
+ superstructure_status_fetcher_->has_piece());
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
@@ -321,7 +388,7 @@
} else if ((release_mode_ == ReleaseButtonMode::kRelease &&
data.IsPressed(kRelease)) ||
data.IsPressed(kReleaseButtonBoard) ||
- !superstructure_status_fetcher_->has_piece) {
+ !superstructure_status_fetcher_->has_piece()) {
grab_piece_ = false;
AOS_LOG(INFO, "releasing due to other thing\n");
}
@@ -329,8 +396,8 @@
if (data.IsPressed(kRocketBackwardUnpressed)) {
elevator_wrist_pos_ = kStowPos;
}
- new_superstructure_goal->intake.unsafe_goal = -1.2;
- new_superstructure_goal->roller_voltage = 0.0;
+ mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(-1.2);
+ mutable_superstructure_goal->mutate_roller_voltage(0.0);
const bool kDoBallIntake =
(!climbed_ && release_mode_ == ReleaseButtonMode::kBallIntake &&
@@ -345,8 +412,10 @@
}
if (switch_ball_) {
- if (superstructure_status_fetcher_->has_piece) {
- new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
+ if (superstructure_status_fetcher_->has_piece()) {
+ mutable_superstructure_goal->mutable_wrist()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(20);
}
// Go to intake position and apply vacuum
@@ -411,18 +480,18 @@
if (kDoBallOutake ||
(kDoBallIntake &&
monotonic_now < last_not_has_piece_ + chrono::milliseconds(200))) {
- new_superstructure_goal->intake.unsafe_goal = 0.83;
+ mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(0.83);
}
- if (kDoBallIntake && !superstructure_status_fetcher_->has_piece) {
+ if (kDoBallIntake && !superstructure_status_fetcher_->has_piece()) {
elevator_wrist_pos_ = kBallIntakePos;
- new_superstructure_goal->roller_voltage = 9.0;
+ mutable_superstructure_goal->mutate_roller_voltage(9.0);
grab_piece_ = true;
} else {
if (kDoBallOutake) {
- new_superstructure_goal->roller_voltage = -6.0;
+ mutable_superstructure_goal->mutate_roller_voltage(-6.0);
} else {
- new_superstructure_goal->roller_voltage = 0.0;
+ mutable_superstructure_goal->mutate_roller_voltage(0.0);
}
}
}
@@ -430,47 +499,70 @@
constexpr double kDeployStiltPosition = 0.5;
if (data.IsPressed(kFallOver)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.71;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 1.25;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.71);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(1.25);
} else if (data.IsPressed(kHalfStilt)) {
was_above_ = false;
- new_superstructure_goal->stilts.unsafe_goal = 0.345;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal ( 0.345);
+ mutable_superstructure_goal->mutable_stilts()->mutable_profile_params()->mutate_max_velocity ( 0.65);
} else if (data.IsPressed(kDeployStilt) || was_above_) {
- new_superstructure_goal->stilts.unsafe_goal = kDeployStiltPosition;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(
+ kDeployStiltPosition);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(2.0);
} else {
- new_superstructure_goal->stilts.unsafe_goal = 0.005;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.005);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(2.0);
}
- if (superstructure_status_fetcher_->stilts.position > 0.1) {
+ if (superstructure_status_fetcher_->stilts()->position() > 0.1) {
elevator_wrist_pos_ = kClimbPos;
climbed_ = true;
- new_superstructure_goal->wrist.profile_params.max_acceleration = 10;
- new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
+ mutable_superstructure_goal->mutable_wrist()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(10);
+ mutable_superstructure_goal->mutable_elevator()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(6);
}
// If we've been asked to go above deploy and made it up that high, latch
// was_above.
- if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
- superstructure_status_fetcher_->stilts.position >=
+ if (mutable_superstructure_goal->stilts()->unsafe_goal() >
+ kDeployStiltPosition &&
+ superstructure_status_fetcher_->stilts()->position() >=
kDeployStiltPosition) {
was_above_ = true;
- } else if ((superstructure_position_fetcher_->platform_left_detect &&
- superstructure_position_fetcher_->platform_right_detect) &&
+ } else if ((superstructure_position_fetcher_->platform_left_detect() &&
+ superstructure_position_fetcher_->platform_right_detect()) &&
!data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
was_above_ = false;
}
- if (superstructure_status_fetcher_->stilts.position >
+ if (superstructure_status_fetcher_->stilts()->position() >
kDeployStiltPosition &&
- new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
+ mutable_superstructure_goal->stilts()->unsafe_goal() ==
+ kDeployStiltPosition) {
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.30);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(0.75);
}
if ((release_mode_ == ReleaseButtonMode::kRelease &&
@@ -481,21 +573,22 @@
}
if (switch_ball_) {
- new_superstructure_goal->suction.gamepiece_mode = 0;
+ mutable_superstructure_goal->mutable_suction()->mutate_gamepiece_mode(0);
} else {
- new_superstructure_goal->suction.gamepiece_mode = 1;
+ mutable_superstructure_goal->mutable_suction()->mutate_gamepiece_mode(1);
}
vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0);
- new_superstructure_goal->suction.grab_piece = grab_piece_;
+ mutable_superstructure_goal->mutable_suction()->mutate_grab_piece(
+ grab_piece_);
- new_superstructure_goal->elevator.unsafe_goal =
- elevator_wrist_pos_.elevator;
- new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist;
+ mutable_superstructure_goal->mutable_elevator()->mutate_unsafe_goal(
+ elevator_wrist_pos_.elevator);
+ mutable_superstructure_goal->mutable_wrist()->mutate_unsafe_goal(
+ elevator_wrist_pos_.wrist);
- AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
+ if (!main_superstructure_goal_builder.Send(superstructure_goal_offset)) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
@@ -506,10 +599,8 @@
}
{
- auto camera_log_message = camera_log_sender_.MakeMessage();
- camera_log_message->log = data.IsPressed(kCameraLog);
- AOS_LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
- camera_log_message.Send();
+ auto builder = camera_log_sender_.MakeBuilder();
+ builder.Send(CreateCameraLog(*builder.fbb(), data.IsPressed(kCameraLog)));
}
}
@@ -517,26 +608,16 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
- localizer_control_sender_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
::aos::Sender<::y2019::CameraLog> camera_log_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
- superstructure_goal_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
- superstructure_goal_sender_;
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Position>
- superstructure_position_fetcher_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
- superstructure_status_fetcher_;
-
+ ::aos::Fetcher<superstructure::Position> superstructure_position_fetcher_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
// Bool to track if we've been above the deploy position. Once this bool is
// set, don't let the stilts retract until we see the platform.
@@ -576,7 +657,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::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2019/status_light.fbs b/y2019/status_light.fbs
new file mode 100644
index 0000000..633b030
--- /dev/null
+++ b/y2019/status_light.fbs
@@ -0,0 +1,15 @@
+namespace y2019;
+
+table StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ red:float;
+ green:float;
+ blue:float;
+}
+
+table CameraLog {
+ log:bool;
+}
+
+root_type CameraLog;
+root_type StatusLight;
diff --git a/y2019/status_light.q b/y2019/status_light.q
deleted file mode 100644
index 66e8ad7..0000000
--- a/y2019/status_light.q
+++ /dev/null
@@ -1,14 +0,0 @@
-package y2019;
-
-// Published on ".y2019.status_light"
-message StatusLight {
- // How bright to make each one. 0 is off, 1 is full on.
- float red;
- float green;
- float blue;
-};
-
-// Published on ".y2019.camera_log"
-message CameraLog {
- bool log;
-};
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index c7f0009..22b973a 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -1,4 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("//tools:environments.bzl", "mcu_cpus")
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index 304f4cb..ac559b0 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -11,9 +11,6 @@
#include "aos/vision/image/reader.h"
#include "y2019/vision/target_finder.h"
-#undef CHECK_NOTNULL
-#undef CHECK_OP
-#undef PCHECK
// CERES Clashes with logging symbols...
#include "ceres/ceres.h"
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 3600194..a7126e4 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -1,6 +1,6 @@
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
-load("//aos/downloader:downloader.bzl", "aos_downloader_dir")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
load("@build_bazel_rules_typescript//:defs.bzl", "ts_library")
load("@build_bazel_rules_nodejs//:defs.bzl", "rollup_bundle")
@@ -49,15 +49,15 @@
":server_data_proto",
"//aos:init",
"//aos/containers:ring_buffer",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
"//aos/seasocks:seasocks_logger",
"//aos/time",
"//frc971/control_loops:pose",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//third_party/seasocks",
"//y2019:constants",
- "//y2019/control_loops/drivetrain:camera_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index e66a70b..91dc61a 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -8,11 +8,12 @@
#include <sstream>
#include "aos/containers/ring_buffer.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/seasocks/seasocks_logger.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
#include "google/protobuf/util/json_util.h"
#include "internal/Embedded.h"
@@ -20,8 +21,8 @@
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/vision/server/server_data.pb.h"
namespace y2019 {
@@ -133,19 +134,21 @@
}
void DataThread(seasocks::Server *server, WebsocketHandler *websocket_handler) {
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
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");
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
- superstructure_status_fetcher = event_loop.MakeFetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status");
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Status>
+ superstructure_status_fetcher =
+ event_loop
+ .MakeFetcher<::y2019::control_loops::superstructure::Status>(
+ "/superstructure");
::std::array<LocalCameraFrame, 5> latest_frames;
::std::array<aos::monotonic_clock::time_point, 5> last_target_time;
@@ -155,7 +158,7 @@
::aos::RingBuffer<DrivetrainPosition, 200> drivetrain_log;
event_loop.MakeWatcher(
- ".y2019.control_loops.drivetrain.camera_frames",
+ "/drivetrain",
[websocket_handler, server, &latest_frames, &last_target_time,
&drivetrain_status_fetcher, &superstructure_status_fetcher,
&last_send_time, &drivetrain_log,
@@ -163,9 +166,9 @@
&camera_frames) {
while (drivetrain_status_fetcher.FetchNext()) {
DrivetrainPosition drivetrain_position{
- drivetrain_status_fetcher->sent_time,
- drivetrain_status_fetcher->x, drivetrain_status_fetcher->y,
- drivetrain_status_fetcher->theta};
+ drivetrain_status_fetcher.context().monotonic_sent_time,
+ drivetrain_status_fetcher->x(), drivetrain_status_fetcher->y(),
+ drivetrain_status_fetcher->theta()};
drivetrain_log.Push(drivetrain_position);
}
@@ -181,25 +184,26 @@
const auto &new_frame = camera_frames;
// TODO(james): Maybe we only want to fill out a new frame if it has
// targets or the saved target is > 0.1 sec old? Not sure, but for now
- if (new_frame.camera < latest_frames.size()) {
- latest_frames[new_frame.camera].capture_time =
+ if (new_frame.camera() < latest_frames.size()) {
+ latest_frames[new_frame.camera()].capture_time =
aos::monotonic_clock::time_point(
- chrono::nanoseconds(new_frame.timestamp));
- latest_frames[new_frame.camera].targets.clear();
- if (new_frame.num_targets > 0) {
- last_target_time[new_frame.camera] =
- latest_frames[new_frame.camera].capture_time;
+ chrono::nanoseconds(new_frame.timestamp()));
+ latest_frames[new_frame.camera()].targets.clear();
+ if (new_frame.has_targets() && new_frame.targets()->size() > 0) {
+ last_target_time[new_frame.camera()] =
+ latest_frames[new_frame.camera()].capture_time;
}
- for (int target = 0; target < new_frame.num_targets; ++target) {
- latest_frames[new_frame.camera].targets.emplace_back();
- const float heading = new_frame.targets[target].heading;
- const float distance = new_frame.targets[target].distance;
- latest_frames[new_frame.camera].targets.back().x =
+ for (const control_loops::drivetrain::CameraTarget *target :
+ *new_frame.targets()) {
+ latest_frames[new_frame.camera()].targets.emplace_back();
+ const float heading = target->heading();
+ const float distance = target->distance();
+ latest_frames[new_frame.camera()].targets.back().x =
::std::cos(heading) * distance;
- latest_frames[new_frame.camera].targets.back().y =
+ latest_frames[new_frame.camera()].targets.back().y =
::std::sin(heading) * distance;
- latest_frames[new_frame.camera].targets.back().theta =
- new_frame.targets[target].skew;
+ latest_frames[new_frame.camera()].targets.back().theta =
+ target->skew();
}
}
}
@@ -235,32 +239,33 @@
if (now > last_send_time + chrono::milliseconds(100)) {
last_send_time = now;
- debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x);
- debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y);
+ debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x());
+ debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y());
debug_data.mutable_robot_pose()->set_theta(
- drivetrain_status_fetcher->theta);
+ drivetrain_status_fetcher->theta());
{
LineFollowDebug *line_debug =
debug_data.mutable_line_follow_debug();
line_debug->set_frozen(
- drivetrain_status_fetcher->line_follow_logging.frozen);
+ drivetrain_status_fetcher->line_follow_logging()->frozen());
line_debug->set_have_target(
- drivetrain_status_fetcher->line_follow_logging.have_target);
+ drivetrain_status_fetcher->line_follow_logging()->have_target());
line_debug->mutable_goal_target()->set_x(
- drivetrain_status_fetcher->line_follow_logging.x);
+ drivetrain_status_fetcher->line_follow_logging()->x());
line_debug->mutable_goal_target()->set_y(
- drivetrain_status_fetcher->line_follow_logging.y);
+ drivetrain_status_fetcher->line_follow_logging()->y());
line_debug->mutable_goal_target()->set_theta(
- drivetrain_status_fetcher->line_follow_logging.theta);
+ drivetrain_status_fetcher->line_follow_logging()->theta());
}
Sensors *sensors = debug_data.mutable_sensors();
- sensors->set_wrist(superstructure_status_fetcher->wrist.position);
+ sensors->set_wrist(
+ superstructure_status_fetcher->wrist()->position());
sensors->set_elevator(
- superstructure_status_fetcher->elevator.position);
- sensors->set_intake(superstructure_status_fetcher->intake.position);
- sensors->set_stilts(superstructure_status_fetcher->stilts.position);
- sensors->set_has_piece(superstructure_status_fetcher->has_piece);
+ superstructure_status_fetcher->elevator()->position());
+ sensors->set_intake(superstructure_status_fetcher->intake()->position());
+ sensors->set_stilts(superstructure_status_fetcher->stilts()->position());
+ sensors->set_has_piece(superstructure_status_fetcher->has_piece());
::std::string json;
google::protobuf::util::MessageToJsonString(debug_data, &json);
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 4f09da1..227d3ca 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -20,20 +20,20 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -41,24 +41,25 @@
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
#include "y2019/jevois/spi.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2019::control_loops::superstructure::SuperstructureQueue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
+namespace superstructure = ::y2019::control_loops::superstructure;
namespace chrono = ::std::chrono;
using aos::make_unique;
@@ -133,15 +134,14 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -233,79 +233,106 @@
void RunIteration() override {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ drivetrain_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed = -drivetrain_velocity_translate(
- drivetrain_right_encoder_->GetPeriod());
+ drivetrain_builder.add_right_encoder(
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ builder.Send(drivetrain_builder.Finish());
}
const auto values = constants::GetValues();
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
// Elevator
- CopyPosition(elevator_encoder_, &superstructure_message->elevator,
+ frc971::PotAndAbsolutePositionT elevator;
+ CopyPosition(elevator_encoder_, &elevator,
Values::kElevatorEncoderCountsPerRevolution(),
Values::kElevatorEncoderRatio(), elevator_pot_translate,
false, values.elevator.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &elevator);
+
// Intake
- CopyPosition(intake_encoder_, &superstructure_message->intake_joint,
+ frc971::AbsolutePositionT intake_joint;
+ CopyPosition(intake_encoder_, &intake_joint,
Values::kIntakeEncoderCountsPerRevolution(),
Values::kIntakeEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
// Wrist
- CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ frc971::PotAndAbsolutePositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist,
Values::kWristEncoderCountsPerRevolution(),
Values::kWristEncoderRatio(), wrist_pot_translate, false,
values.wrist.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &wrist);
// Stilts
- CopyPosition(stilts_encoder_, &superstructure_message->stilts,
+ frc971::PotAndAbsolutePositionT stilts;
+ CopyPosition(stilts_encoder_, &stilts,
Values::kStiltsEncoderCountsPerRevolution(),
Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
values.stilts.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &stilts);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_intake_joint(intake_joint_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_stilts(stilts_offset);
// Suction
constexpr float kMinVoltage = 0.5;
constexpr float kMaxVoltage = 2.1;
- superstructure_message->suction_pressure =
+ position_builder.add_suction_pressure(
(vacuum_sensor_->GetVoltage() - kMinVoltage) /
- (kMaxVoltage - kMinVoltage);
+ (kMaxVoltage - kMinVoltage));
- superstructure_message->platform_left_detect =
- !platform_left_detect_->Get();
- superstructure_message->platform_right_detect =
- !platform_right_detect_->Get();
+ position_builder.add_platform_left_detect(!platform_left_detect_->Get());
+ position_builder.add_platform_right_detect(
+ !platform_right_detect_->Get());
- superstructure_message.Send();
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+
+ uint32_t mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
+ mode |= 1 << i;
}
}
- AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
+
+ auto auto_mode_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+ auto_mode_builder.add_mode(mode);
+
+ builder.Send(auto_mode_builder.Finish());
}
}
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
@@ -319,7 +346,6 @@
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
- // TODO(sabina): Add wrist and elevator hall effects.
};
class CameraReader {
@@ -363,7 +389,7 @@
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
- } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log) {
+ } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log()) {
to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
@@ -388,25 +414,49 @@
return;
}
- const auto now = aos::monotonic_clock::now();
+ const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
for (const auto &received : unpacked->frames) {
- auto to_send = camera_frame_sender_.MakeMessage();
+ auto builder = camera_frame_sender_.MakeBuilder();
+
+ std::array<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>,
+ 3>
+ targets;
+
+ for (size_t i = 0; i < received.targets.size(); ++i) {
+ y2019::control_loops::drivetrain::CameraTarget::Builder
+ camera_target_builder = builder.MakeBuilder<
+ y2019::control_loops::drivetrain::CameraTarget>();
+
+ camera_target_builder.add_distance(received.targets[i].distance);
+ camera_target_builder.add_height(received.targets[i].height);
+ camera_target_builder.add_heading(received.targets[i].heading);
+ camera_target_builder.add_skew(received.targets[i].skew);
+
+ targets[i] = camera_target_builder.Finish();
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>>>
+ targets_offset = builder.fbb()->CreateVector(targets.begin(),
+ received.targets.size());
+
+ y2019::control_loops::drivetrain::CameraFrame::Builder
+ camera_frame_builder =
+ builder
+ .MakeBuilder<y2019::control_loops::drivetrain::CameraFrame>();
+
+ camera_frame_builder.add_targets(targets_offset);
+
// Add an extra 10ms delay to account for unmodeled delays that Austin
// thinks exists.
- to_send->timestamp =
+ camera_frame_builder.add_timestamp(
std::chrono::nanoseconds(
(now - received.age - ::std::chrono::milliseconds(10))
- .time_since_epoch()).count();
- to_send->num_targets = received.targets.size();
- for (size_t i = 0; i < received.targets.size(); ++i) {
- to_send->targets[i].distance = received.targets[i].distance;
- to_send->targets[i].height = received.targets[i].height;
- to_send->targets[i].heading = received.targets[i].heading;
- to_send->targets[i].skew = received.targets[i].skew;
- }
- to_send->camera = received.camera_index;
- AOS_LOG_STRUCT(DEBUG, "camera_frames", *to_send);
- to_send.Send();
+ .time_since_epoch())
+ .count());
+ camera_frame_builder.add_camera(received.camera_index);
+ builder.Send(camera_frame_builder.Finish());
}
if (dummy_spi_) {
@@ -458,14 +508,13 @@
};
class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
- event_loop,
- ".y2019.control_loops.superstructure.superstructure_queue.output"),
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure"),
robot_state_fetcher_(
- event_loop->MakeFetcher<::aos::RobotState>(".aos.robot_state")) {}
+ event_loop->MakeFetcher<::aos::RobotState>("/aos")) {}
void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
elevator_victor_ = ::std::move(t);
@@ -488,28 +537,27 @@
}
private:
- void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage,
+ void Write(const superstructure::Output &output) override {
+ elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage(),
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage,
+ intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage,
+ wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage,
+ stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
robot_state_fetcher_.Fetch();
const double battery_voltage = robot_state_fetcher_.get()
- ? robot_state_fetcher_->voltage_battery
+ ? robot_state_fetcher_->voltage_battery()
: 12.0;
// Throw a fast low pass filter on the battery voltage so we don't respond
@@ -518,7 +566,7 @@
0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
suction_victor_->SetSpeed(::aos::Clip(
- output.pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
+ output.pump_voltage() / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -543,11 +591,12 @@
public:
SolenoidWriter(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- superstructure_fetcher_(event_loop->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
- status_light_fetcher_(event_loop->MakeFetcher<::y2019::StatusLight>(
- ".y2019.status_light")) {
+ superstructure_fetcher_(
+ event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
+ status_light_fetcher_(
+ event_loop->MakeFetcher<::y2019::StatusLight>("/superstructure")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
@@ -580,36 +629,40 @@
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- big_suction_cup0_->Set(!superstructure_fetcher_->intake_suction_bottom);
- big_suction_cup1_->Set(!superstructure_fetcher_->intake_suction_bottom);
- small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
- small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
+ big_suction_cup0_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ big_suction_cup1_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top());
+ small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top());
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
+ ::aos::Clip(superstructure_fetcher_->intake_roller_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
pcm_.Flush();
- to_log.read_solenoids = pcm_.GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_.GetAll());
+ builder.Send(to_log_builder.Finish());
}
status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
+ StatusLightT color;
if (!status_light_fetcher_.get() ||
- status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
+ status_light_fetcher_.context().monotonic_sent_time +
+ chrono::milliseconds(100) <
event_loop_->monotonic_now()) {
- StatusLight color;
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
@@ -623,15 +676,13 @@
light_flash_ = 0;
}
- AOS_LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
} else {
- AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
- SetColor(*status_light_fetcher_.get());
+ status_light_fetcher_->UnPackTo(&color);
}
+ SetColor(color);
}
- void SetColor(const StatusLight &status_light) {
+ void SetColor(const StatusLightT status_light) {
// Save CAN bandwidth and CPU at the cost of RT. Only change the light when
// it actually changes. This is pretty low priority anyways.
static int time_since_last_send = 0;
@@ -669,11 +720,12 @@
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Output>
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Output>
superstructure_fetcher_;
::aos::Fetcher<::y2019::StatusLight> status_light_fetcher_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
+
::ctre::phoenix::CANifier canifier_{0};
double last_red_ = -1.0;
@@ -691,19 +743,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
@@ -734,7 +789,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
CameraReader camera_reader(&imu_event_loop);
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
@@ -757,7 +812,7 @@
// variety so all the Victors are written as SPs.
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
@@ -779,7 +834,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
diff --git a/y2019/y2019.json b/y2019/y2019.json
new file mode 100644
index 0000000..c32db3f
--- /dev/null
+++ b/y2019/y2019.json
@@ -0,0 +1,49 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.StatusLight",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2019.control_loops.drivetrain.TargetSelectorHint",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2019.control_loops.drivetrain.CameraFrame",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}