Merge "Finish camera_reader (in theory)"
diff --git a/aos/BUILD b/aos/BUILD
index ac5c3e9..47de872 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -475,6 +475,7 @@
"testdata/expected.json",
"testdata/expected_multinode.json",
"testdata/good_multinode.json",
+ "testdata/good_multinode_hostnames.json",
"testdata/invalid_destination_node.json",
"testdata/invalid_nodes.json",
"testdata/invalid_source_node.json",
diff --git a/aos/configuration.cc b/aos/configuration.cc
index 2d4b867..ac897bf 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -289,7 +289,7 @@
}
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
// Start by building the vectors. They need to come before the final table.
// Channels
@@ -362,7 +362,7 @@
// Check that if there is a node list, all the source nodes are filled out and
// valid, and all the destination nodes are valid (and not the source). This
// is a basic consistency check.
- if (result.message().has_nodes()) {
+ if (result.message().has_nodes() && config.message().has_channels()) {
for (const Channel *c : *config.message().channels()) {
CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
<< " is missing \"source_node\"";
@@ -492,7 +492,7 @@
const Flatbuffer<Configuration> &config,
const std::vector<aos::FlatbufferString<reflection::Schema>> &schemas) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Channel>>>
channels_offset;
@@ -500,7 +500,7 @@
std::vector<flatbuffers::Offset<Channel>> channel_offsets;
for (const Channel *c : *config.message().channels()) {
flatbuffers::FlatBufferBuilder channel_fbb;
- channel_fbb.ForceDefaults(1);
+ channel_fbb.ForceDefaults(true);
// Search for a schema with a matching type.
const aos::FlatbufferString<reflection::Schema> *found_schema = nullptr;
@@ -597,9 +597,16 @@
const Node *GetNodeFromHostname(const Configuration *config,
std::string_view hostname) {
for (const Node *node : *config->nodes()) {
- if (node->hostname()->string_view() == hostname) {
+ if (node->has_hostname() && node->hostname()->string_view() == hostname) {
return node;
}
+ if (node->has_hostnames()) {
+ for (const auto &candidate : *node->hostnames()) {
+ if (candidate->string_view() == hostname) {
+ return node;
+ }
+ }
+ }
}
return nullptr;
}
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 317c100..576616e 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -126,6 +126,12 @@
hostname:string;
// Port to serve forwarded data from.
port:ushort = 9971;
+
+ // An alternative to hostname which allows specifying multiple hostnames,
+ // any of which will match this node.
+ //
+ // Don't specify a hostname in multiple nodes in the same configuration.
+ hostnames:[string];
}
// Overall configuration datastructure for the pubsub.
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index 0c0874b..c36bd93 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -677,6 +677,46 @@
GetNodeOrDie(&config.message(), config2.message().nodes()->Get(0)));
}
+TEST_F(ConfigurationTest, GetNodeFromHostname) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode.json");
+ EXPECT_EQ("pi1",
+ CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi2"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi3"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
+TEST_F(ConfigurationTest, GetNodeFromHostnames) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig("aos/testdata/good_multinode_hostnames.json");
+ EXPECT_EQ("pi1",
+ CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi2"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "raspberrypi3"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ("pi2", CHECK_NOTNULL(
+ GetNodeFromHostname(&config.message(), "other"))
+ ->name()
+ ->string_view());
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi4"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+ EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
} // namespace testing
} // namespace configuration
} // namespace aos
diff --git a/aos/events/BUILD b/aos/events/BUILD
index 1f7a3d7..a905728 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -141,6 +141,7 @@
":ping_fbs",
":pong_fbs",
"//aos/network:message_bridge_client_fbs",
+ "//aos/network:timestamp_fbs",
"//aos/network:message_bridge_server_fbs",
],
deps = [":config"],
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index b50162b..2ffad3b 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -209,7 +209,7 @@
// Now, build up a report with everything pre-filled out.
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
// Pre-fill in the defaults for timers.
std::vector<flatbuffers::Offset<timing::Timer>> timer_offsets;
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 4294226..6047cbc 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -252,7 +252,7 @@
allocator_(allocator),
sender_(sender) {
CheckChannelDataAlignment(allocator->data(), allocator->size());
- fbb_.ForceDefaults(1);
+ fbb_.ForceDefaults(true);
}
Builder() {}
Builder(const Builder &) = delete;
@@ -306,6 +306,16 @@
return sender_ ? true : false;
}
+ // Returns the time_points that the last message was sent at.
+ aos::monotonic_clock::time_point monotonic_sent_time() const {
+ return sender_->monotonic_sent_time();
+ }
+ aos::realtime_clock::time_point realtime_sent_time() const {
+ return sender_->realtime_sent_time();
+ }
+ // Returns the queue index that this was sent with.
+ uint32_t sent_queue_index() const { return sender_->sent_queue_index(); }
+
private:
friend class EventLoop;
Sender(std::unique_ptr<RawSender> sender) : sender_(std::move(sender)) {}
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index 5db503a..02b7bd7 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -27,7 +27,7 @@
const ::std::string header_json =
aos::util::ReadFileToStringOrDie(FLAGS_header);
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
flatbuffers::Offset<aos::logger::LogFileHeader> header =
aos::JsonToFlatbuffer<aos::logger::LogFileHeader>(header_json, &fbb);
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index 57c2c2c..ddd3dac 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -124,7 +124,7 @@
void Logger::WriteHeader(const Node *node) {
// Now write the header with this timestamp in it.
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
flatbuffers::Offset<aos::Configuration> configuration_offset =
CopyFlatBuffer(event_loop_->configuration(), &fbb);
@@ -230,7 +230,7 @@
// Write!
flatbuffers::FlatBufferBuilder fbb(f.fetcher->context().size +
max_header_size_);
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
f.channel_index, f.log_type));
@@ -252,7 +252,7 @@
if (f.timestamp_writer != nullptr) {
// And now handle timestamps.
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.FinishSizePrefixed(PackMessage(&fbb, f.fetcher->context(),
f.channel_index,
@@ -898,13 +898,13 @@
// This is the builder that we use for the config containing all the new
// channels.
flatbuffers::FlatBufferBuilder new_config_fbb;
- new_config_fbb.ForceDefaults(1);
+ new_config_fbb.ForceDefaults(true);
std::vector<flatbuffers::Offset<Channel>> channel_offsets;
for (auto &pair : remapped_channels_) {
// This is the builder that we use for creating the Channel with just the
// new name.
flatbuffers::FlatBufferBuilder new_name_fbb;
- new_name_fbb.ForceDefaults(1);
+ new_name_fbb.ForceDefaults(true);
const flatbuffers::Offset<flatbuffers::String> name_offset =
new_name_fbb.CreateString(pair.second);
ChannelBuilder new_name_builder(new_name_fbb);
diff --git a/aos/events/multinode_pingpong.json b/aos/events/multinode_pingpong.json
index c9bc53e..54cc658 100644
--- a/aos/events/multinode_pingpong.json
+++ b/aos/events/multinode_pingpong.json
@@ -26,6 +26,56 @@
},
{
"name": "/aos/pi1",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi1",
+ "frequency": 10,
+ "num_senders": 2,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "priority": 1,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "pi3",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/aos/pi2",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi2",
+ "frequency": 10,
+ "num_senders": 2,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/aos/pi3",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi3",
+ "frequency": 10,
+ "num_senders": 2,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/aos/pi1",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi1",
"frequency": 2
@@ -141,7 +191,6 @@
{
"match": {
"name": "/aos",
- "type": "aos.logging.LogMessageFbs",
"source_node": "pi1"
},
"rename": {
@@ -151,7 +200,6 @@
{
"match": {
"name": "/aos",
- "type": "aos.logging.LogMessageFbs",
"source_node": "pi2"
},
"rename": {
@@ -161,97 +209,6 @@
{
"match": {
"name": "/aos",
- "type": "aos.logging.LogMessageFbs",
- "source_node": "pi3"
- },
- "rename": {
- "name": "/aos/pi3"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.timing.Report",
- "source_node": "pi1"
- },
- "rename": {
- "name": "/aos/pi1"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.timing.Report",
- "source_node": "pi2"
- },
- "rename": {
- "name": "/aos/pi2"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.timing.Report",
- "source_node": "pi3"
- },
- "rename": {
- "name": "/aos/pi3"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ServerStatistics",
- "source_node": "pi1"
- },
- "rename": {
- "name": "/aos/pi1"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ServerStatistics",
- "source_node": "pi2"
- },
- "rename": {
- "name": "/aos/pi2"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ServerStatistics",
- "source_node": "pi3"
- },
- "rename": {
- "name": "/aos/pi3"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ClientStatistics",
- "source_node": "pi1"
- },
- "rename": {
- "name": "/aos/pi1"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ClientStatistics",
- "source_node": "pi2"
- },
- "rename": {
- "name": "/aos/pi2"
- }
- },
- {
- "match": {
- "name": "/aos",
- "type": "aos.message_bridge.ClientStatistics",
"source_node": "pi3"
},
"rename": {
diff --git a/aos/flatbuffer_merge.cc b/aos/flatbuffer_merge.cc
index 1f5c8a0..e548f70 100644
--- a/aos/flatbuffer_merge.cc
+++ b/aos/flatbuffer_merge.cc
@@ -518,7 +518,7 @@
const uint8_t *data2) {
// Build up a builder.
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
// Finish up the buffer and return it.
fbb.Finish(MergeFlatBuffers(typetable, data1, data2, &fbb));
@@ -537,10 +537,10 @@
// implementation is fine for the usages that we have now. We are better off
// abstracting this into a library call where we can fix it later easily.
flatbuffers::FlatBufferBuilder fbb1;
- fbb1.ForceDefaults(1);
+ fbb1.ForceDefaults(true);
fbb1.Finish(MergeFlatBuffers(typetable, t1, nullptr, &fbb1));
flatbuffers::FlatBufferBuilder fbb2;
- fbb2.ForceDefaults(1);
+ fbb2.ForceDefaults(true);
fbb2.Finish(MergeFlatBuffers(typetable, t2, nullptr, &fbb2));
if (fbb1.GetSize() != fbb2.GetSize()) return false;
diff --git a/aos/flatbuffer_merge.h b/aos/flatbuffer_merge.h
index 5e84160..418ea5e 100644
--- a/aos/flatbuffer_merge.h
+++ b/aos/flatbuffer_merge.h
@@ -55,7 +55,7 @@
inline aos::FlatbufferDetachedBuffer<T> MergeFlatBuffers(const T *fb1,
const T *fb2) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.Finish(MergeFlatBuffers<T>(
reinterpret_cast<const flatbuffers::Table *>(fb1),
reinterpret_cast<const flatbuffers::Table *>(fb2), &fbb));
@@ -72,7 +72,7 @@
template <class T>
inline FlatbufferDetachedBuffer<T> CopyFlatBuffer(const T *t) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.Finish(CopyFlatBuffer<T>(t, &fbb));
return FlatbufferDetachedBuffer<T>(fbb.Release());
}
diff --git a/aos/flatbuffer_merge_test.cc b/aos/flatbuffer_merge_test.cc
index 371639a..890a831 100644
--- a/aos/flatbuffer_merge_test.cc
+++ b/aos/flatbuffer_merge_test.cc
@@ -136,7 +136,7 @@
out);
{
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.Finish(MergeFlatBuffers(
ConfigurationTypeTable(),
flatbuffers::GetRoot<flatbuffers::Table>(fb1.data()),
@@ -145,7 +145,7 @@
}
{
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.Finish(MergeFlatBuffers<Configuration>(
flatbuffers::GetRoot<flatbuffers::Table>(fb1.data()),
flatbuffers::GetRoot<flatbuffers::Table>(fb2.data()), &fbb));
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index 15740a1..6e86d35 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -216,7 +216,7 @@
// Constructs an empty flatbuffer of type T.
static FlatbufferDetachedBuffer<T> Empty() {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
const auto end = fbb.EndTable(fbb.StartTable());
fbb.Finish(flatbuffers::Offset<flatbuffers::Table>(end));
return FlatbufferDetachedBuffer<T>(fbb.Release());
@@ -238,6 +238,7 @@
public:
FlatbufferFixedAllocatorArray() : buffer_(), allocator_(&buffer_[0], Size) {
builder_ = flatbuffers::FlatBufferBuilder(Size, &allocator_);
+ builder_.ForceDefaults(true);
}
flatbuffers::FlatBufferBuilder *Builder() {
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 249a59e..95c95dd 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -777,7 +777,7 @@
const std::string_view data,
const flatbuffers::TypeTable *typetable) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
const flatbuffers::Offset<flatbuffers::Table> result =
JsonToFlatbuffer(data, typetable, &fbb);
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 11eebdb..96d561f 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -14,6 +14,15 @@
)
flatbuffer_cc_library(
+ name = "timestamp_fbs",
+ srcs = ["timestamp.fbs"],
+ gen_reflections = 1,
+ includes = [
+ "//aos:configuration_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
name = "message_bridge_client_fbs",
srcs = ["message_bridge_client.fbs"],
gen_reflections = 1,
@@ -41,9 +50,8 @@
"team_number.h",
],
deps = [
- "//aos/logging",
"//aos/util:string_to_num",
- "@com_google_absl//absl/base",
+ "@com_github_google_glog//:glog",
],
)
@@ -96,9 +104,19 @@
cc_library(
name = "message_bridge_protocol",
+ srcs = [
+ "message_bridge_protocol.cc",
+ ],
hdrs = [
"message_bridge_protocol.h",
],
+ deps = [
+ ":connect_fbs",
+ "//aos:configuration",
+ "//aos:flatbuffer_merge",
+ "//aos:flatbuffers",
+ "@com_github_google_flatbuffers//:flatbuffers",
+ ],
)
cc_library(
@@ -114,10 +132,12 @@
],
deps = [
":connect_fbs",
+ ":message_bridge_client_fbs",
":message_bridge_protocol",
":message_bridge_server_fbs",
":sctp_lib",
":sctp_server",
+ ":timestamp_fbs",
"//aos:unique_malloc_ptr",
"//aos/events:shm_event_loop",
"//aos/events/logging:logger",
@@ -172,6 +192,7 @@
":message_bridge_protocol",
":message_bridge_server_fbs",
":sctp_client",
+ ":timestamp_fbs",
"//aos/events:shm_event_loop",
"//aos/events/logging:logger",
],
@@ -201,6 +222,7 @@
"//aos/events:pong_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
],
deps = ["//aos/events:config"],
)
@@ -240,11 +262,11 @@
flatbuffer_cc_library(
name = "web_proxy_fbs",
srcs = ["web_proxy.fbs"],
+ gen_reflections = True,
includes = [
":connect_fbs_includes",
"//aos:configuration_fbs_includes",
],
- gen_reflections = True,
)
flatbuffer_ts_library(
@@ -258,8 +280,8 @@
cc_library(
name = "web_proxy_utils",
- hdrs = ["web_proxy_utils.h"],
srcs = ["web_proxy_utils.cc"],
+ hdrs = ["web_proxy_utils.h"],
deps = [
":connect_fbs",
":web_proxy_fbs",
@@ -300,25 +322,25 @@
cc_binary(
name = "web_proxy_main",
srcs = ["web_proxy_main.cc"],
- deps = [
- ":gen_embedded",
- ":web_proxy",
- "//aos/events:shm_event_loop",
- "//aos:init",
- "//aos/seasocks:seasocks_logger",
- "//third_party/seasocks",
- "@com_github_google_flatbuffers//:flatbuffers"
- ],
copts = [
"-DWEBRTC_POSIX",
"-Wno-unused-parameter",
],
data = [
- "//aos/network/www:files",
- "//aos/network/www:main_bundle",
- "//aos/network/www:flatbuffers",
- "@com_github_google_flatbuffers//:flatjs",
"//aos/events:pingpong_config.json",
+ "//aos/network/www:files",
+ "//aos/network/www:flatbuffers",
+ "//aos/network/www:main_bundle",
+ "@com_github_google_flatbuffers//:flatjs",
+ ],
+ deps = [
+ ":gen_embedded",
+ ":web_proxy",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/seasocks:seasocks_logger",
+ "//third_party/seasocks",
+ "@com_github_google_flatbuffers//:flatbuffers",
],
)
diff --git a/aos/network/message_bridge_client.fbs b/aos/network/message_bridge_client.fbs
index 58b653a..df3f02f 100644
--- a/aos/network/message_bridge_client.fbs
+++ b/aos/network/message_bridge_client.fbs
@@ -13,6 +13,11 @@
// Number of packets received on all channels.
received_packets:uint;
+ // This is the measured monotonic offset for just the server -> client
+ // direction measured in nanoseconds. Subtract this from our monotonic time
+ // to get their monotonic time.
+ monotonic_offset:int64;
+
// TODO(austin): Per channel counts?
}
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index 39c3d17..263a757 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -9,6 +9,7 @@
#include "aos/network/message_bridge_client_generated.h"
#include "aos/network/message_bridge_protocol.h"
#include "aos/network/sctp_client.h"
+#include "aos/network/timestamp_generated.h"
#include "aos/unique_malloc_ptr.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
@@ -26,40 +27,6 @@
namespace {
namespace chrono = std::chrono;
-aos::FlatbufferDetachedBuffer<aos::message_bridge::Connect> MakeConnectMessage(
- const Configuration *config, const Node *my_node,
- std::string_view remote_name) {
- CHECK(config->has_nodes()) << ": Config must have nodes to transfer.";
-
- flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
-
- flatbuffers::Offset<Node> node_offset = CopyFlatBuffer<Node>(my_node, &fbb);
- const std::string_view node_name = my_node->name()->string_view();
-
- std::vector<flatbuffers::Offset<Channel>> channel_offsets;
- for (const Channel *channel : *config->channels()) {
- if (channel->has_destination_nodes()) {
- for (const Connection *connection : *channel->destination_nodes()) {
- if (connection->name()->string_view() == node_name &&
- channel->source_node()->string_view() == remote_name) {
- channel_offsets.emplace_back(CopyFlatBuffer<Channel>(channel, &fbb));
- }
- }
- }
- }
-
- flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Channel>>>
- channels_offset = fbb.CreateVector(channel_offsets);
-
- Connect::Builder connect_builder(fbb);
- connect_builder.add_channels_to_transfer(channels_offset);
- connect_builder.add_node(node_offset);
- fbb.Finish(connect_builder.Finish());
-
- return fbb.Release();
-}
-
std::vector<int> StreamToChannel(const Configuration *config,
const Node *my_node, const Node *other_node) {
std::vector<int> stream_to_channel;
@@ -107,7 +74,7 @@
aos::FlatbufferDetachedBuffer<aos::logger::MessageHeader>
MakeMessageHeaderReply() {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
logger::MessageHeader::Builder message_header_builder(fbb);
message_header_builder.add_channel_index(0);
message_header_builder.add_monotonic_sent_time(0);
@@ -120,20 +87,25 @@
}
FlatbufferDetachedBuffer<ClientStatistics> MakeClientStatistics(
- const std::vector<std::string_view> &source_node_names,
- const Configuration *configuration) {
+ const std::vector<std::string_view> &source_node_names) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
std::vector<flatbuffers::Offset<ClientConnection>> connection_offsets;
for (const std::string_view node_name : source_node_names) {
- flatbuffers::Offset<Node> node_offset =
- CopyFlatBuffer(configuration::GetNode(configuration, node_name), &fbb);
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ fbb.CreateString(node_name);
+
+ Node::Builder node_builder(fbb);
+ node_builder.add_name(node_name_offset);
+ flatbuffers::Offset<Node> node_offset = node_builder.Finish();
+
ClientConnection::Builder connection_builder(fbb);
connection_builder.add_node(node_offset);
connection_builder.add_state(State::DISCONNECTED);
// TODO(austin): Track dropped packets.
connection_builder.add_received_packets(0);
+ connection_builder.add_monotonic_offset(0);
connection_offsets.emplace_back(connection_builder.Finish());
}
flatbuffers::Offset<
@@ -266,6 +238,26 @@
chrono::nanoseconds(message_header->realtime_sent_time())),
message_header->queue_index());
+ const std::chrono::nanoseconds offset =
+ sender->monotonic_sent_time() -
+ aos::monotonic_clock::time_point(
+ chrono::nanoseconds(message_header->monotonic_sent_time()));
+
+ // If this is our first observation, use that to seed the base offset. That
+ // gets us in the ballpark.
+ if (!filter_.has_sample()) {
+ filter_.set_base_offset(offset);
+ }
+
+ // We can now measure the latency!
+ filter_.Sample(sender->monotonic_sent_time(), offset);
+
+ connection_->mutate_monotonic_offset(
+ (chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(filter_.offset())) +
+ filter_.base_offset())
+ .count());
+
if (stream_reply_with_timestamp_[stream]) {
// TODO(austin): Send back less if we are only acking. Maybe only a
// stream id? Nothing if we are only forwarding?
@@ -316,8 +308,9 @@
sender_(event_loop_->MakeSender<ClientStatistics>("/aos")),
source_node_names_(configuration::SourceNodeNames(
event_loop->configuration(), event_loop->node())),
- statistics_(MakeClientStatistics(source_node_names_,
- event_loop->configuration())) {
+ statistics_(MakeClientStatistics(source_node_names_)) {
+ client_connection_offsets_.reserve(
+ statistics_.message().connections()->size());
std::string_view node_name = event_loop->node()->name()->string_view();
// Find all the channels which are supposed to be delivered to us.
@@ -355,10 +348,54 @@
// And kick it all off.
statistics_timer_ = event_loop_->AddTimer([this]() { SendStatistics(); });
event_loop_->OnRun([this]() {
- statistics_timer_->Setup(event_loop_->monotonic_now() + chrono::seconds(1),
- chrono::seconds(1));
+ statistics_timer_->Setup(
+ event_loop_->monotonic_now() + chrono::milliseconds(100),
+ chrono::milliseconds(100));
});
}
+void MessageBridgeClient::SendStatistics() {
+ // Copy from statistics_ and drop monotonic_offset if it isn't populated yet.
+ // There doesn't exist a good way to drop fields otherwise.
+ aos::Sender<ClientStatistics>::Builder builder = sender_.MakeBuilder();
+ client_connection_offsets_.clear();
+
+ for (const ClientConnection *connection :
+ *statistics_.message().connections()) {
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ builder.fbb()->CreateString(connection->node()->name()->string_view());
+ Node::Builder node_builder = builder.MakeBuilder<Node>();
+ node_builder.add_name(node_name_offset);
+ flatbuffers::Offset<Node> node_offset = node_builder.Finish();
+
+ ClientConnection::Builder client_connection_builder =
+ builder.MakeBuilder<ClientConnection>();
+
+ client_connection_builder.add_node(node_offset);
+ client_connection_builder.add_state(connection->state());
+ client_connection_builder.add_received_packets(
+ connection->received_packets());
+
+ // Strip out the monotonic offset if it isn't populated.
+ if (connection->monotonic_offset() != 0) {
+ client_connection_builder.add_monotonic_offset(
+ connection->monotonic_offset());
+ }
+
+ client_connection_offsets_.emplace_back(client_connection_builder.Finish());
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<ClientConnection>>>
+ client_connections_offset =
+ builder.fbb()->CreateVector(client_connection_offsets_);
+
+ ClientStatistics::Builder client_statistics_builder =
+ builder.MakeBuilder<ClientStatistics>();
+ client_statistics_builder.add_connections(client_connections_offset);
+
+ builder.Send(client_statistics_builder.Finish());
+}
+
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/message_bridge_client_lib.h b/aos/network/message_bridge_client_lib.h
index 77bf2b7..2811553 100644
--- a/aos/network/message_bridge_client_lib.h
+++ b/aos/network/message_bridge_client_lib.h
@@ -7,6 +7,7 @@
#include "aos/events/logging/logger_generated.h"
#include "aos/events/shm_event_loop.h"
#include "aos/network/connect_generated.h"
+#include "aos/network/timestamp_filter.h"
#include "aos/network/message_bridge_client_generated.h"
#include "aos/network/sctp_client.h"
#include "aos/network/sctp_lib.h"
@@ -76,6 +77,9 @@
// id of the server once known. This is only valid if connection_ says
// connected.
sctp_assoc_t remote_assoc_id_ = 0;
+
+ // Filter for the timestamp offset for this connection.
+ TimestampFilter filter_;
};
// This encapsulates the state required to talk to *all* the servers from this
@@ -89,7 +93,7 @@
private:
// Sends out the statistics that are continually updated by the
// SctpClientConnections.
- void SendStatistics() { sender_.Send(statistics_); }
+ void SendStatistics();
// Event loop to schedule everything on.
aos::ShmEventLoop *event_loop_;
@@ -102,6 +106,9 @@
// Data to publish.
FlatbufferDetachedBuffer<ClientStatistics> statistics_;
+ // Reserved memory for the client connection offsets to reduce heap
+ // allocations.
+ std::vector<flatbuffers::Offset<ClientConnection>> client_connection_offsets_;
// Channels to send data over.
std::vector<std::unique_ptr<aos::RawSender>> channels_;
diff --git a/aos/network/message_bridge_protocol.cc b/aos/network/message_bridge_protocol.cc
new file mode 100644
index 0000000..9663edf
--- /dev/null
+++ b/aos/network/message_bridge_protocol.cc
@@ -0,0 +1,49 @@
+#include "aos/network/message_bridge_protocol.h"
+
+#include <string_view>
+
+#include "aos/configuration.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/flatbuffers.h"
+#include "aos/network/connect_generated.h"
+#include "flatbuffers/flatbuffers.h"
+
+namespace aos {
+namespace message_bridge {
+
+aos::FlatbufferDetachedBuffer<aos::message_bridge::Connect> MakeConnectMessage(
+ const Configuration *config, const Node *my_node,
+ std::string_view remote_name) {
+ CHECK(config->has_nodes()) << ": Config must have nodes to transfer.";
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+
+ flatbuffers::Offset<Node> node_offset = CopyFlatBuffer<Node>(my_node, &fbb);
+ const std::string_view node_name = my_node->name()->string_view();
+
+ std::vector<flatbuffers::Offset<Channel>> channel_offsets;
+ for (const Channel *channel : *config->channels()) {
+ if (channel->has_destination_nodes()) {
+ for (const Connection *connection : *channel->destination_nodes()) {
+ if (connection->name()->string_view() == node_name &&
+ channel->source_node()->string_view() == remote_name) {
+ channel_offsets.emplace_back(CopyFlatBuffer<Channel>(channel, &fbb));
+ }
+ }
+ }
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Channel>>>
+ channels_offset = fbb.CreateVector(channel_offsets);
+
+ Connect::Builder connect_builder(fbb);
+ connect_builder.add_channels_to_transfer(channels_offset);
+ connect_builder.add_node(node_offset);
+ fbb.Finish(connect_builder.Finish());
+
+ return fbb.Release();
+}
+
+} // namespace message_bridge
+} // namespace aos
diff --git a/aos/network/message_bridge_protocol.h b/aos/network/message_bridge_protocol.h
index 1136188..7b6a248 100644
--- a/aos/network/message_bridge_protocol.h
+++ b/aos/network/message_bridge_protocol.h
@@ -1,6 +1,11 @@
#ifndef AOS_NETWORK_MESSAGE_BRIDGE_PROTOCOL_H_
#define AOS_NETWORK_MESSAGE_BRIDGE_PROTOCOL_H_
+#include <string_view>
+
+#include "aos/configuration.h"
+#include "aos/network/connect_generated.h"
+
namespace aos {
namespace message_bridge {
@@ -25,6 +30,11 @@
// The stream on which timestamp replies are sent.
constexpr size_t kTimestampStream() { return 1; }
+// Builds up a subscription request for my_node to remote_name.
+aos::FlatbufferDetachedBuffer<aos::message_bridge::Connect> MakeConnectMessage(
+ const Configuration *config, const Node *my_node,
+ std::string_view remote_name);
+
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/message_bridge_server.fbs b/aos/network/message_bridge_server.fbs
index 75a6a15..8f06fdb 100644
--- a/aos/network/message_bridge_server.fbs
+++ b/aos/network/message_bridge_server.fbs
@@ -22,6 +22,11 @@
// Number of packets received on all channels.
sent_packets:uint;
+ // This is the measured monotonic offset for the connected node in
+ // nanoseconds. Add this to our monotonic time to get their
+ // monotonic time.
+ monotonic_offset:int64;
+
// TODO(austin): Per channel counts?
}
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index 38958ba..f54e2bf 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -22,6 +22,7 @@
const std::vector<std::string_view> &source_node_names,
const Configuration *configuration) {
flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
std::vector<flatbuffers::Offset<ServerConnection>> connection_offsets;
for (const std::string_view node_name : source_node_names) {
@@ -32,6 +33,7 @@
connection_builder.add_state(State::DISCONNECTED);
connection_builder.add_dropped_packets(0);
connection_builder.add_sent_packets(0);
+ connection_builder.add_monotonic_offset(0);
connection_offsets.emplace_back(connection_builder.Finish());
}
flatbuffers::Offset<
@@ -78,6 +80,7 @@
// TODO(austin): I don't like allocating this buffer when we are just freeing
// it at the end of the function.
flatbuffers::FlatBufferBuilder fbb(channel_->max_size() + 100);
+ fbb.ForceDefaults(true);
VLOG(1) << "Found " << peers_.size() << " peers on channel "
<< channel_->name()->string_view() << " size " << context.size;
@@ -209,11 +212,52 @@
configuration::DestinationNodeNames(event_loop->configuration(),
event_loop->node()),
event_loop->configuration())),
+ timestamp_sender_(event_loop_->MakeSender<Timestamp>("/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<ClientStatistics>("/aos")),
server_("::", event_loop->node()->port()) {
CHECK(event_loop_->node() != nullptr) << ": No nodes configured.";
- // TODO(austin): Time sync. sctp gives us filtered round trip time, not
- // target time.
+ server_connection_offsets_.reserve(
+ statistics_.message().connections()->size());
+
+ int32_t max_size = 0;
+ timestamp_fetchers_.resize(event_loop->configuration()->nodes()->size());
+ filters_.resize(event_loop->configuration()->nodes()->size());
+ server_connection_.resize(event_loop->configuration()->nodes()->size());
+
+ // Seed up all the per-node connection state.
+ for (std::string_view destination_node_name :
+ configuration::DestinationNodeNames(event_loop->configuration(),
+ event_loop->node())) {
+ // Find the largest connection message so we can size our buffers big enough
+ // to receive a connection message.
+ max_size = std::max(
+ max_size,
+ static_cast<int32_t>(MakeConnectMessage(event_loop->configuration(),
+ event_loop->node(),
+ destination_node_name)
+ .size()));
+ const Node *destination_node = configuration::GetNode(
+ event_loop->configuration(), destination_node_name);
+
+ const int node_index = configuration::GetNodeIndex(
+ event_loop->configuration(), destination_node);
+
+ // Now find the timestamp channel forwarded from the other node.
+ const Channel *const other_timestamp_channel =
+ configuration::GetChannel(event_loop_->configuration(), "/aos",
+ Timestamp::GetFullyQualifiedName(),
+ event_loop_->name(), destination_node);
+
+ timestamp_fetchers_[node_index] = event_loop_->MakeFetcher<Timestamp>(
+ other_timestamp_channel->name()->string_view());
+
+ // And then find the server connection that we should be populating
+ // statistics into.
+ server_connection_[node_index] = FindServerConnection(
+ statistics_.mutable_message(), destination_node->name()->string_view());
+ }
// TODO(austin): Logging synchronization.
//
@@ -226,11 +270,16 @@
LOG(INFO) << "Hostname: " << event_loop_->node()->hostname()->string_view();
int channel_index = 0;
+ const Channel *const timestamp_channel = configuration::GetChannel(
+ event_loop_->configuration(), "/aos", Timestamp::GetFullyQualifiedName(),
+ event_loop_->name(), event_loop_->node());
+
for (const Channel *channel : *event_loop_->configuration()->channels()) {
CHECK(channel->has_source_node());
- if (channel->source_node()->string_view() ==
- event_loop_->node()->name()->string_view() &&
+
+ if (configuration::ChannelIsSendableOnNode(channel, event_loop_->node()) &&
channel->has_destination_nodes()) {
+ max_size = std::max(channel->max_size(), max_size);
std::unique_ptr<ChannelState> state(
new ChannelState{channel, channel_index});
@@ -244,13 +293,20 @@
configuration::ChannelMessageIsLoggedOnNode(channel, other_node));
}
- // Call SendData for every message.
- ChannelState *state_ptr = state.get();
- event_loop_->MakeRawWatcher(
- channel,
- [this, state_ptr](const Context &context, const void * /*message*/) {
- state_ptr->SendData(&server_, context);
- });
+ // Don't subscribe to timestamps on the timestamp channel. Those get
+ // handled specially.
+ if (channel != timestamp_channel) {
+ // Call SendData for every message.
+ ChannelState *state_ptr = state.get();
+ event_loop_->MakeRawWatcher(
+ channel, [this, state_ptr](const Context &context,
+ const void * /*message*/) {
+ state_ptr->SendData(&server_, context);
+ });
+ } else {
+ CHECK(timestamp_state_ == nullptr);
+ timestamp_state_ = state.get();
+ }
channels_.emplace_back(std::move(state));
} else {
channels_.emplace_back(nullptr);
@@ -258,10 +314,13 @@
++channel_index;
}
- statistics_timer_ = event_loop_->AddTimer([this]() { SendStatistics(); });
+ // Buffer up the max size a bit so everything fits nicely.
+ server_.SetMaxSize(max_size + 100);
+
+ statistics_timer_ = event_loop_->AddTimer([this]() { Tick(); });
event_loop_->OnRun([this]() {
- statistics_timer_->Setup(event_loop_->monotonic_now() + chrono::seconds(1),
- chrono::seconds(1));
+ statistics_timer_->Setup(event_loop_->monotonic_now() + kPingPeriod,
+ kPingPeriod);
});
}
@@ -363,5 +422,173 @@
}
}
+void MessageBridgeServer::SendStatistics() {
+ aos::Sender<ServerStatistics>::Builder builder = sender_.MakeBuilder();
+
+ server_connection_offsets_.clear();
+
+ // Copy the statistics over, but only add monotonic_offset if it is valid.
+ for (const ServerConnection *connection :
+ *statistics_.message().connections()) {
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ builder.fbb()->CreateString(connection->node()->name()->string_view());
+ Node::Builder node_builder = builder.MakeBuilder<Node>();
+ node_builder.add_name(node_name_offset);
+ flatbuffers::Offset<Node> node_offset = node_builder.Finish();
+
+ ServerConnection::Builder server_connection_builder =
+ builder.MakeBuilder<ServerConnection>();
+ server_connection_builder.add_node(node_offset);
+ server_connection_builder.add_state(connection->state());
+ server_connection_builder.add_dropped_packets(
+ connection->dropped_packets());
+ server_connection_builder.add_sent_packets(connection->sent_packets());
+
+ // TODO(austin): If it gets stale, drop it too.
+ if (connection->monotonic_offset() != 0) {
+ server_connection_builder.add_monotonic_offset(
+ connection->monotonic_offset());
+ }
+
+ server_connection_offsets_.emplace_back(server_connection_builder.Finish());
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<ServerConnection>>>
+ server_connections_offset =
+ builder.fbb()->CreateVector(server_connection_offsets_);
+
+ ServerStatistics::Builder server_statistics_builder =
+ builder.MakeBuilder<ServerStatistics>();
+ server_statistics_builder.add_connections(server_connections_offset);
+ builder.Send(server_statistics_builder.Finish());
+}
+
+void MessageBridgeServer::Tick() {
+ // Send statistics every kStatisticsPeriod. Use the context so we don't get
+ // caught up with the wakeup delay and jitter.
+ if (event_loop_->context().monotonic_event_time >=
+ last_statistics_send_time_ + kStatisticsPeriod) {
+ SendStatistics();
+ last_statistics_send_time_ = event_loop_->context().monotonic_event_time;
+ }
+
+ // The message_bridge_client application measures and filters the offsets from
+ // all messages it receives. It then sends this on in the ClientStatistics
+ // message. Collect that up and forward it back over the Timestamp message so
+ // we have guarenteed traffic on the other node for timestamping. This also
+ // moves the offsets back across the network so both directions can be
+ // observed.
+ client_statistics_fetcher_.Fetch();
+
+ // Build up the timestamp message. Do it here so that we don't have invalid
+ // data in it.
+ FlatbufferFixedAllocatorArray<Timestamp, 1000> timestamp_copy;
+ flatbuffers::FlatBufferBuilder *fbb = timestamp_copy.Builder();
+
+ if (client_statistics_fetcher_.get()) {
+ // Build up the list of client offsets.
+ std::vector<flatbuffers::Offset<ClientOffset>> client_offsets;
+
+ // Iterate through the connections this node has made.
+ for (const ClientConnection *connection :
+ *client_statistics_fetcher_->connections()) {
+ // Filter out the ones which aren't connected.
+ if (connection->state() != State::CONNECTED) continue;
+ // And the ones without monotonic offsets.
+ if (!connection->has_monotonic_offset()) continue;
+
+ const int node_index = configuration::GetNodeIndex(
+ event_loop_->configuration(),
+ connection->node()->name()->string_view());
+
+ timestamp_fetchers_[node_index].Fetch();
+
+ // Find the offset computed on their node for this client connection
+ // using their timestamp message.
+ bool has_their_offset = false;
+ std::chrono::nanoseconds their_offset = std::chrono::nanoseconds(0);
+ if (timestamp_fetchers_[node_index].get() != nullptr) {
+ for (const ClientOffset *client_offset :
+ *timestamp_fetchers_[node_index]->offsets()) {
+ if (client_offset->node()->name()->string_view() ==
+ event_loop_->node()->name()->string_view()) {
+ if (client_offset->has_monotonic_offset()) {
+ their_offset =
+ std::chrono::nanoseconds(client_offset->monotonic_offset());
+ has_their_offset = true;
+ }
+ break;
+ }
+ }
+ }
+
+ if (has_their_offset) {
+ // Update the filters.
+ if (filters_[node_index].MissingSamples()) {
+ // Update the offset the first time. This should be representative.
+ filters_[node_index].set_base_offset(
+ std::chrono::nanoseconds(connection->monotonic_offset()));
+ }
+ // The message_bridge_clients are the ones running the first filter. So
+ // set the values from that and let the averaging filter run from there.
+ filters_[node_index].FwdSet(
+ timestamp_fetchers_[node_index].context().monotonic_remote_time,
+ std::chrono::nanoseconds(connection->monotonic_offset()));
+ filters_[node_index].RevSet(
+ client_statistics_fetcher_.context().monotonic_event_time,
+ their_offset);
+
+ // Publish!
+ server_connection_[node_index]->mutate_monotonic_offset(
+ -filters_[node_index].offset().count());
+ }
+
+ // Now fill out the Timestamp message with the offset from the client.
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ fbb->CreateString(connection->node()->name()->string_view());
+
+ Node::Builder node_builder(*fbb);
+ node_builder.add_name(node_name_offset);
+ flatbuffers::Offset<Node> node_offset = node_builder.Finish();
+
+ ClientOffset::Builder client_offset_builder(*fbb);
+ client_offset_builder.add_node(node_offset);
+ client_offset_builder.add_monotonic_offset(
+ connection->monotonic_offset());
+ client_offsets.emplace_back(client_offset_builder.Finish());
+ }
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<ClientOffset>>>
+ offsets_offset = fbb->CreateVector(client_offsets);
+
+ Timestamp::Builder builder(*fbb);
+ builder.add_offsets(offsets_offset);
+ timestamp_copy.Finish(builder.Finish());
+ } else {
+ // Publish an empty timestamp if we have nothing.
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<ClientOffset>>>
+ offsets_offset =
+ fbb->CreateVector(std::vector<flatbuffers::Offset<ClientOffset>>{});
+ Timestamp::Builder builder(*fbb);
+ builder.add_offsets(offsets_offset);
+ timestamp_copy.Finish(builder.Finish());
+ }
+
+ // Send it out over shm, and using that timestamp, then send it out over sctp.
+ // This avoid some context switches.
+ timestamp_sender_.Send(timestamp_copy);
+
+ Context context;
+ context.monotonic_event_time = timestamp_sender_.monotonic_sent_time();
+ context.realtime_event_time = timestamp_sender_.realtime_sent_time();
+ context.queue_index = timestamp_sender_.sent_queue_index();
+ context.size = timestamp_copy.size();
+ context.data = timestamp_copy.data();
+
+ // Since we are building up the timestamp to send here, we need to trigger the
+ // SendData call ourselves.
+ timestamp_state_->SendData(&server_, context);
+}
+
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/message_bridge_server_lib.h b/aos/network/message_bridge_server_lib.h
index f202fc9..d271c8c 100644
--- a/aos/network/message_bridge_server_lib.h
+++ b/aos/network/message_bridge_server_lib.h
@@ -8,8 +8,10 @@
#include "aos/events/logging/logger_generated.h"
#include "aos/events/shm_event_loop.h"
#include "aos/network/connect_generated.h"
+#include "aos/network/message_bridge_client_generated.h"
#include "aos/network/message_bridge_server_generated.h"
#include "aos/network/sctp_server.h"
+#include "aos/network/timestamp_generated.h"
#include "glog/logging.h"
namespace aos {
@@ -105,9 +107,12 @@
// received.
void HandleData(const Message *message);
+ // Handle timestamps and statistics.
+ void Tick();
+
// Sends out the statistics that are continually updated by the
// ChannelState's.
- void SendStatistics() { sender_.Send(statistics_); }
+ void SendStatistics();
// Event loop to schedule everything on.
aos::ShmEventLoop *event_loop_;
@@ -116,9 +121,33 @@
aos::Sender<ServerStatistics> sender_;
aos::TimerHandler *statistics_timer_;
FlatbufferDetachedBuffer<ServerStatistics> statistics_;
+ std::vector<flatbuffers::Offset<ServerConnection>> server_connection_offsets_;
+
+ // Sender for the timestamps that we are forwarding over the network.
+ aos::Sender<Timestamp> timestamp_sender_;
+ // ChannelState to send timestamps over the network with.
+ ChannelState *timestamp_state_ = nullptr;
+
+ // Fetcher to grab the measured offsets in the client.
+ aos::Fetcher<ClientStatistics> client_statistics_fetcher_;
+ // All of these are indexed by the other node index.
+ // Fetcher to grab timestamps and therefore offsets from the other nodes.
+ std::vector<aos::Fetcher<Timestamp>> timestamp_fetchers_;
+ // Bidirectional filters for each connection.
+ std::vector<ClippedAverageFilter> filters_;
+ // ServerConnection to fill out the offsets for from each node.
+ std::vector<ServerConnection *> server_connection_;
SctpServer server_;
+ static constexpr std::chrono::nanoseconds kStatisticsPeriod =
+ std::chrono::seconds(1);
+ static constexpr std::chrono::nanoseconds kPingPeriod =
+ std::chrono::milliseconds(100);
+
+ aos::monotonic_clock::time_point last_statistics_send_time_ =
+ aos::monotonic_clock::min_time;
+
// List of channels. The entries that aren't sent from this node are left
// null.
std::vector<std::unique_ptr<ChannelState>> channels_;
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 383c1c4..c1dd1ca 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -49,8 +49,12 @@
FLAGS_application_name = "pi1_message_bridge_server";
// Force ourselves to be "raspberrypi" and allocate everything.
FLAGS_override_hostname = "raspberrypi";
- aos::ShmEventLoop server_event_loop(&server_config.message());
- MessageBridgeServer message_bridge_server(&server_event_loop);
+ aos::ShmEventLoop pi1_server_event_loop(&server_config.message());
+ MessageBridgeServer pi1_message_bridge_server(&pi1_server_event_loop);
+
+ FLAGS_application_name = "pi1_message_bridge_client";
+ aos::ShmEventLoop pi1_client_event_loop(&server_config.message());
+ MessageBridgeClient pi1_message_bridge_client(&pi1_client_event_loop);
// And build the app which sends the pings.
FLAGS_application_name = "ping";
@@ -61,42 +65,41 @@
// Now do it for "raspberrypi2", the client.
FLAGS_application_name = "pi2_message_bridge_client";
FLAGS_override_hostname = "raspberrypi2";
- aos::ShmEventLoop client_event_loop(&client_config.message());
- MessageBridgeClient message_bridge_client(&client_event_loop);
+ aos::ShmEventLoop pi2_client_event_loop(&client_config.message());
+ MessageBridgeClient pi2_message_bridge_client(&pi2_client_event_loop);
+
+ FLAGS_application_name = "pi2_message_bridge_server";
+ aos::ShmEventLoop pi2_server_event_loop(&client_config.message());
+ MessageBridgeServer pi2_message_bridge_server(&pi2_server_event_loop);
// And build the app which sends the pongs.
FLAGS_application_name = "pong";
aos::ShmEventLoop pong_event_loop(&client_config.message());
+ // And build the app for testing.
+ FLAGS_application_name = "test";
+ aos::ShmEventLoop test_event_loop(&client_config.message());
+
+ aos::Fetcher<ClientStatistics> client_statistics_fetcher =
+ test_event_loop.MakeFetcher<ClientStatistics>("/aos");
+
// Count the pongs.
int pong_count = 0;
pong_event_loop.MakeWatcher(
- "/test2", [&pong_count, &ping_event_loop](const examples::Ping &ping) {
+ "/test2", [&pong_count](const examples::Ping &ping) {
++pong_count;
LOG(INFO) << "Got ping back " << FlatbufferToJson(&ping);
- if (pong_count >= 2) {
- LOG(INFO) << "That's enough bailing early.";
- // And Exit is async safe, so thread safe is easy.
- ping_event_loop.Exit();
- }
});
FLAGS_override_hostname = "";
- // Start everything up. Pong is the only thing we don't know how to wait on,
- // so start it first.
- std::thread pong_thread([&pong_event_loop]() { pong_event_loop.Run(); });
-
- std::thread server_thread(
- [&server_event_loop]() { server_event_loop.Run(); });
- std::thread client_thread(
- [&client_event_loop]() { client_event_loop.Run(); });
-
// Wait until we are connected, then send.
int ping_count = 0;
+ int pi1_server_statistics_count = 0;
ping_event_loop.MakeWatcher(
- "/aos/pi1", [&ping_count, &client_event_loop,
- &ping_sender](const ServerStatistics &stats) {
+ "/aos/pi1",
+ [&ping_count, &pi2_client_event_loop, &ping_sender,
+ &pi1_server_statistics_count](const ServerStatistics &stats) {
LOG(INFO) << FlatbufferToJson(&stats);
ASSERT_TRUE(stats.has_connections());
@@ -104,12 +107,21 @@
bool connected = false;
for (const ServerConnection *connection : *stats.connections()) {
+ // Confirm that we are estimating the server time offset correctly. It
+ // should be about 0 since we are on the same machine here.
+ if (connection->has_monotonic_offset()) {
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(1));
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(-1));
+ ++pi1_server_statistics_count;
+ }
+
if (connection->node()->name()->string_view() ==
- client_event_loop.node()->name()->string_view()) {
+ pi2_client_event_loop.node()->name()->string_view()) {
if (connection->state() == State::CONNECTED) {
connected = true;
}
- break;
}
}
@@ -124,29 +136,136 @@
}
});
- // Time ourselves out after a while if Pong doesn't do it for us.
+ // Confirm both client and server statistics messages have decent offsets in
+ // them.
+ int pi2_server_statistics_count = 0;
+ pong_event_loop.MakeWatcher("/aos/pi2", [&pi2_server_statistics_count](
+ const ServerStatistics &stats) {
+ LOG(INFO) << FlatbufferToJson(&stats);
+ for (const ServerConnection *connection : *stats.connections()) {
+ if (connection->has_monotonic_offset()) {
+ ++pi2_server_statistics_count;
+ // Confirm that we are estimating the server time offset correctly. It
+ // should be about 0 since we are on the same machine here.
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(1));
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(-1));
+ }
+ }
+ });
+
+ int pi1_client_statistics_count = 0;
+ ping_event_loop.MakeWatcher(
+ "/aos/pi1", [&pi1_client_statistics_count](const ClientStatistics &stats) {
+ LOG(INFO) << FlatbufferToJson(&stats);
+
+ for (const ClientConnection *connection : *stats.connections()) {
+ if (connection->has_monotonic_offset()) {
+ ++pi1_client_statistics_count;
+ // It takes at least 10 microseconds to send a message between the
+ // client and server. The min (filtered) time shouldn't be over 10
+ // milliseconds on localhost. This might have to bump up if this is
+ // proving flaky.
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(10));
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::microseconds(10));
+ }
+ }
+ });
+
+ int pi2_client_statistics_count = 0;
+ pong_event_loop.MakeWatcher("/aos/pi2", [&pi2_client_statistics_count](
+ const ClientStatistics &stats) {
+ LOG(INFO) << FlatbufferToJson(&stats);
+
+ for (const ClientConnection *connection : *stats.connections()) {
+ if (connection->has_monotonic_offset()) {
+ ++pi2_client_statistics_count;
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(10));
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::microseconds(10));
+ }
+ }
+ });
+
+ ping_event_loop.MakeWatcher("/aos/pi1", [](const Timestamp ×tamp) {
+ EXPECT_TRUE(timestamp.has_offsets());
+ LOG(INFO) << FlatbufferToJson(×tamp);
+ });
+ pong_event_loop.MakeWatcher("/aos/pi2", [](const Timestamp ×tamp) {
+ EXPECT_TRUE(timestamp.has_offsets());
+ LOG(INFO) << FlatbufferToJson(×tamp);
+ });
+
+ // Run for 5 seconds to make sure we have time to estimate the offset.
aos::TimerHandler *quit = ping_event_loop.AddTimer(
[&ping_event_loop]() { ping_event_loop.Exit(); });
ping_event_loop.OnRun([quit, &ping_event_loop]() {
- quit->Setup(ping_event_loop.monotonic_now() + chrono::seconds(10));
+ // Stop between timestamps, not exactly on them.
+ quit->Setup(ping_event_loop.monotonic_now() + chrono::milliseconds(5050));
});
+ // Start everything up. Pong is the only thing we don't know how to wait on,
+ // so start it first.
+ std::thread pong_thread([&pong_event_loop]() { pong_event_loop.Run(); });
+
+ std::thread pi1_server_thread(
+ [&pi1_server_event_loop]() { pi1_server_event_loop.Run(); });
+ std::thread pi1_client_thread(
+ [&pi1_client_event_loop]() { pi1_client_event_loop.Run(); });
+ std::thread pi2_client_thread(
+ [&pi2_client_event_loop]() { pi2_client_event_loop.Run(); });
+ std::thread pi2_server_thread(
+ [&pi2_server_event_loop]() { pi2_server_event_loop.Run(); });
// And go!
ping_event_loop.Run();
// Shut everyone else down
- server_event_loop.Exit();
- client_event_loop.Exit();
+ pi1_server_event_loop.Exit();
+ pi1_client_event_loop.Exit();
+ pi2_client_event_loop.Exit();
+ pi2_server_event_loop.Exit();
pong_event_loop.Exit();
- server_thread.join();
- client_thread.join();
+ pi1_server_thread.join();
+ pi1_client_thread.join();
+ pi2_client_thread.join();
+ pi2_server_thread.join();
pong_thread.join();
// Make sure we sent something.
EXPECT_GE(ping_count, 1);
// And got something back.
EXPECT_GE(pong_count, 1);
+
+ // Confirm that we are estimating a monotonic offset on the client.
+ ASSERT_TRUE(client_statistics_fetcher.Fetch());
+
+ EXPECT_EQ(client_statistics_fetcher->connections()->size(), 1u);
+ EXPECT_EQ(client_statistics_fetcher->connections()
+ ->Get(0)
+ ->node()
+ ->name()
+ ->string_view(),
+ "pi1");
+
+ // Make sure the offset in one direction is less than a second.
+ EXPECT_GT(
+ client_statistics_fetcher->connections()->Get(0)->monotonic_offset(), 0);
+ EXPECT_LT(
+ client_statistics_fetcher->connections()->Get(0)->monotonic_offset(),
+ 1000000000);
+
+ EXPECT_GE(pi1_server_statistics_count, 2);
+ EXPECT_GE(pi2_server_statistics_count, 2);
+ EXPECT_GE(pi1_client_statistics_count, 2);
+ EXPECT_GE(pi2_client_statistics_count, 2);
+
+ // TODO(austin): Need 2 servers going so we can do the round trip offset
+ // estimation.
}
} // namespace testing
diff --git a/aos/network/message_bridge_test_client.json b/aos/network/message_bridge_test_client.json
index 65d28d2..d748225 100644
--- a/aos/network/message_bridge_test_client.json
+++ b/aos/network/message_bridge_test_client.json
@@ -11,7 +11,7 @@
{
"name": "pi2",
"hostname": "raspberrypi2",
- "port": 9971
+ "port": 9972
}
]
}
diff --git a/aos/network/message_bridge_test_common.json b/aos/network/message_bridge_test_common.json
index a073869..1e5dfce 100644
--- a/aos/network/message_bridge_test_common.json
+++ b/aos/network/message_bridge_test_common.json
@@ -18,6 +18,46 @@
},
{
"name": "/aos/pi1",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi1",
+ "frequency": 10,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "priority": 1
+ }
+ ]
+ },
+ {
+ "name": "/aos/pi2",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi2",
+ "frequency": 10,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 1
+ }
+ ]
+ },
+ {
+ "name": "/aos/pi1_forwarded",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi2",
+ "frequency": 10,
+ "max_size": 200
+ },
+ {
+ "name": "/aos/pi2_forwarded",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi1",
+ "frequency": 10,
+ "max_size": 200
+ },
+ {
+ "name": "/aos/pi1",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi1",
"frequency": 2
@@ -90,22 +130,6 @@
]
}
],
- "applications": [
- {
- "name": "pi2_message_bridge_client",
- "maps": [
- {
- "match": {
- "name": "/test",
- "type": "aos.examples.Ping"
- },
- "rename": {
- "name": "/test2"
- }
- }
- ]
- }
- ],
"maps": [
{
"match": {
@@ -124,6 +148,34 @@
"rename": {
"name": "/aos/pi2"
}
+ },
+ {
+ "match": {
+ "name": "/test",
+ "type": "aos.examples.Ping",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/test2"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos/pi1",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/aos/pi1_forwarded"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos/pi2",
+ "source_node": "pi1"
+ },
+ "rename": {
+ "name": "/aos/pi2_forwarded"
+ }
}
]
}
diff --git a/aos/network/message_bridge_test_server.json b/aos/network/message_bridge_test_server.json
index eea92e7..35cedb6 100644
--- a/aos/network/message_bridge_test_server.json
+++ b/aos/network/message_bridge_test_server.json
@@ -11,7 +11,7 @@
{
"name": "pi2",
"hostname": "localhost",
- "port": 9971
+ "port": 9972
}
]
}
diff --git a/aos/network/sctp_client.h b/aos/network/sctp_client.h
index 926e59b..5b6df3b 100644
--- a/aos/network/sctp_client.h
+++ b/aos/network/sctp_client.h
@@ -43,6 +43,8 @@
void LogSctpStatus(sctp_assoc_t assoc_id);
+ void set_max_size(size_t max_size) { max_size_ = max_size; }
+
private:
struct sockaddr_storage sockaddr_remote_;
struct sockaddr_storage sockaddr_local_;
diff --git a/aos/network/sctp_lib.cc b/aos/network/sctp_lib.cc
index 1ed816b..5af994e 100644
--- a/aos/network/sctp_lib.cc
+++ b/aos/network/sctp_lib.cc
@@ -175,9 +175,9 @@
memset(&inmessage, 0, sizeof(struct msghdr));
aos::unique_c_ptr<Message> result(
- reinterpret_cast<Message *>(malloc(sizeof(Message) + max_size)));
+ reinterpret_cast<Message *>(malloc(sizeof(Message) + max_size + 1)));
- iov.iov_len = max_size;
+ iov.iov_len = max_size + 1;
iov.iov_base = result->mutable_data();
inmessage.msg_iov = &iov;
@@ -193,6 +193,7 @@
PCHECK((size = recvmsg(fd, &inmessage, 0)) > 0);
result->size = size;
+ CHECK_LE(size, max_size) << ": Message overflowed buffer.";
if ((MSG_NOTIFICATION & inmessage.msg_flags)) {
result->message_type = Message::kNotification;
diff --git a/aos/network/sctp_server.cc b/aos/network/sctp_server.cc
index 70d5b28..5fd9f53 100644
--- a/aos/network/sctp_server.cc
+++ b/aos/network/sctp_server.cc
@@ -63,8 +63,7 @@
PCHECK(listen(fd_, 100) == 0);
- PCHECK(setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &max_size_,
- sizeof(max_size_)) == 0);
+ SetMaxSize(1000);
}
aos::unique_c_ptr<Message> SctpServer::Read() {
diff --git a/aos/network/sctp_server.h b/aos/network/sctp_server.h
index a3086d9..b702aa8 100644
--- a/aos/network/sctp_server.h
+++ b/aos/network/sctp_server.h
@@ -46,11 +46,19 @@
void SetStreamPriority(sctp_assoc_t assoc_id, int stream_id,
uint16_t priority);
+ void SetMaxSize(size_t max_size) {
+ max_size_ = max_size;
+ // Have the kernel give us a factor of 10 more. This lets us have more than
+ // one full sized packet in flight.
+ max_size = max_size * 10;
+ PCHECK(setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &max_size,
+ sizeof(max_size)) == 0);
+ }
+
private:
struct sockaddr_storage sockaddr_local_;
int fd_;
- // TODO(austin): Configure this.
size_t max_size_ = 1000;
int ppid_ = 1;
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 1b90079..14fadab 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -5,16 +5,15 @@
#include <unistd.h>
#include <stdlib.h>
-#include <string>
+#include "glog/logging.h"
-#include "absl/base/call_once.h"
-#include "aos/logging/logging.h"
#include "aos/util/string_to_num.h"
namespace aos {
namespace network {
-namespace internal {
-int ParseTeamNumber(const std::string &hostname, uint16_t *teamnumber) {
+namespace team_number_internal {
+
+std::optional<uint16_t> ParseRoborioTeamNumber(const std::string &hostname) {
for (size_t i = 0; i < hostname.size(); i++) {
if (hostname[i] == '-') {
const std::string num_as_s =
@@ -24,45 +23,79 @@
int num;
if (!::aos::util::StringToNumber(num_as_s, &num)) {
- return -1;
+ return std::nullopt;
}
if (hostname.substr(0, i) == "roboRIO" &&
std::to_string(num) == num_as_s) {
- *teamnumber = num;
- return 0;
- } else {
- return -1;
+ return num;
}
+ return std::nullopt;
}
}
- return -1;
+ return std::nullopt;
}
-} // namespace internal
+
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname) {
+ if (hostname.substr(0, 3) != "pi-") {
+ return std::nullopt;
+ }
+ size_t first_separator = hostname.find('-');
+ if (first_separator == hostname.npos ||
+ first_separator >= hostname.size() - 2) {
+ return std::nullopt;
+ }
+ ++first_separator;
+ const size_t second_separator = hostname.find('-', first_separator);
+ if (second_separator == hostname.npos) {
+ return std::nullopt;
+ }
+ const std::string number_string =
+ hostname.substr(first_separator, second_separator - first_separator);
+ int number;
+ if (!util::StringToNumber(number_string, &number)) {
+ return std::nullopt;
+ }
+ return number;
+}
+
+} // namespace team_number_internal
namespace {
uint16_t override_team;
-void DoGetTeamNumber(uint16_t *result) {
+uint16_t DoGetTeamNumber() {
if (override_team != 0) {
- *result = override_team;
- return;
+ return override_team;
}
const char *override_number = getenv("AOS_TEAM_NUMBER");
if (override_number != nullptr) {
- if (!::aos::util::StringToNumber(override_number, result)) {
- AOS_LOG(FATAL, "error parsing AOS_TEAM_NUMBER '%s'\n", override_number);
+ uint16_t result;
+ if (!::aos::util::StringToNumber(override_number, &result)) {
+ LOG(FATAL) << "Error parsing AOS_TEAM_NUMBER: " << override_number;
}
- AOS_LOG(WARNING,
- "team number overridden by AOS_TEAM_NUMBER to %" PRIu16 "\n", *result);
- } else {
- int error = internal::ParseTeamNumber(GetHostname(), result);
- if (error) {
- AOS_LOG(FATAL, "Invalid hostname %s\n", GetHostname().c_str());
- }
- AOS_LOG(INFO, "team number is %" PRIu16 "\n", *result);
+ LOG(WARNING)
+ << "Team number overriden by AOS_TEAM_NUMBER environment variable to "
+ << result;
+ return result;
}
+ const auto hostname = GetHostname();
+ {
+ const auto result = team_number_internal::ParseRoborioTeamNumber(hostname);
+ if (result) {
+ LOG(INFO) << "roboRIO hostname team number is: " << *result;
+ return *result;
+ }
+ }
+ {
+ const auto result = team_number_internal::ParsePiTeamNumber(hostname);
+ if (result) {
+ LOG(INFO) << "Pi hostname team number is: " << *result;
+ return *result;
+ }
+ }
+ LOG(FATAL) << "Failed to parse a team number from hostname: " << hostname;
}
} // namespace
@@ -70,14 +103,12 @@
::std::string GetHostname() {
char buf[256];
buf[sizeof(buf) - 1] = '\0';
- AOS_PCHECK(gethostname(buf, sizeof(buf) - 1));
+ PCHECK(gethostname(buf, sizeof(buf) - 1) == 0);
return buf;
}
uint16_t GetTeamNumber() {
- static absl::once_flag once;
- static uint16_t result;
- absl::call_once(once, DoGetTeamNumber, &result);
+ const static uint16_t result = DoGetTeamNumber();
return result;
}
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index cacc2b3..618affb 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -3,6 +3,7 @@
#include <stdint.h>
+#include <optional>
#include <string>
namespace aos {
@@ -23,9 +24,13 @@
// Guaranteed to be safe to call during static initialization time.
void OverrideTeamNumber(uint16_t team);
-namespace internal {
-int ParseTeamNumber(const std::string &hostname, uint16_t *teamnumber);
-} // namespace internal
+namespace team_number_internal {
+
+std::optional<uint16_t> ParseRoborioTeamNumber(const std::string &hostname);
+
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname);
+
+} // namespace team_number_internal
} // namespace network
} // namespace aos
diff --git a/aos/network/team_number_test.cc b/aos/network/team_number_test.cc
index ee3f2db..c11f0a0 100644
--- a/aos/network/team_number_test.cc
+++ b/aos/network/team_number_test.cc
@@ -2,32 +2,45 @@
#include "gtest/gtest.h"
-#include "aos/macros.h"
-
namespace aos {
namespace network {
-namespace internal {
namespace testing {
-TEST(TeamNumberTest, Parse2015TeamNumber) {
- uint16_t team_number;
- EXPECT_EQ(0, ParseTeamNumber("roboRIO-971", &team_number));
- EXPECT_EQ(971u, team_number);
+using team_number_internal::ParseRoborioTeamNumber;
+using team_number_internal::ParsePiTeamNumber;
- EXPECT_EQ(0, ParseTeamNumber("roboRIO-8971", &team_number));
- EXPECT_EQ(8971u, team_number);
+TEST(TeamNumberTest, Parse2015TeamNumber) {
+ EXPECT_EQ(971u, *ParseRoborioTeamNumber("roboRIO-971"));
+
+ EXPECT_EQ(8971u, ParseRoborioTeamNumber("roboRIO-8971"));
+
+ EXPECT_FALSE(ParseRoborioTeamNumber("abc"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO-8abc"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO-"));
}
TEST(TeamNumberTest, Parse2016TeamNumber) {
- uint16_t team_number;
- EXPECT_EQ(0, ParseTeamNumber("roboRIO-971-FRC", &team_number));
- EXPECT_EQ(971u, team_number);
+ EXPECT_EQ(971u, *ParseRoborioTeamNumber("roboRIO-971-FRC"));
- EXPECT_EQ(0, ParseTeamNumber("roboRIO-8971-FRC", &team_number));
- EXPECT_EQ(8971u, team_number);
+ EXPECT_EQ(8971u, *ParseRoborioTeamNumber("roboRIO-8971-FRC"));
+
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO-8abc-FRC"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO-8971-FRC2"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO-8971-2FRC"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO--FRC"));
+}
+
+TEST(TeamNumberTest, ParsePiTeamNumber) {
+ EXPECT_EQ(971u, *ParsePiTeamNumber("pi-971-1"));
+ EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-22"));
+ EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-"));
+
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-971"));
+ EXPECT_FALSE(ParseRoborioTeamNumber("pi-971a-1"));
}
} // namespace testing
-} // namespace internal
} // namespace network
} // namespace aos
diff --git a/aos/network/timestamp.fbs b/aos/network/timestamp.fbs
new file mode 100644
index 0000000..299ecaf
--- /dev/null
+++ b/aos/network/timestamp.fbs
@@ -0,0 +1,15 @@
+include "aos/configuration.fbs";
+
+namespace aos.message_bridge;
+
+table ClientOffset {
+ node:Node;
+
+ monotonic_offset:int64;
+}
+
+table Timestamp {
+ offsets:[ClientOffset];
+}
+
+root_type Timestamp;
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index b4280f6..6f2baca 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -24,6 +24,18 @@
// much about precision when solving for the global offset.
class TimestampFilter {
public:
+ // Forces the offset and time to the provided sample without filtering. Used
+ // for syncing with a remote filter calculation.
+ void Set(aos::monotonic_clock::time_point monotonic_now,
+ std::chrono::nanoseconds sample_ns) {
+ const double sample =
+ std::chrono::duration_cast<std::chrono::duration<double>>(sample_ns -
+ base_offset_)
+ .count();
+ offset_ = sample;
+ last_time_ = monotonic_now;
+ }
+
// Updates with a new sample. monotonic_now is the timestamp of the sample on
// the destination node, and sample_ns is destination_time - source_time.
void Sample(aos::monotonic_clock::time_point monotonic_now,
@@ -132,6 +144,13 @@
}
}
+ // Sets the forward sample without filtering. See FwdSample for more details.
+ void FwdSet(aos::monotonic_clock::time_point monotonic_now,
+ std::chrono::nanoseconds sample_ns) {
+ fwd_.Set(monotonic_now, sample_ns);
+ Update(monotonic_now, &last_fwd_time_);
+ }
+
// Adds a forward sample. sample_ns = destination - source; Forward samples
// are from A -> B.
void FwdSample(aos::monotonic_clock::time_point monotonic_now,
@@ -156,6 +175,13 @@
}
}
+ // Sets the forward sample without filtering. See FwdSample for more details.
+ void RevSet(aos::monotonic_clock::time_point monotonic_now,
+ std::chrono::nanoseconds sample_ns) {
+ rev_.Set(monotonic_now, sample_ns);
+ Update(monotonic_now, &last_rev_time_);
+ }
+
// Adds a reverse sample. sample_ns = destination - source; Reverse samples
// are B -> A.
void RevSample(aos::monotonic_clock::time_point monotonic_now,
@@ -214,6 +240,11 @@
last_rev_time_ = aos::monotonic_clock::min_time;
}
+ bool MissingSamples() {
+ return (last_fwd_time_ == aos::monotonic_clock::min_time) ||
+ (last_rev_time_ == aos::monotonic_clock::min_time);
+ }
+
private:
// Updates the offset estimate given the current time, and a pointer to the
// variable holding the last time.
@@ -226,7 +257,6 @@
const double hard_max = fwd_.offset();
const double hard_min = -rev_.offset();
const double average = (hard_max + hard_min) / 2.0;
- LOG(INFO) << "max " << hard_max << " min " << hard_min;
// We don't want to clip the offset to the hard min/max. We really want to
// keep it within a band around the middle. ratio of 0.5 means stay within
// +- 0.25 of the middle of the hard min and max.
diff --git a/aos/testdata/good_multinode_hostnames.json b/aos/testdata/good_multinode_hostnames.json
new file mode 100644
index 0000000..b82f6f3
--- /dev/null
+++ b/aos/testdata/good_multinode_hostnames.json
@@ -0,0 +1,18 @@
+{
+ "nodes": [
+ {
+ "name": "pi1",
+ "hostnames": [
+ "raspberrypi"
+ ]
+ },
+ {
+ "name": "pi2",
+ "hostnames": [
+ "raspberrypi2",
+ "raspberrypi3",
+ "other"
+ ]
+ }
+ ]
+}
diff --git a/frc971/config/BUILD b/frc971/config/BUILD
index 0f4f7ba..098d1f8 100644
--- a/frc971/config/BUILD
+++ b/frc971/config/BUILD
@@ -8,6 +8,7 @@
srcs = ["setup_roborio.sh"],
data = [
":rio_robotCommand",
+ ":sctp.ko",
"@arm_frc_linux_gnueabi_repo//:compiler_pieces",
],
visibility = ["//visibility:public"],
diff --git a/frc971/config/sctp.ko b/frc971/config/sctp.ko
new file mode 100644
index 0000000..81e41ec
--- /dev/null
+++ b/frc971/config/sctp.ko
Binary files differ
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
index 8c2be86..f288df8 100755
--- a/frc971/config/setup_roborio.sh
+++ b/frc971/config/setup_roborio.sh
@@ -34,6 +34,15 @@
ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current robot_code/aos_log-current
fi
+if [[ "$(ssh admin@${ROBOT_HOSTNAME} uname -r)" != "4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189" ]]; then
+ echo "Target roboRIO has the wrong kernel"
+ exit 1
+fi
+
+ssh "admin@${ROBOT_HOSTNAME}" mkdir "/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/"
+scp frc971/config/sctp.ko "admin@${ROBOT_HOSTNAME}:/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/sctp.ko"
+ssh "admin@${ROBOT_HOSTNAME}" depmod
+
# This fails if the code isn't running.
ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t' || true
diff --git a/frc971/constants.h b/frc971/constants.h
index a52add2..debfb55 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -98,6 +98,8 @@
double upper;
double middle() const { return (lower_hard + upper_hard) / 2.0; }
+
+ double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 26b27e9..a6d02f6 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -136,7 +136,7 @@
Eigen::AngleAxis<double>(kPitch, uy) *
Eigen::AngleAxis<double>(kRoll, ux));
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
fbb.Finish(dtukf.PopulateStatus(&fbb));
aos::FlatbufferDetachedBuffer<drivetrain::DownEstimatorState> state(
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 180af4b..c6bedf6 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -51,7 +51,7 @@
void Iterate() {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
Goal::Builder goal_builder(fbb);
goal_builder.add_throttle(driver_model_(state_));
goal_builder.add_controller_type(freeze_target_
@@ -105,7 +105,7 @@
double GoalTheta(double x, double y, double v, double throttle) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
Goal::Builder goal_builder(fbb);
goal_builder.add_throttle(throttle);
fbb.Finish(goal_builder.Finish());
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 38dbd53..13f74fa 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -127,7 +127,7 @@
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
- Eigen::Matrix<double, 4, 1>::Constant(-1));
+ Eigen::Matrix<double, 4, 1>::Constant(-1), std::chrono::milliseconds(5));
// Build a plant.
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index e4b2f25..185133b 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -237,4 +237,11 @@
unsafe_goal:double;
profile_params:frc971.ProfileParameters;
+
+ // Sets the goal velocity of the subsystem.
+ goal_velocity:double;
+
+ // If set to true, then we will ignore the profiling on this joint and pass
+ // the goal + goal velocity directly to the control loop.
+ ignore_profile:bool;
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 2752dcc..b9ec5c5 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -164,9 +164,13 @@
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
+ // Sets whether to use the trapezoidal profiler or whether to just bypass it
+ // and pass the unprofiled goal through directly.
+ void set_enable_profile(bool enable) { enable_profile_ = enable; }
// Sets the unprofiled goal. The profiler will generate a profile to go to
// this goal.
- void set_unprofiled_goal(double unprofiled_goal);
+ void set_unprofiled_goal(double unprofiled_goal,
+ double unprofiled_goal_velocity = 0.0);
// Limits our profiles to a max velocity and acceleration for proper motion.
void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
@@ -196,6 +200,7 @@
void UpdateOffset(double offset);
aos::util::TrapezoidProfile profile_;
+ bool enable_profile_ = true;
// Current measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -338,9 +343,9 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
- double unprofiled_goal) {
+ double unprofiled_goal, double unprofiled_goal_velocity) {
this->unprofiled_goal_(0, 0) = unprofiled_goal;
- this->unprofiled_goal_(1, 0) = 0.0;
+ this->unprofiled_goal_(1, 0) = unprofiled_goal_velocity;
this->unprofiled_goal_(2, 0) = 0.0;
CapGoal("unprofiled R", &this->unprofiled_goal_);
}
@@ -357,12 +362,21 @@
}
if (!disable) {
- ::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
- this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));
+ if (enable_profile_) {
+ ::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
+ this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));
- this->loop_->mutable_next_R(0, 0) = goal_state(0, 0);
- this->loop_->mutable_next_R(1, 0) = goal_state(1, 0);
- this->loop_->mutable_next_R(2, 0) = 0.0;
+ this->loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ this->loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ this->loop_->mutable_next_R(2, 0) = 0.0;
+ } else {
+ this->loop_->mutable_R() = this->unprofiled_goal_;
+ this->loop_->mutable_next_R() = this->unprofiled_goal_;
+ this->loop_->mutable_next_R(0, 0) +=
+ this->unprofiled_goal_(1) *
+ aos::time::DurationInSeconds(this->loop_->plant().coefficients().dt);
+ CapGoal("R", &this->loop_->mutable_R());
+ }
CapGoal("next R", &this->loop_->mutable_next_R());
}
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 36da41a..bd4447b 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -22,6 +22,13 @@
kalman_q_voltage,
kalman_r_position,
dt=0.00505):
+ """Constructs an AngularSystemParams object.
+
+ Args:
+ motor: Motor object with the motor constants.
+ G: float, Gear ratio. Less than 1 means output moves slower than the
+ input.
+ """
self.name = name
self.motor = motor
self.G = G
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 919968e..27a659a 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -183,6 +183,7 @@
fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
header_file_name))
fd.write('\n')
+ fd.write('#include <chrono>\n')
fd.write('#include <vector>\n')
fd.write('\n')
fd.write(
@@ -389,8 +390,10 @@
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
ans.append(
+ ' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
+ ans.append(
' return %s'
- '(A, B, C, D, U_max, U_min);\n' % (plant_coefficient_type))
+ '(A, B, C, D, U_max, U_min, dt);\n' % (plant_coefficient_type))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
self._DumpMatrix('A_continuous', self.A_continuous,
@@ -685,3 +688,7 @@
(12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ # Diameter of 1.9", weight of: 100 grams
+ # TODO(austin): Get a number from Scott Westbrook for the mass
+ self.motor_inertia = 0.1 * ((0.95 * 0.0254) ** 2.0)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 81783cc..83a54f0 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -39,7 +39,8 @@
C(other.C),
D(other.D),
U_min(other.U_min),
- U_max(other.U_max) {}
+ U_max(other.U_max),
+ dt(other.dt) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
@@ -47,8 +48,9 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
- : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min,
+ const std::chrono::nanoseconds dt)
+ : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max), dt(dt) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
@@ -56,6 +58,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
+ const std::chrono::nanoseconds dt;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
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 e115b0e..948ed5d 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -203,7 +203,11 @@
AOS_LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
safe_goal = max_position_;
}
- profiled_subsystem_.set_unprofiled_goal(safe_goal);
+ if (goal->has_ignore_profile()) {
+ profiled_subsystem_.set_enable_profile(!goal->ignore_profile());
+ }
+ profiled_subsystem_.set_unprofiled_goal(safe_goal,
+ goal->goal_velocity());
}
} break;
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 d515a07..026d372 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
@@ -288,6 +288,12 @@
const auto params_builder_offset = params_builder.Finish();
StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
goal_builder.add_unsafe_goal(unsafe_goal->unsafe_goal());
+ if (unsafe_goal->has_goal_velocity()) {
+ goal_builder.add_goal_velocity(unsafe_goal->goal_velocity());
+ }
+ if (unsafe_goal->has_ignore_profile()) {
+ goal_builder.add_ignore_profile(unsafe_goal->ignore_profile());
+ }
goal_builder.add_profile_params(params_builder_offset);
fbb.Finish(goal_builder.Finish());
} else {
@@ -463,6 +469,65 @@
this->VerifyNearGoal();
}
+// Tests that the subsystem loop can reach a goal when the profile is disabled.
+TYPED_TEST_P(IntakeSystemTest, FunctionsWhenProfileDisabled) {
+ this->SetEnabled(true);
+ {
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ // By setting NaN for the profile, we would cause the entire system to fail
+ // or blow up if it is not ignoring the profile correctly.
+ profile_builder.add_max_velocity(std::numeric_limits<double>::quiet_NaN());
+ profile_builder.add_max_acceleration(
+ std::numeric_limits<double>::quiet_NaN());
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish(), 0.0, true)));
+ }
+
+ // Give it a lot of time to get there.
+ this->RunFor(chrono::seconds(8));
+
+ this->VerifyNearGoal();
+}
+
+// Tests that the subsystem loop can maintain a velocity when using the
+// goal_velocity setting.
+TYPED_TEST_P(IntakeSystemTest, MaintainConstantVelocityWithoutProfile) {
+ this->SetEnabled(true);
+
+ const double kStartingGoal = -0.10;
+ const double kVelocity = 0.05;
+ this->test_event_loop_->AddPhasedLoop(
+ [this, kStartingGoal, kVelocity](int) {
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(0);
+ profile_builder.add_max_acceleration(0);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kStartingGoal +
+ aos::time::DurationInSeconds(
+ this->monotonic_now().time_since_epoch()) *
+ kVelocity,
+ profile_builder.Finish(), kVelocity, true)));
+ },
+ this->dt());
+
+ const double kRunTimeSec = 4;
+ // Give time for the system to settle down--it should've been running at a
+ // constant velocity the whole time, once it converged.
+ this->RunFor(chrono::seconds(static_cast<int>(kRunTimeSec)));
+
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(kStartingGoal + kVelocity * kRunTimeSec,
+ this->subsystem_status_fetcher_->position(), 0.001);
+ EXPECT_NEAR(kStartingGoal + kVelocity * kRunTimeSec,
+ this->subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(kVelocity, this->subsystem_status_fetcher_->velocity(), 0.001);
+}
+
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
@@ -755,6 +820,8 @@
}
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
+ FunctionsWhenProfileDisabled,
+ MaintainConstantVelocityWithoutProfile,
SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
LowerHardstopStartup, UpperHardstopStartup,
ResetTest, DisabledGoalTest, DisabledZeroTest,
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
index e39272a..da6e7ae 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
@@ -6,6 +6,8 @@
table SubsystemGoal {
unsafe_goal:double;
profile_params:frc971.ProfileParameters;
+ goal_velocity:double;
+ ignore_profile:bool;
}
table SubsystemOutput {
diff --git a/frc971/wpilib/ahal/AnalogTriggerOutput.cc b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
index db55efa..1bbef1b 100644
--- a/frc971/wpilib/ahal/AnalogTriggerOutput.cc
+++ b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
@@ -41,3 +41,5 @@
HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
trigger.GetIndex() + 1, static_cast<uint8_t>(outputType) + 1);
}
+
+AnalogTriggerOutput::~AnalogTriggerOutput() {}
diff --git a/frc971/wpilib/ahal/TalonFX.cc b/frc971/wpilib/ahal/TalonFX.cc
new file mode 100644
index 0000000..93dc62e
--- /dev/null
+++ b/frc971/wpilib/ahal/TalonFX.cc
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc971/wpilib/ahal/TalonFX.h"
+
+#include "hal/HAL.h"
+
+using namespace frc;
+
+/**
+ * Constructor for a TalonFX.
+ *
+ * @param channel The PWM channel that the TalonFX is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+TalonFX::TalonFX(int channel) : PWM(channel) {
+ /**
+ * Note that the TalonFX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the TalonFX User
+ * Manual available from Vex.
+ *
+ * 2.004ms = full "forward"
+ * 1.52ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.48ms = the "low end" of the deadband range
+ * 0.997ms = full "reverse"
+ */
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+}
+
diff --git a/frc971/wpilib/ahal/TalonFX.h b/frc971/wpilib/ahal/TalonFX.h
new file mode 100644
index 0000000..0de0019
--- /dev/null
+++ b/frc971/wpilib/ahal/TalonFX.h
@@ -0,0 +1,23 @@
+
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc971/wpilib/ahal/PWM.h"
+
+namespace frc {
+
+/**
+ * Vex Robotics Victor SP Speed Controller
+ */
+class TalonFX : public PWM {
+ public:
+ explicit TalonFX(int channel);
+ virtual ~TalonFX() = default;
+};
+
+} // namespace frc
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 3e24fd7..25e7592 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -166,6 +166,18 @@
position->index_pulses = encoder.index_posedge_count();
}
+ // Copies a relative encoder.
+ void CopyPosition(const ::frc::Encoder &encoder,
+ ::frc971::RelativePositionT *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.GetRaw(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 7a74413..eee3f57 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -10,7 +10,7 @@
aos::FlatbufferDetachedBuffer<IMUValues> MakeMeasurement(
const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel) {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
IMUValuesBuilder builder(fbb);
builder.add_gyro_x(gyro.x());
builder.add_gyro_y(gyro.y());
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 6b1f031..90b1659 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -31,7 +31,7 @@
FlatbufferDetachedBuffer<Goal> MakeZeroGoal() {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal> wrist_offset;
{
@@ -71,7 +71,7 @@
FlatbufferDetachedBuffer<Status> MakeZeroStatus() {
flatbuffers::FlatBufferBuilder fbb;
- fbb.ForceDefaults(1);
+ fbb.ForceDefaults(true);
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus> wrist_offset;
{
diff --git a/y2020/BUILD b/y2020/BUILD
index 215c760..9a233d4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -33,6 +33,9 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
"//y2020/control_loops/superstructure/turret:turret_plants",
@@ -75,6 +78,7 @@
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
+ "//third_party:phoenix",
"//third_party:wpilib",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 6f79b6b..a35798f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,6 +12,7 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "y2020/control_loops/superstructure/control_panel/integral_control_panel_plant.h"
#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2020/control_loops/superstructure/turret/integral_turret_plant.h"
@@ -74,7 +75,7 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
*const turret_params = &turret->subsystem_params;
- //Turret Constants
+ // Turret Constants
turret_params->zeroing_voltage = 4.0;
turret_params->operating_voltage = 12.0;
// TODO(austin): Tune these.
@@ -91,6 +92,11 @@
turret_params->zeroing_constants.moving_buffer_size = 20;
turret_params->zeroing_constants.allowable_encoder_error = 0.9;
+ CHECK_LE(hood->range.range(),
+ hood->zeroing_constants.one_revolution_distance);
+ CHECK_LE(intake->range.range(),
+ intake->zeroing_constants.one_revolution_distance);
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -149,8 +155,8 @@
static ::aos::Mutex mutex;
::aos::MutexLocker locker(&mutex);
- // IMPORTANT: This declaration has to stay after the mutex is locked to avoid
- // race conditions.
+ // IMPORTANT: This declaration has to stay after the mutex is locked to
+ // avoid race conditions.
static ::std::map<uint16_t, const Values *> values;
if (values.count(team_number) == 0) {
diff --git a/y2020/constants.h b/y2020/constants.h
index d7b9b4b..ed58777 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -10,6 +10,9 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/turret/turret_plant.h"
@@ -24,10 +27,10 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
- control_loops::drivetrain::kHighOutputRatio /
+ control_loops::drivetrain::kHighGearRatio /
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
@@ -35,22 +38,20 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- // TODO(sabina): Update constants
- static constexpr double kHoodEncoderRatio() { return 1.0; }
+ static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
- return control_loops::superstructure::hood::kFreeSpeed *
+ return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::hood::kOutputRatio /
- kHoodEncoderRatio() / (2.0 * M_PI) *
- kHoodEncoderCountsPerRevolution();
+ kHoodEncoderRatio() * kHoodEncoderCountsPerRevolution();
}
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- 0.00, // Back Hard
- 0.79, // Front Hard
- 0.14, // Back Soft
- 0.78 // Front Soft
+ -0.01, // Back Hard
+ 0.65, // Front Hard
+ 0.0, // Back Soft
+ 0.64 // Front Soft
};
}
@@ -64,10 +65,9 @@
static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
- return control_loops::superstructure::intake::kFreeSpeed *
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::intake::kOutputRatio /
- kIntakeEncoderRatio() / (2.0 * M_PI) *
- kIntakeEncoderCountsPerRevolution();
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
}
// TODO(sabina): update range
@@ -84,22 +84,23 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
intake;
+ static constexpr double kIntakeRollerSupplyCurrentLimit() { return 30.0; }
+ static constexpr double kIntakeRollerStatorCurrentLimit() { return 40.0; }
+
// Turret
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kTurretEncoderRatio() {
- return 1.0; // TODO (Kai): Get Gear Ratios when ready
+ return (26.0 / 150.0) * (130.0 / 40.0);
}
static constexpr double kMaxTurretEncoderPulsesPerSecond() {
- return control_loops::superstructure::turret::kFreeSpeed *
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::turret::kOutputRatio /
- kTurretEncoderRatio() / (2.0 * M_PI) *
- kTurretEncoderCountsPerRevolution();
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
}
- // TODO(austin): Figure out the actual constant here.
- static constexpr double kTurretPotRatio() { return 1.0; }
+ static constexpr double kTurretPotRatio() { return (26.0 / 150.0); }
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
@@ -119,6 +120,55 @@
};
PotAndAbsEncoderConstants turret;
+
+ // Control Panel
+
+ // Mag encoder
+ static constexpr double kControlPanelEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ // Ratio is encoder to output
+ static constexpr double kControlPanelEncoderRatio() { return (56.0 / 28.0); }
+
+ static constexpr double kMaxControlPanelEncoderPulsesPerSecond() {
+ return control_loops::superstructure::control_panel::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::control_panel::kOutputRatio /
+ kControlPanelEncoderRatio() *
+ kControlPanelEncoderCountsPerRevolution();
+ }
+
+ // Shooter
+ static constexpr double kFinisherEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+ static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+
+ static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
+ return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::finisher::kOutputRatio /
+ kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
+ }
+
+
+ static constexpr double kAcceleratorEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+ static constexpr double kAcceleratorEncoderRatio() {
+ return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
+ }
+
+ static constexpr double kMaxAcceleratorEncoderPulsesPerSecond() {
+ return control_loops::superstructure::accelerator::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::accelerator::kOutputRatio /
+ kAcceleratorEncoderRatio() *
+ kAcceleratorEncoderCountsPerRevolution();
+ }
+
+ // Climber
+ static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 257ed23..64ae12e 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -13,22 +13,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 1" wide neopreme wheel.
+J_wheel = 0.000319
+# Gear ratio between wheels (speed up!)
+G_per_wheel = 1.2
+# Gear ratio to the final wheel.
+G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
+# Overall flywheel inertia.
+J = J_wheel * (
+ 1.0 + numpy.power(G, -2.0) + numpy.power(G, -4.0) + numpy.power(G, -6.0))
+
+# The position and velocity are measured for the final wheel.
kAccelerator = flywheel.FlywheelParams(
name='Accelerator',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
- q_voltage=0.3,
+ q_voltage=0.4,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.80])
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ R = numpy.matrix([[0.0], [500.0], [0.0]])
flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
return 0
diff --git a/y2020/control_loops/python/control_panel.py b/y2020/control_loops/python/control_panel.py
index 206a39e..e02aa13 100644
--- a/y2020/control_loops/python/control_panel.py
+++ b/y2020/control_loops/python/control_panel.py
@@ -17,12 +17,11 @@
except gflags.DuplicateFlagError:
pass
-#TODO(sabina): update moment
kControlPanel = angular_system.AngularSystemParams(
name='ControlPanel',
motor=control_loop.BAG(),
- G=(1.0),
- J=0.3,
+ G=1.0,
+ J=0.000009,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
index 54745dd..95b9fcf 100644
--- a/y2020/control_loops/python/drivetrain.py
+++ b/y2020/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
import sys
import gflags
@@ -11,19 +12,20 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
kDrivetrain = drivetrain.DrivetrainParams(
- J=1.5,
- mass=38.5,
- robot_radius=0.45 / 2.0,
- wheel_radius=4.0 * 0.0254 / 2.0,
- G=9.0 / 52.0,
+ J=6.0,
+ mass=68.0,
+ # TODO(austin): Measure radius a bit better.
+ robot_radius=0.7 / 2.0,
+ wheel_radius=6.0 * 0.0254 / 2.0,
+ motor_type=control_loop.Falcon(),
+ G=(8.0 / 70.0) * (17.0 / 24.0),
q_pos=0.14,
q_vel=1.30,
efficiency=0.80,
has_imu=True,
force=True,
kf_q_voltage=13.0,
- controller_poles=[0.82, 0.82],
- robot_cg_offset=0.0)
+ controller_poles=[0.82, 0.82])
def main(argv):
@@ -38,5 +40,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2020', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 0b0fbb4..0d46bb9 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,22 +13,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+# Inertia for a single 4" diameter, 2" wide neopreme wheel.
+J_wheel = 0.000319 * 2.0
+# Gear ratio to the final wheel.
+# 40 tooth on the flywheel
+# 48 for the falcon.
+# 60 tooth on the outer wheel.
+G = 48.0 / 40.0
+# Overall flywheel inertia.
+J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+
+# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
motor=control_loop.Falcon(),
- G=1.0,
- J=0.006,
+ G=G,
+ J=J,
q_pos=0.08,
q_vel=4.00,
- q_voltage=0.3,
+ q_voltage=0.4,
r_pos=0.05,
- controller_poles=[.87],
- dt=0.00505)
+ controller_poles=[.80])
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ R = numpy.matrix([[0.0], [500.0], [0.0]])
flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
return 0
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 9a6fc46..630d223 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -214,7 +214,7 @@
offset.append(observer_flywheel.X_hat[2, 0])
applied_U = U.copy()
- if i > 30:
+ if i > 100:
applied_U += 2
flywheel.Update(applied_U)
@@ -273,10 +273,10 @@
loop_writer = control_loop.ControlLoopWriter(
name, flywheels, namespaces=namespace)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio' + params.name, '%f',
+ control_loop.Constant('kOutputRatio', '%f',
flywheels[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed' + params.name, '%f',
+ control_loop.Constant('kFreeSpeed', '%f',
flywheels[0].motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 8c99940..b4d8c84 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -17,29 +17,33 @@
except gflags.DuplicateFlagError:
pass
-'''
-Hood is an angular subsystem due to the mounting of the encoder on the hood joint.
-We are currently electing to ignore potential non-linearity.
-'''
-#TODO: update constants
+# Hood is an angular subsystem due to the mounting of the encoder on the hood
+# joint. We are currently electing to ignore potential non-linearity.
+
+range_of_travel_radians = (37.0 * numpy.pi / 180.0)
+# 0.083 inches/turn
+# 6.38 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+
+radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+
kHood = angular_system.AngularSystemParams(
name='Hood',
motor=control_loop.BAG(),
- # meters / rad (used the displacement of the lead screw and the angle)
- G=(0.1601 / 0.6457),
- J=0.3,
- q_pos=0.20,
- q_vel=5.0,
+ G=radians_per_turn,
+ J=0.08389,
+ q_pos=0.55,
+ q_vel=14.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
+ kalman_q_voltage=2.5,
kalman_r_position=0.05)
def main(argv):
if FLAGS.plot:
- R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 4.0], [0.0]])
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index f65679a..9e80244 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -17,14 +17,13 @@
except gflags.DuplicateFlagError:
pass
-#TODO: update constants
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(1.0 / 1.0),
- J=0.3,
- q_pos=0.20,
- q_vel=5.0,
+ G=(12.0 / 24.0) * (1.0 / 7.0) * (1.0 / 7.0) * (16.0 / 32.0),
+ J=3.0 * 0.139 * 0.139,
+ q_pos=0.40,
+ q_vel=20.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=4.0,
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index 58ea7d8..e276457 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -21,18 +21,21 @@
kTurret = angular_system.AngularSystemParams(
name='Turret',
motor=control_loop.Vex775Pro(),
- #TODO: Update Gear Ratios when they are ready
- G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- #TODO: Get number from Bryan (moment of inertia)
- J=0.30,
- q_pos=0.20,
- q_vel=5.0,
+ G=(6.0 / 60.0) * (26.0 / 150.0),
+ J=0.11,
+ q_pos=0.40,
+ q_vel=7.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
+ kalman_q_voltage=3.0,
kalman_r_position=0.05)
def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 87ee689..d81b6b5 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -63,6 +63,7 @@
"//aos/controls:control_loop",
"//aos/events:event_loop",
"//y2020:constants",
+ "//y2020/control_loops/superstructure/shooter",
],
)
@@ -94,6 +95,7 @@
":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop_test",
+ "//aos/events/logging:logger",
"//aos/testing:googletest",
"//aos/time",
"//frc971/control_loops:capped_test_plant",
@@ -102,6 +104,7 @@
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
+ "//y2020/control_loops/superstructure/shooter:shooter_plants",
],
)
diff --git a/y2020/control_loops/superstructure/accelerator/BUILD b/y2020/control_loops/superstructure/accelerator/BUILD
index 2c680bf..30fca08 100644
--- a/y2020/control_loops/superstructure/accelerator/BUILD
+++ b/y2020/control_loops/superstructure/accelerator/BUILD
@@ -7,11 +7,28 @@
outs = [
"accelerator_plant.h",
"accelerator_plant.cc",
- "accelerator_integral_plant.h",
- "accelerator_integral_plant.cc",
+ "integral_accelerator_plant.h",
+ "integral_accelerator_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:accelerator) $(OUTS)",
tools = [
"//y2020/control_loops/python:accelerator",
],
)
+
+cc_library(
+ name = "accelerator_plants",
+ srcs = [
+ "accelerator_plant.cc",
+ "integral_accelerator_plant.cc",
+ ],
+ hdrs = [
+ "accelerator_plant.h",
+ "integral_accelerator_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/finisher/BUILD b/y2020/control_loops/superstructure/finisher/BUILD
index b9bfc4f..2009886 100644
--- a/y2020/control_loops/superstructure/finisher/BUILD
+++ b/y2020/control_loops/superstructure/finisher/BUILD
@@ -7,11 +7,28 @@
outs = [
"finisher_plant.h",
"finisher_plant.cc",
- "finisher_integral_plant.h",
- "finisher_integral_plant.cc",
+ "integral_finisher_plant.h",
+ "integral_finisher_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:finisher) $(OUTS)",
tools = [
"//y2020/control_loops/python:finisher",
],
)
+
+cc_library(
+ name = "finisher_plants",
+ srcs = [
+ "finisher_plant.cc",
+ "integral_finisher_plant.cc",
+ ],
+ hdrs = [
+ "finisher_plant.h",
+ "integral_finisher_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index d63231b..fe8a29a 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -4,18 +4,6 @@
cc_library(
name = "shooter_plants",
- srcs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
- ],
- hdrs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
- ],
deps = [
"//frc971/control_loops:state_feedback_loop",
],
@@ -49,10 +37,11 @@
"flywheel_controller.h",
],
deps = [
- ":shooter_plants",
"//aos/controls:control_loop",
"//frc971/control_loops:profiled_subsystem",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
],
)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 0f920a4..77fb769 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -13,7 +13,8 @@
FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
: loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
- history_.fill(0);
+ history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
+ 0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
}
@@ -22,12 +23,16 @@
last_goal_ = angular_velocity_goal;
}
-void FlywheelController::set_position(double current_position) {
+void FlywheelController::set_position(
+ double current_position,
+ const aos::monotonic_clock::time_point position_timestamp) {
// Update position in the model.
Y_ << current_position;
// Add the position to the history.
- history_[history_position_] = current_position;
+ history_[history_position_] =
+ std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
+ position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
}
@@ -50,15 +55,20 @@
const int oldest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+ const double total_loop_time = ::aos::time::DurationInSeconds(
+ std::get<1>(history_[history_position_]) -
+ std::get<1>(history_[oldest_history_position]));
+
+ const double distance_traveled =
+ std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[oldest_history_position]);
+
// Compute the distance moved over that time period.
- const double avg_angular_velocity =
- (history_[oldest_history_position] - history_[history_position_]) /
- (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
- static_cast<double>(kHistoryLength - 1));
+ avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
FlywheelControllerStatusBuilder builder(*fbb);
- builder.add_avg_angular_velocity(avg_angular_velocity);
+ builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 0));
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 20cd681..a3e9bdb 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -6,8 +6,8 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
@@ -23,7 +23,8 @@
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
// Sets the current encoder position in radians
- void set_position(double current_position);
+ void set_position(double current_position,
+ const aos::monotonic_clock::time_point position_timestamp);
// Populates the status structure.
flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
@@ -38,6 +39,8 @@
// Executes the control loop for a cycle.
void Update(bool disabled);
+ double avg_angular_velocity() { return avg_angular_velocity_; }
+
private:
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -46,8 +49,14 @@
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
- ::std::array<double, kHistoryLength> history_;
+ ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+ kHistoryLength>
+ history_;
ptrdiff_t history_position_ = 0;
+
+ // Average velocity logging.
+ double avg_angular_velocity_;
+
double last_goal_ = 0;
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6a1d201..25f6a6a 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -11,29 +11,59 @@
namespace superstructure {
namespace shooter {
+namespace {
+const double kVelocityTolerance = 0.01;
+} // namespace
+
Shooter::Shooter()
: finisher_(finisher::MakeIntegralFinisherLoop()),
accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+bool Shooter::UpToSpeed(const ShooterGoal *goal) {
+ return (
+ std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+ kVelocityTolerance);
+}
+
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output) {
- if (goal) {
- // Update position/goal for our two shooter sides.
- finisher_.set_goal(goal->velocity_finisher());
- accelerator_left_.set_goal(goal->velocity_accelerator());
- accelerator_right_.set_goal(goal->velocity_accelerator());
- }
-
- finisher_.set_position(position->theta_finisher());
- accelerator_left_.set_position(position->theta_accelerator_left());
- accelerator_right_.set_position(position->theta_accelerator_right());
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp) {
+ // Update position, output, and status for our two shooter sides.
+ finisher_.set_position(position->theta_finisher(), position_timestamp);
+ accelerator_left_.set_position(position->theta_accelerator_left(),
+ position_timestamp);
+ accelerator_right_.set_position(position->theta_accelerator_right(),
+ position_timestamp);
finisher_.Update(output == nullptr);
accelerator_left_.Update(output == nullptr);
accelerator_right_.Update(output == nullptr);
+ // Update goal.
+ if (goal) {
+ finisher_.set_goal(goal->velocity_finisher());
+ accelerator_left_.set_goal(goal->velocity_accelerator());
+ accelerator_right_.set_goal(goal->velocity_accelerator());
+
+ if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
+ goal->velocity_accelerator() > kVelocityTolerance) {
+ ready_ = true;
+ } else {
+ ready_ = false;
+ }
+ }
+
flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
finisher_.SetStatus(fbb);
flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 88bcb3b..f72eeeb 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -21,11 +21,17 @@
flatbuffers::Offset<ShooterStatus> RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp);
+
+ bool ready() { return ready_; }
private:
FlywheelController finisher_, accelerator_left_, accelerator_right_;
+ bool UpToSpeed(const ShooterGoal *goal);
+ bool ready_ = false;
+
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
new file mode 100644
index 0000000..01a1e20
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -0,0 +1,44 @@
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Status"
+ alias: "Status"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Output"
+ alias: "Output"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Position"
+ alias: "Position"
+}
+channel {
+ name: "/superstructure"
+ type: "y2020.control_loops.superstructure.Goal"
+ alias: "Goal"
+}
+
+figure {
+ axes {
+ line {
+ y_signal {
+ channel: "Status"
+ field: "hood.position"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Goal"
+ field: "hood.unsafe_goal"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Position"
+ field: "hood.encoder"
+ }
+ }
+ ylabel: "hood position"
+ }
+}
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c2512db..39c913e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -15,7 +15,8 @@
name),
hood_(constants::GetValues().hood),
intake_joint_(constants::GetValues().intake),
- turret_(constants::GetValues().turret.subsystem_params) {
+ turret_(constants::GetValues().turret.subsystem_params),
+ shooter_() {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -30,6 +31,9 @@
turret_.Reset();
}
+ const aos::monotonic_clock::time_point position_timestamp =
+ event_loop()->context().monotonic_event_time;
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -52,6 +56,12 @@
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<ShooterStatus> shooter_status_offset =
+ shooter_.RunIteration(
+ unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+ position->shooter(), status->fbb(),
+ output != nullptr ? &(output_struct) : nullptr, position_timestamp);
+
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
bool zeroed;
@@ -81,6 +91,7 @@
status_builder.add_hood(hood_status_offset);
status_builder.add_intake(intake_status_offset);
status_builder.add_turret(turret_status_offset);
+ status_builder.add_shooter(shooter_status_offset);
status->Send(status_builder.Finish());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 2bc0cda..d8ce917 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -32,6 +33,7 @@
const AbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
+ const shooter::Shooter &shooter() const { return shooter_; }
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
@@ -42,6 +44,7 @@
AbsoluteEncoderSubsystem hood_;
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
+ shooter::Shooter shooter_;
Climber climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 458a771..3cd391c 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,15 +4,21 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
+#include "aos/events/logging/logger.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"
#include "gtest/gtest.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/superstructure.h"
+DEFINE_string(output_file, "",
+ "If set, logs all channels to the provided logfile.");
+
namespace y2020 {
namespace control_loops {
namespace superstructure {
@@ -34,6 +40,25 @@
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
+class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
// Class which simulates the superstructure and sends out queue messages with
// the position.
class SuperstructureSimulation {
@@ -57,7 +82,12 @@
turret_plant_(new CappedTestPlant(turret::MakeTurretPlant())),
turret_encoder_(constants::GetValues()
.turret.subsystem_params.zeroing_constants
- .one_revolution_distance) {
+ .one_revolution_distance),
+ accelerator_left_plant_(
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+ accelerator_right_plant_(
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+ finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant())) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -104,6 +134,14 @@
.measured_absolute_position);
}
+ flatbuffers::Offset<ShooterPosition> shooter_pos_offset(
+ ShooterPositionBuilder *builder) {
+ builder->add_theta_finisher(finisher_plant_->Y(0, 0));
+ builder->add_theta_accelerator_left(accelerator_left_plant_->Y(0, 0));
+ builder->add_theta_accelerator_right(accelerator_right_plant_->Y(0, 0));
+ return builder->Finish();
+ }
+
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::Sender<Position>::Builder builder =
@@ -124,11 +162,17 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
turret_encoder_.GetSensorValues(&turret_builder);
+ ShooterPosition::Builder shooter_builder =
+ builder.MakeBuilder<ShooterPosition>();
+ flatbuffers::Offset<ShooterPosition> shooter_offset =
+ shooter_pos_offset(&shooter_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_hood(hood_offset);
position_builder.add_intake_joint(intake_offset);
position_builder.add_turret(turret_offset);
+ position_builder.add_shooter(shooter_offset);
builder.Send(position_builder.Finish());
}
@@ -142,6 +186,16 @@
double turret_position() const { return turret_plant_->X(0, 0); }
double turret_velocity() const { return turret_plant_->X(1, 0); }
+ double accelerator_left_velocity() const {
+ return accelerator_left_plant_->X(1, 0);
+ }
+
+ double accelerator_right_velocity() const {
+ return accelerator_right_plant_->X(1, 0);
+ }
+
+ double finisher_velocity() const { return finisher_plant_->X(1, 0); }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
const double last_hood_velocity = hood_velocity();
@@ -193,9 +247,26 @@
turret_U << superstructure_output_fetcher_->turret_voltage() +
turret_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> accelerator_left_U;
+ accelerator_left_U
+ << superstructure_output_fetcher_->accelerator_left_voltage() +
+ accelerator_left_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> accelerator_right_U;
+ accelerator_right_U
+ << superstructure_output_fetcher_->accelerator_right_voltage() +
+ accelerator_right_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> finisher_U;
+ finisher_U << superstructure_output_fetcher_->finisher_voltage() +
+ finisher_plant_->voltage_offset();
+
hood_plant_->Update(hood_U);
intake_plant_->Update(intake_U);
turret_plant_->Update(turret_U);
+ accelerator_left_plant_->Update(accelerator_left_U);
+ accelerator_right_plant_->Update(accelerator_right_U);
+ finisher_plant_->Update(finisher_U);
const double position_hood = hood_plant_->Y(0, 0);
const double position_intake = intake_plant_->Y(0, 0);
@@ -280,6 +351,10 @@
::std::unique_ptr<CappedTestPlant> turret_plant_;
PositionSensorSimulator turret_encoder_;
+ ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
+ ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
+ ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
double peak_intake_acceleration_ = 1e10;
@@ -315,6 +390,15 @@
superstructure_plant_event_loop_(MakeEventLoop("plant")),
superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
+
+ if (!FLAGS_output_file.empty()) {
+ unlink(FLAGS_output_file.c_str());
+ log_buffer_writer_ = std::make_unique<aos::logger::DetachedBufferWriter>(
+ FLAGS_output_file);
+ logger_event_loop_ = MakeEventLoop("logger");
+ logger_ = std::make_unique<aos::logger::Logger>(log_buffer_writer_.get(),
+ logger_event_loop_.get());
+ }
}
void VerifyNearGoal() {
@@ -336,6 +420,48 @@
EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
superstructure_status_fetcher_->turret()->position(), 0.001);
}
+
+ if (superstructure_goal_fetcher_->has_shooter()) {
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+ superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->avg_angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->avg_angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+ superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->avg_angular_velocity(),
+ 0.001);
+ }
}
void CheckIfZeroed() {
@@ -352,8 +478,8 @@
// 2 Seconds
ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
- // Since there is a delay when sending running, make sure we have a status
- // before checking it.
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
} while (superstructure_status_fetcher_.get() == nullptr ||
!superstructure_status_fetcher_.get()->zeroed());
}
@@ -372,9 +498,14 @@
::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
SuperstructureSimulation superstructure_plant_;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
+ std::unique_ptr<aos::logger::Logger> logger_;
};
-// Tests that the superstructure does nothing when the goal is to remain still.
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
superstructure_plant_.InitializeHoodPosition(
@@ -399,11 +530,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -418,8 +553,10 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().middle());
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().middle());
WaitUntilZeroed();
{
@@ -427,23 +564,27 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kHoodRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 300.0, 300.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -453,8 +594,10 @@
VerifyNearGoal();
}
+
// Makes sure that the voltage on a motor is properly pulled back after
-// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
TEST_F(SuperstructureTest, SaturationTest) {
SetEnabled(true);
// Zero it before we move.
@@ -474,11 +617,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -504,11 +651,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -526,6 +677,81 @@
VerifyNearGoal();
}
+// Tests the shooter can spin up correctly.
+TEST_F(SuperstructureTest, SpinUp) {
+ SetEnabled(true);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().upper);
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().upper);
+
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+
+ // Start up the accelerator and make sure both run.
+ shooter_builder.add_velocity_accelerator(200.0);
+ shooter_builder.add_velocity_finisher(200.0);
+
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(8));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(9));
+
+ VerifyNearGoal();
+}
+
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
SetEnabled(true);
@@ -552,9 +778,6 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
-
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2020/control_loops/superstructure/superstructure_output.fbs b/y2020/control_loops/superstructure/superstructure_output.fbs
index 2583106..fd57825 100644
--- a/y2020/control_loops/superstructure/superstructure_output.fbs
+++ b/y2020/control_loops/superstructure/superstructure_output.fbs
@@ -29,9 +29,6 @@
// Voltage sent to the flywheel. Positive is shooting.
finisher_voltage:double;
- // Voltage sent to the motor driving the control panel. Positive is counterclockwise from above.
- control_panel_voltage:double;
-
// Positive is deploying climber and to climb; cannot run in reverse
climber_voltage:double;
}
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 023a242..e8f7637 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -21,7 +21,7 @@
finisher:FlywheelControllerStatus;
// The subsystem to accelerate the ball before the finisher
- // Velocity is the slowest (lowest) wheel
+ // Velocity is the fastest (top) wheel
accelerator_left:FlywheelControllerStatus;
accelerator_right:FlywheelControllerStatus;
}
@@ -33,7 +33,7 @@
// If true, we have aborted. This is if one or all subsystem estops.
estopped:bool;
- //Subsystem status.
+ // Subsystem status.
hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 3276722..a1083eb 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -15,6 +15,7 @@
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
#include "frc971/wpilib/ahal/VictorSP.h"
#undef ERROR
@@ -30,6 +31,7 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16470.h"
@@ -64,18 +66,6 @@
// DMA stuff and then removing the * 2.0 in *_translate.
// The low bit is direction.
-// TODO(brian): Use ::std::max instead once we have C++14 so that can be
-// constexpr.
-template <typename T>
-constexpr T max(T a, T b) {
- return (a > b) ? a : b;
-}
-
-template <typename T, typename... Rest>
-constexpr T max(T a, T b, T c, Rest... rest) {
- return max(max(a, b), c, rest...);
-}
-
double drivetrain_translate(int32_t in) {
return ((static_cast<double>(in) /
Values::kDrivetrainEncoderCountsPerRevolution()) *
@@ -91,13 +81,24 @@
control_loops::drivetrain::kWheelRadius;
}
-constexpr double kMaxFastEncoderPulsesPerSecond =
- Values::kMaxDrivetrainEncoderPulsesPerSecond();
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
- "fast encoders are too fast");
-constexpr double kMaxMediumEncoderPulsesPerSecond = kMaxFastEncoderPulsesPerSecond;
+double turret_pot_translate(double voltage) {
+ return voltage * Values::kTurretPotRatio() *
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
-static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+constexpr double kMaxFastEncoderPulsesPerSecond =
+ std::max({Values::kMaxControlPanelEncoderPulsesPerSecond(),
+ Values::kMaxFinisherEncoderPulsesPerSecond(),
+ Values::kMaxAcceleratorEncoderPulsesPerSecond()});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1000000.0,
+ "fast encoders are too fast");
+constexpr double kMaxMediumEncoderPulsesPerSecond =
+ std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxHoodEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond(),
+ Values::kMaxTurretEncoderPulsesPerSecond()});
+
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000.0,
"medium encoders are too fast");
} // namespace
@@ -123,8 +124,74 @@
UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
}
+ // Hood
+
+ void set_hood_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ hood_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_hood_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ hood_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ // Intake
+
+ void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ intake_joint_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_joint_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ // Turret
+
+ void set_turret_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ turret_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_turret_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ turret_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_turret_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ turret_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ // Shooter
+
+ void set_flywheel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ flywheel_encoder_ = ::std::move(encoder);
+ }
+
+ void set_left_kicker_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ left_kicker_encoder_ = ::std::move(encoder);
+ }
+
+ void set_right_kicker_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ right_kicker_encoder_ = ::std::move(encoder);
+ }
+
+ // Control Panel
+
+ void set_control_panel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ control_panel_encoder_ = ::std::move(encoder);
+ }
+
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ medium_encoder_filter_.Add(sensor.get());
autonomous_modes_.at(i) = ::std::move(sensor);
}
@@ -149,11 +216,72 @@
builder.Send(drivetrain_builder.Finish());
}
+ const auto values = constants::GetValues();
{
auto builder = superstructure_position_sender_.MakeBuilder();
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
+ // TODO(alex): check new absolute encoder api.
+ // Hood
+ frc971::AbsolutePositionT hood;
+ CopyPosition(hood_encoder_, &hood,
+ Values::kHoodEncoderCountsPerRevolution(),
+ Values::kHoodEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsolutePosition> hood_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &hood);
+
+ // Intake
+ frc971::AbsolutePositionT intake_joint;
+ CopyPosition(intake_joint_encoder_, &intake_joint,
+ Values::kIntakeEncoderCountsPerRevolution(),
+ Values::kIntakeEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
+
+ // Turret
+ frc971::PotAndAbsolutePositionT turret;
+ CopyPosition(turret_encoder_, &turret,
+ Values::kTurretEncoderCountsPerRevolution(),
+ Values::kTurretEncoderRatio(), turret_pot_translate, false,
+ values.turret.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
+
+ // Shooter
+ y2020::control_loops::superstructure::ShooterPositionT shooter;
+ shooter.theta_finisher =
+ encoder_translate(flywheel_encoder_->GetRaw(),
+ Values::kFinisherEncoderCountsPerRevolution(),
+ Values::kFinisherEncoderRatio());
+ // TODO; check sign
+ shooter.theta_accelerator_left =
+ encoder_translate(left_kicker_encoder_->GetRaw(),
+ Values::kAcceleratorEncoderCountsPerRevolution(),
+ Values::kAcceleratorEncoderRatio());
+ shooter.theta_accelerator_right =
+ encoder_translate(right_kicker_encoder_->GetRaw(),
+ Values::kAcceleratorEncoderCountsPerRevolution(),
+ Values::kAcceleratorEncoderRatio());
+ flatbuffers::Offset<y2020::control_loops::superstructure::ShooterPosition>
+ shooter_offset =
+ y2020::control_loops::superstructure::ShooterPosition::Pack(
+ *builder.fbb(), &shooter);
+
+ // Control Panel
+ frc971::RelativePositionT control_panel;
+ CopyPosition(*control_panel_encoder_, &control_panel,
+ Values::kControlPanelEncoderCountsPerRevolution(),
+ Values::kControlPanelEncoderRatio(), false);
+ flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
+
+ position_builder.add_hood(hood_offset);
+ position_builder.add_intake_joint(intake_joint_offset);
+ position_builder.add_turret(turret_offset);
+ position_builder.add_shooter(shooter_offset);
+ position_builder.add_control_panel(control_panel_offset);
+
builder.Send(position_builder.Finish());
}
@@ -182,6 +310,13 @@
::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
+ ::frc971::wpilib::AbsoluteEncoderAndPotentiometer turret_encoder_;
+
+ ::frc971::wpilib::AbsoluteEncoder hood_encoder_, intake_joint_encoder_;
+
+ ::std::unique_ptr<::frc::Encoder> flywheel_encoder_, left_kicker_encoder_,
+ right_kicker_encoder_, control_panel_encoder_;
+
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
frc971::wpilib::ADIS16470 *imu_ = nullptr;
@@ -194,17 +329,138 @@
: ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
- private:
- void Write(const superstructure::Output & /*output*/) override {}
+ void set_hood_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ hood_victor_ = ::std::move(t);
+ }
- void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+ void set_intake_joint_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ intake_joint_victor_ = ::std::move(t);
+ }
+
+ void set_intake_roller_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ intake_roller_falcon_ = ::std::move(t);
+ intake_roller_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kIntakeRollerSupplyCurrentLimit(),
+ Values::kIntakeRollerSupplyCurrentLimit(), 0});
+ intake_roller_falcon_->ConfigStatorCurrentLimit(
+ {true, Values::kIntakeRollerStatorCurrentLimit(),
+ Values::kIntakeRollerStatorCurrentLimit(), 0});
+ }
+
+ void set_turret_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ turret_victor_ = ::std::move(t);
+ }
+
+ void set_feeder_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ feeder_falcon_ = ::std::move(t);
+ }
+
+ void set_washing_machine_control_panel_victor(
+ ::std::unique_ptr<::frc::VictorSP> t) {
+ washing_machine_control_panel_victor_ = ::std::move(t);
+ }
+
+ void set_kicker_left_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ kicker_left_falcon_ = ::std::move(t);
+ }
+
+ void set_kicker_right_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ kicker_right_falcon_ = ::std::move(t);
+ }
+
+ void set_flywheel_falcon(::std::unique_ptr<::frc::TalonFX> t) {
+ flywheel_falcon_ = ::std::move(t);
+ }
+
+ void set_climber_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ climber_falcon_ = ::std::move(t);
+ climber_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kClimberSupplyCurrentLimit(),
+ Values::kClimberSupplyCurrentLimit(), 0});
+ }
+
+ private:
+ void Write(const superstructure::Output &output) override {
+ hood_victor_->SetSpeed(std::clamp(output.hood_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ intake_joint_victor_->SetSpeed(std::clamp(output.intake_joint_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ intake_roller_falcon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ std::clamp(output.intake_roller_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ turret_victor_->SetSpeed(std::clamp(output.turret_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ feeder_falcon_->SetSpeed(std::clamp(output.feeder_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ washing_machine_control_panel_victor_->SetSpeed(
+ std::clamp(output.washing_machine_spinner_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ kicker_left_falcon_->SetSpeed(std::clamp(output.accelerator_left_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ kicker_right_falcon_->SetSpeed(
+ std::clamp(output.accelerator_right_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ flywheel_falcon_->SetSpeed(std::clamp(output.finisher_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ climber_falcon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ std::clamp(output.climber_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ }
+
+ void Stop() override {
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
+ hood_victor_->SetDisabled();
+ intake_joint_victor_->SetDisabled();
+ turret_victor_->SetDisabled();
+ feeder_falcon_->SetDisabled();
+ washing_machine_control_panel_victor_->SetDisabled();
+ kicker_left_falcon_->SetDisabled();
+ kicker_right_falcon_->SetDisabled();
+ flywheel_falcon_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
+ turret_victor_, washing_machine_control_panel_victor_;
+
+ ::std::unique_ptr<::frc::TalonFX> feeder_falcon_, kicker_left_falcon_,
+ kicker_right_falcon_, flywheel_falcon_;
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ intake_roller_falcon_, climber_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
- ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ ::std::unique_ptr<frc::Encoder> make_encoder(
+ int index, frc::Encoder::EncodingType encodingType = frc::Encoder::k4X) {
return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
- frc::Encoder::k4X);
+ encodingType);
}
void Run() override {
@@ -228,6 +484,23 @@
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+ // TODO: pin numbers
+ sensor_reader.set_hood_encoder(make_encoder(2));
+ sensor_reader.set_hood_absolute_pwm(make_unique<frc::DigitalInput>(2));
+
+ sensor_reader.set_intake_encoder(make_encoder(3));
+ sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+
+ sensor_reader.set_turret_encoder(make_encoder(4));
+ sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(4));
+ sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(4));
+
+ sensor_reader.set_flywheel_encoder(make_encoder(5));
+ sensor_reader.set_left_kicker_encoder(make_encoder(6));
+ sensor_reader.set_right_kicker_encoder(make_encoder(7));
+
+ sensor_reader.set_control_panel_encoder(make_encoder(8, frc::Encoder::k2X));
+
// Note: If ADIS16470 is plugged in directly to the roboRIO SPI port without
// the Spartan Board, then trigger is on 26, reset 27, and chip select is
// CS0.
@@ -250,6 +523,23 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
SuperstructureWriter superstructure_writer(&output_event_loop);
+ // TODO: check ports
+ superstructure_writer.set_hood_victor(make_unique<frc::VictorSP>(2));
+ superstructure_writer.set_intake_joint_victor(
+ make_unique<frc::VictorSP>(3));
+ superstructure_writer.set_intake_roller_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4));
+ superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(5));
+ superstructure_writer.set_feeder_falcon(make_unique<frc::TalonFX>(6));
+ superstructure_writer.set_washing_machine_control_panel_victor(
+ make_unique<frc::VictorSP>(7));
+ superstructure_writer.set_kicker_left_falcon(
+ make_unique<::frc::TalonFX>(8));
+ superstructure_writer.set_kicker_right_falcon(
+ make_unique<::frc::TalonFX>(9));
+ superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(10));
+ superstructure_writer.set_climber_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(11));
AddLoop(&output_event_loop);