Merge "Minor plotting tool improvements"
diff --git a/aos/dump_rtprio.cc b/aos/dump_rtprio.cc
index 12000f0..5d9a0b4 100644
--- a/aos/dump_rtprio.cc
+++ b/aos/dump_rtprio.cc
@@ -248,7 +248,7 @@
int main() {
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
const int pid_max = find_pid_max();
diff --git a/aos/events/logging/log_stats.cc b/aos/events/logging/log_stats.cc
index 9383fdd..2d9604f 100644
--- a/aos/events/logging/log_stats.cc
+++ b/aos/events/logging/log_stats.cc
@@ -79,7 +79,6 @@
if (channel->name()->string_view().find(FLAGS_name) != std::string::npos) {
// Add a record to the stats vector.
channel_stats.push_back({channel});
- it++;
// Lambda to read messages and parse for information
stats_event_loop->MakeRawWatcher(
channel,
@@ -99,6 +98,7 @@
// update the overall logfile statistics
logfile_stats.logfile_length += context.size;
});
+ it++;
// TODO (Stephan): Frequency of messages per second
// - Sliding window
// - Max / Deviation
diff --git a/aos/ipc_lib/ipc_comparison.cc b/aos/ipc_lib/ipc_comparison.cc
index 1553159..cf5581e 100644
--- a/aos/ipc_lib/ipc_comparison.cc
+++ b/aos/ipc_lib/ipc_comparison.cc
@@ -955,7 +955,7 @@
::gflags::ParseCommandLineFlags(&argc, &argv, true);
::aos::InitNRT();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
return ::aos::Main(argc, argv);
diff --git a/aos/ipc_lib/raw_queue_test.cc b/aos/ipc_lib/raw_queue_test.cc
index 3048e1b..1c02c53 100644
--- a/aos/ipc_lib/raw_queue_test.cc
+++ b/aos/ipc_lib/raw_queue_test.cc
@@ -994,52 +994,52 @@
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->WriteMessage(nullptr, RawQueue::kPeek);
},
".*illegal write option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->WriteMessage(nullptr, RawQueue::kFromEnd);
},
".*illegal write option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->WriteMessage(nullptr, RawQueue::kPeek | RawQueue::kFromEnd);
},
".*illegal write option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->WriteMessage(nullptr, RawQueue::kNonBlock | RawQueue::kBlock);
},
".*invalid write option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->ReadMessageIndex(
RawQueue::kBlock | RawQueue::kFromEnd | RawQueue::kPeek, nullptr);
},
".*ReadMessageIndex.*is not allowed.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->ReadMessageIndex(RawQueue::kOverride, nullptr);
},
".*illegal read option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->ReadMessageIndex(RawQueue::kOverride | RawQueue::kBlock,
nullptr);
},
".*illegal read option.*");
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
queue->ReadMessage(RawQueue::kNonBlock | RawQueue::kBlock);
},
".*invalid read option.*");
diff --git a/aos/logging/implementations.cc b/aos/logging/implementations.cc
index 89fc904..cfc14c0 100644
--- a/aos/logging/implementations.cc
+++ b/aos/logging/implementations.cc
@@ -1,16 +1,16 @@
#include "aos/logging/implementations.h"
-#include <stdarg.h>
#include <inttypes.h>
+#include <stdarg.h>
#include <algorithm>
#include <chrono>
+#include "absl/base/call_once.h"
#include "aos/die.h"
+#include "aos/ipc_lib/queue.h"
#include "aos/logging/printf_formats.h"
#include "aos/time/time.h"
-#include "aos/ipc_lib/queue.h"
-#include "absl/base/call_once.h"
namespace aos {
namespace logging {
@@ -23,30 +23,18 @@
// apply here (mostly the parts about being able to use AOS_LOG) because this is
// the root one.
class RootLogImplementation : public LogImplementation {
- public:
- void have_other_implementation() { only_implementation_ = false; }
-
protected:
virtual ::aos::monotonic_clock::time_point monotonic_now() const {
return ::aos::monotonic_clock::now();
}
private:
- void set_next(LogImplementation *) override {
- AOS_LOG(FATAL, "can't have a next logger from here\n");
- }
-
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
- void DoLog(log_level level, const char *format, va_list ap) override {
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0))) void DoLog(
+ log_level level, const char *format, va_list ap) override {
LogMessage message;
internal::FillInMessage(level, monotonic_now(), format, ap, &message);
internal::PrintMessage(stderr, message);
- if (!only_implementation_) {
- fputs("root logger got used, see stderr for message\n", stdout);
- }
}
-
- bool only_implementation_ = true;
};
RootLogImplementation *root_implementation = nullptr;
@@ -63,15 +51,13 @@
internal::global_top_implementation.store(implementation);
}
-void NewContext() {
- internal::Context::Delete();
-}
+void NewContext() { internal::Context::Delete(); }
void DoInit() {
SetGlobalImplementation(root_implementation = new RootLogImplementation());
- if (pthread_atfork(NULL /*prepare*/, NULL /*parent*/,
- NewContext /*child*/) != 0) {
+ if (pthread_atfork(NULL /*prepare*/, NULL /*parent*/, NewContext /*child*/) !=
+ 0) {
AOS_LOG(FATAL, "pthread_atfork(NULL, NULL, %p) failed\n", NewContext);
}
}
@@ -142,22 +128,26 @@
internal::PrintMessage(stream_, message);
}
-void AddImplementation(LogImplementation *implementation) {
+void SetImplementation(LogImplementation *implementation, bool update_global) {
internal::Context *context = internal::Context::Get();
- if (implementation->next() != NULL) {
- AOS_LOG(FATAL,
- "%p already has a next implementation, but it's not"
- " being used yet\n",
- implementation);
+ if (implementation == nullptr) {
+ AOS_LOG(FATAL, "SetImplementation got invalid implementation");
}
- LogImplementation *old = context->implementation;
- if (old != NULL) {
- implementation->set_next(old);
+ context->implementation = implementation;
+ if (update_global) {
+ SetGlobalImplementation(implementation);
}
- SetGlobalImplementation(implementation);
- root_implementation->have_other_implementation();
+}
+
+LogImplementation *SwapImplementation(LogImplementation *implementation) {
+ internal::Context *context = internal::Context::Get();
+
+ LogImplementation *old = context->implementation;
+ context->implementation = implementation;
+
+ return old;
}
void Init() {
@@ -165,13 +155,9 @@
absl::call_once(once, DoInit);
}
-void Load() {
- internal::Context::Get();
-}
+void Load() { internal::Context::Get(); }
-void Cleanup() {
- internal::Context::Delete();
-}
+void Cleanup() { internal::Context::Delete(); }
namespace {
@@ -239,14 +225,26 @@
return ::aos::monotonic_clock::now();
}
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
- void DoLog(log_level level, const char *format, va_list ap) override {
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0))) void DoLog(
+ log_level level, const char *format, va_list ap) override {
LogMessage *message = GetMessageOrDie();
internal::FillInMessage(level, monotonic_now(), format, ap, message);
Write(message);
}
};
+class CallbackLogImplementation : public HandleMessageLogImplementation {
+ public:
+ CallbackLogImplementation(
+ const ::std::function<void(const LogMessage &)> &callback)
+ : callback_(callback) {}
+
+ private:
+ void HandleMessage(const LogMessage &message) override { callback_(message); }
+
+ ::std::function<void(const LogMessage &)> callback_;
+};
+
} // namespace
RawQueue *GetLoggingQueue() {
@@ -261,7 +259,14 @@
Die("logging: couldn't fetch queue\n");
}
- AddImplementation(new LinuxQueueLogImplementation());
+ SetImplementation(new LinuxQueueLogImplementation());
+}
+
+void RegisterCallbackImplementation(
+ const ::std::function<void(const LogMessage &)> &callback,
+ bool update_global = true) {
+ Init();
+ SetImplementation(new CallbackLogImplementation(callback), update_global);
}
} // namespace logging
diff --git a/aos/logging/implementations.h b/aos/logging/implementations.h
index 27c0472..78980ff 100644
--- a/aos/logging/implementations.h
+++ b/aos/logging/implementations.h
@@ -1,17 +1,17 @@
#ifndef AOS_LOGGING_IMPLEMENTATIONS_H_
#define AOS_LOGGING_IMPLEMENTATIONS_H_
+#include <limits.h>
+#include <stdarg.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
#include <sys/types.h>
#include <unistd.h>
-#include <stdint.h>
-#include <limits.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdarg.h>
-#include <string>
-#include <functional>
#include <atomic>
+#include <functional>
+#include <string>
#include "aos/logging/context.h"
#include "aos/logging/interface.h"
@@ -42,9 +42,7 @@
// Contains all of the information about a given logging call.
struct LogMessage {
- enum class Type : uint8_t {
- kString
- };
+ enum class Type : uint8_t { kString };
int32_t seconds, nseconds;
// message_length is just the length of the actual data (which member depends
@@ -71,8 +69,7 @@
int rows, cols;
size_t string_length;
// The message string and then the serialized matrix.
- char
- data[LOG_MESSAGE_LEN - sizeof(type) - sizeof(rows) - sizeof(cols)];
+ char data[LOG_MESSAGE_LEN - sizeof(type) - sizeof(rows) - sizeof(cols)];
} matrix;
};
};
@@ -87,14 +84,16 @@
// Returns a string representing level or "unknown".
static inline const char *log_str(log_level level) {
-#define DECL_LEVEL(name, value) if (level == name) return #name;
+#define DECL_LEVEL(name, value) \
+ if (level == name) return #name;
DECL_LEVELS;
#undef DECL_LEVEL
return "unknown";
}
// Returns the log level represented by str or LOG_UNKNOWN.
static inline log_level str_log(const char *str) {
-#define DECL_LEVEL(name, value) if (!strcmp(str, #name)) return name;
+#define DECL_LEVEL(name, value) \
+ if (!strcmp(str, #name)) return name;
DECL_LEVELS;
#undef DECL_LEVEL
return LOG_UNKNOWN;
@@ -109,8 +108,8 @@
}
private:
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
- void DoLog(log_level level, const char *format, va_list ap) override;
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0))) void DoLog(
+ log_level level, const char *format, va_list ap) override;
virtual void HandleMessage(const LogMessage &message) = 0;
};
@@ -133,7 +132,12 @@
// when needed or by calling Load()).
// The logging system takes ownership of implementation. It will delete it if
// necessary, so it must be created with new.
-void AddImplementation(LogImplementation *implementation);
+void SetImplementation(LogImplementation *implementation,
+ bool update_global = true);
+
+// Updates the log implementation for the current thread, returning the current
+// implementation.
+LogImplementation *SwapImplementation(LogImplementation *implementation);
// Must be called at least once per process/load before anything else is
// called. This function is safe to call multiple times from multiple
@@ -153,12 +157,15 @@
// The caller takes ownership.
RawQueue *GetLoggingQueue();
-// Calls AddImplementation to register the standard linux logging implementation
+// Calls SetImplementation to register the standard linux logging implementation
// which sends the messages through a queue. This implementation relies on
// another process(es) to read the log messages that it puts into the queue.
// This function is usually called by aos::Init*.
void RegisterQueueImplementation();
+void RegisterCallbackImplementation(
+ const ::std::function<void(const LogMessage &)> &callback);
+
// This is where all of the code that is only used by actual LogImplementations
// goes.
namespace internal {
diff --git a/aos/logging/implementations_test.cc b/aos/logging/implementations_test.cc
index d97e165..a7b8b7d 100644
--- a/aos/logging/implementations_test.cc
+++ b/aos/logging/implementations_test.cc
@@ -1,17 +1,17 @@
+#include "aos/logging/implementations.h"
+
#include <inttypes.h>
#include <chrono>
#include <string>
+#include "aos/logging/printf_formats.h"
+#include "aos/time/time.h"
#include "gtest/gtest.h"
-#include "aos/logging/implementations.h"
-#include "aos/time/time.h"
-#include "aos/logging/printf_formats.h"
-
+using ::testing::AssertionFailure;
using ::testing::AssertionResult;
using ::testing::AssertionSuccess;
-using ::testing::AssertionFailure;
namespace aos {
namespace logging {
@@ -51,8 +51,8 @@
protected:
AssertionResult WasAnythingLogged() {
if (log_implementation->used()) {
- return AssertionSuccess() << "read message '" <<
- log_implementation->message().message << "'";
+ return AssertionSuccess() << "read message '"
+ << log_implementation->message().message << "'";
}
return AssertionFailure();
}
@@ -61,9 +61,9 @@
return AssertionFailure() << "nothing was logged";
}
if (log_implementation->message().level != level) {
- return AssertionFailure() << "a message with level " <<
- log_str(log_implementation->message().level) <<
- " was logged instead of " << log_str(level);
+ return AssertionFailure() << "a message with level "
+ << log_str(log_implementation->message().level)
+ << " was logged instead of " << log_str(level);
}
internal::Context *context = internal::Context::Get();
if (log_implementation->message().source != context->source) {
@@ -73,17 +73,16 @@
}
if (log_implementation->message().name_length != context->name_size ||
memcmp(log_implementation->message().name, context->name,
- context->name_size) !=
- 0) {
+ context->name_size) != 0) {
AOS_LOG(FATAL, "got a message from %.*s, but we're %s\n",
static_cast<int>(log_implementation->message().name_length),
log_implementation->message().name, context->name);
}
- if (strstr(log_implementation->message().message, message.c_str())
- == NULL) {
- return AssertionFailure() << "got a message of '" <<
- log_implementation->message().message <<
- "' but expected it to contain '" << message << "'";
+ if (strstr(log_implementation->message().message, message.c_str()) ==
+ NULL) {
+ return AssertionFailure()
+ << "got a message of '" << log_implementation->message().message
+ << "' but expected it to contain '" << message << "'";
}
return AssertionSuccess() << log_implementation->message().message;
@@ -96,14 +95,12 @@
first = false;
Init();
- AddImplementation(log_implementation = new TestLogImplementation());
+ SetImplementation(log_implementation = new TestLogImplementation());
}
log_implementation->reset_used();
}
- void TearDown() override {
- Cleanup();
- }
+ void TearDown() override { Cleanup(); }
static TestLogImplementation *log_implementation;
};
@@ -169,7 +166,7 @@
TEST_F(LoggingTest, Timing) {
// For writing only.
- //static const long kTimingCycles = 5000000;
+ // static const long kTimingCycles = 5000000;
static const long kTimingCycles = 5000;
monotonic_clock::time_point start = monotonic_clock::now();
diff --git a/aos/logging/interface.cc b/aos/logging/interface.cc
index b72f8e1..e6e9203 100644
--- a/aos/logging/interface.cc
+++ b/aos/logging/interface.cc
@@ -4,8 +4,8 @@
#include <stdio.h>
#include <string.h>
-#include <type_traits>
#include <functional>
+#include <type_traits>
#include "aos/die.h"
#include "aos/logging/context.h"
@@ -21,8 +21,8 @@
const int ret = vsnprintf(output, size, format, ap);
typedef ::std::common_type<int, size_t>::type RetType;
if (ret < 0) {
- AOS_PLOG(FATAL, "vsnprintf(%p, %zd, %s, args) failed",
- output, size, format);
+ AOS_PLOG(FATAL, "vsnprintf(%p, %zd, %s, args) failed", output, size,
+ format);
} else if (static_cast<RetType>(ret) >= static_cast<RetType>(size)) {
// Overwrite the '\0' at the end of the existing data and
// copy in the one on the end of continued.
@@ -32,30 +32,22 @@
}
void RunWithCurrentImplementation(
- int levels, ::std::function<void(LogImplementation *)> function) {
+ ::std::function<void(LogImplementation *)> function) {
Context *context = Context::Get();
- LogImplementation *const top_implementation = context->implementation;
- LogImplementation *new_implementation = top_implementation;
- LogImplementation *implementation = NULL;
- for (int i = 0; i < levels; ++i) {
- implementation = new_implementation;
- if (new_implementation == NULL) {
- Die("no logging implementation to use\n");
- }
- new_implementation = new_implementation->next();
+ LogImplementation *const implementation = context->implementation;
+ if (implementation == NULL) {
+ Die("no logging implementation to use\n");
}
- context->implementation = new_implementation;
function(implementation);
- context->implementation = top_implementation;
}
} // namespace internal
using internal::Context;
-void LogImplementation::DoVLog(log_level level, const char *format, va_list ap,
- int levels) {
+void LogImplementation::DoVLog(log_level level, const char *format,
+ va_list ap) {
auto log_impl = [&](LogImplementation *implementation) {
va_list ap1;
va_copy(ap1, ap);
@@ -66,11 +58,11 @@
VDie(format, ap);
}
};
- internal::RunWithCurrentImplementation(levels, ::std::ref(log_impl));
+ internal::RunWithCurrentImplementation(::std::ref(log_impl));
}
void VLog(log_level level, const char *format, va_list ap) {
- LogImplementation::DoVLog(level, format, ap, 1);
+ LogImplementation::DoVLog(level, format, ap);
}
void VCork(int line, const char *function, const char *format, va_list ap) {
@@ -101,9 +93,8 @@
VCork(line, function, format, ap);
- log_do(level, "%s: %d-%d: %s: %s", file,
- context->cork_data.line_min, context->cork_data.line_max, function,
- context->cork_data.message);
+ log_do(level, "%s: %d-%d: %s: %s", file, context->cork_data.line_min,
+ context->cork_data.line_max, function, context->cork_data.message);
context->cork_data.Reset();
}
diff --git a/aos/logging/interface.h b/aos/logging/interface.h
index 92f0b10..3695e40 100644
--- a/aos/logging/interface.h
+++ b/aos/logging/interface.h
@@ -3,8 +3,8 @@
#include <stdarg.h>
-#include <string>
#include <functional>
+#include <string>
#include "aos/logging/logging.h"
#include "aos/macros.h"
@@ -38,24 +38,19 @@
// overriden methods may end up logging through a given implementation's DoLog.
class LogImplementation {
public:
- LogImplementation() : next_(NULL) {}
+ LogImplementation() {}
- // The one that this one's implementation logs to.
- // NULL means that there is no next one.
- LogImplementation *next() { return next_; }
- // Virtual in case a subclass wants to perform checks. There will be a valid
- // logger other than this one available while this is called.
- virtual void set_next(LogImplementation *next) { next_ = next; }
+ virtual ~LogImplementation() {}
virtual bool fill_type_cache() { return true; }
protected:
// Actually logs the given message. Implementations should somehow create a
// LogMessage and then call internal::FillInMessage.
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0)))
- virtual void DoLog(log_level level, const char *format, va_list ap) = 0;
- __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 4)))
- void DoLogVariadic(log_level level, const char *format, ...) {
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 0))) virtual void DoLog(
+ log_level level, const char *format, va_list ap) = 0;
+ __attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 3, 4))) void DoLogVariadic(
+ log_level level, const char *format, ...) {
va_list ap;
va_start(ap, format);
DoLog(level, format, ap);
@@ -66,12 +61,10 @@
// These functions call similar methods on the "current" LogImplementation or
// Die if they can't find one.
// levels is how many LogImplementations to not use off the stack.
- static void DoVLog(log_level, const char *format, va_list ap, int levels)
+ static void DoVLog(log_level, const char *format, va_list ap)
__attribute__((format(GOOD_PRINTF_FORMAT_TYPE, 2, 0)));
friend void VLog(log_level, const char *, va_list);
-
- LogImplementation *next_;
};
namespace internal {
diff --git a/aos/logging/log_displayer.cc b/aos/logging/log_displayer.cc
index d3e5728..fa8fe6b 100644
--- a/aos/logging/log_displayer.cc
+++ b/aos/logging/log_displayer.cc
@@ -154,7 +154,7 @@
int32_t source_pid = -1;
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
while (true) {
diff --git a/aos/mutex/mutex_test.cc b/aos/mutex/mutex_test.cc
index 53b47ca..e7bd617 100644
--- a/aos/mutex/mutex_test.cc
+++ b/aos/mutex/mutex_test.cc
@@ -72,7 +72,7 @@
test_mutex_.Unlock();
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
test_mutex_.Unlock();
},
".*multiple unlock.*");
@@ -83,7 +83,7 @@
logging::Init();
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
test_mutex_.Unlock();
},
".*multiple unlock.*");
@@ -93,7 +93,7 @@
TEST_F(MutexDeathTest, RepeatLock) {
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
ASSERT_FALSE(test_mutex_.Lock());
ASSERT_FALSE(test_mutex_.Lock());
},
@@ -297,7 +297,7 @@
});
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
MutexLocker locker(mutex);
},
".*previous owner of mutex [^ ]+ died.*");
diff --git a/aos/testing/test_logging.cc b/aos/testing/test_logging.cc
index bc60081..1799cda 100644
--- a/aos/testing/test_logging.cc
+++ b/aos/testing/test_logging.cc
@@ -143,7 +143,7 @@
void *DoEnableTestLogging() {
logging::Init();
- logging::AddImplementation(TestLogImplementation::GetInstance());
+ logging::SetImplementation(TestLogImplementation::GetInstance());
::testing::UnitTest::GetInstance()->listeners().Append(
new MyTestEventListener());
diff --git a/aos/transaction/transaction_test.cc b/aos/transaction/transaction_test.cc
index f7551c8..5d1fe90 100644
--- a/aos/transaction/transaction_test.cc
+++ b/aos/transaction/transaction_test.cc
@@ -93,7 +93,7 @@
logging::Init();
EXPECT_DEATH(
{
- logging::AddImplementation(new util::DeathTestLogImplementation());
+ logging::SetImplementation(new util::DeathTestLogImplementation());
for (int i = 0; i < 1000; ++i) {
CreateWork(i);
}
diff --git a/aos/vision/debug/debug_framework.cc b/aos/vision/debug/debug_framework.cc
index 7b3ad76..47e0856 100644
--- a/aos/vision/debug/debug_framework.cc
+++ b/aos/vision/debug/debug_framework.cc
@@ -151,7 +151,7 @@
void DebugFrameworkMain(int argc, char **argv, FilterHarness *filter,
CameraParams camera_params) {
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
gtk_init(&argc, &argv);
diff --git a/aos/vision/tools/camera_primer.cc b/aos/vision/tools/camera_primer.cc
index 1ac96fd..e1d5fc5 100644
--- a/aos/vision/tools/camera_primer.cc
+++ b/aos/vision/tools/camera_primer.cc
@@ -27,7 +27,7 @@
// target_sender
int main(int argc, char **argv) {
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
aos::vision::CameraParams params;
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
index 5267041..4ba290c 100644
--- a/aos/vision/tools/jpeg_vision_test.cc
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -112,7 +112,7 @@
int main(int argc, char *argv[]) {
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
aos::events::EpollLoop loop;
gtk_init(&argc, &argv);
diff --git a/debian/packages.bzl b/debian/packages.bzl
index 8054d1d..ba1f840 100644
--- a/debian/packages.bzl
+++ b/debian/packages.bzl
@@ -55,13 +55,14 @@
)
def _convert_deb_to_target(deb):
- """Converts a debian package filename to a valid bazel target name."""
- target = deb.split("_")[0]
- target = target.replace("-", "_")
- target = target.replace(".", "_")
- target = target.replace(":", "_")
- target = target.replace("+", "x")
- return "deb_%s_repo" % target
+ """Converts a debian package filename to a valid bazel target name."""
+ target = deb
+ target = target.replace('-', '_')
+ target = target.replace('.', '_')
+ target = target.replace(':', '_')
+ target = target.replace('+', 'x')
+ target = target.replace('~', '_')
+ return "deb_%s_repo" % target
def generate_repositories_for_debs(files, base_url = "http://www.frc971.org/Build-Dependencies"):
"""A WORKSPACE helper to add all the deb packages in the dictionary as a repo.
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index a723d96..f263d14 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -106,7 +106,8 @@
":drivetrain_status_fbs",
":drivetrain_position_fbs",
":localizer_fbs",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
+ "//frc971/queues:gyro_uid_fbs",
"//frc971/wpilib:imu_fbs",
],
visibility = ["//visibility:public"],
@@ -382,7 +383,7 @@
"//aos/controls:control_loop",
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
"//frc971/wpilib:imu_fbs",
],
)
@@ -403,7 +404,7 @@
"//aos/testing:googletest",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
],
@@ -434,7 +435,7 @@
":drivetrain_test_lib",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
] + cpu_select({
"amd64": [
"//third_party/matplotlib-cpp",
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index dc828bb..df64659 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -13,6 +13,11 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.sensors.Uid",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Goal",
"frequency": 200
},
diff --git a/frc971/queues/BUILD b/frc971/queues/BUILD
index e440638..4005092 100644
--- a/frc971/queues/BUILD
+++ b/frc971/queues/BUILD
@@ -3,9 +3,17 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
flatbuffer_cc_library(
- name = "gyro",
+ name = "gyro_fbs",
srcs = [
"gyro.fbs",
],
gen_reflections = 1,
)
+
+flatbuffer_cc_library(
+ name = "gyro_uid_fbs",
+ srcs = [
+ "gyro_uid.fbs",
+ ],
+ gen_reflections = 1,
+)
diff --git a/frc971/queues/gyro.fbs b/frc971/queues/gyro.fbs
index 8a76678..a6dce45 100644
--- a/frc971/queues/gyro.fbs
+++ b/frc971/queues/gyro.fbs
@@ -10,9 +10,4 @@
velocity:double;
}
-// Published on "/drivetrain"
-table Uid {
- uid:uint;
-}
-
root_type GyroReading;
diff --git a/frc971/queues/gyro_uid.fbs b/frc971/queues/gyro_uid.fbs
new file mode 100644
index 0000000..05934bf
--- /dev/null
+++ b/frc971/queues/gyro_uid.fbs
@@ -0,0 +1,8 @@
+namespace frc971.sensors;
+
+// Published on "/drivetrain"
+table Uid {
+ uid:uint;
+}
+
+root_type Uid;
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 48fda81..b1bf587 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -115,7 +115,8 @@
"//aos/robot_state:robot_state_fbs",
"//aos/time",
"//aos/util:phased_loop",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
+ "//frc971/queues:gyro_uid_fbs",
"//frc971/zeroing:averager",
],
)
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 4f88f9e..e7273ee 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -14,6 +14,7 @@
#include "aos/time/time.h"
#include "frc971/queues/gyro_generated.h"
+#include "frc971/queues/gyro_uid_generated.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 8125598..a0048fb 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -9,6 +9,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/robot_state/robot_state_generated.h"
#include "frc971/queues/gyro_generated.h"
+#include "frc971/queues/gyro_uid_generated.h"
#include "frc971/wpilib/gyro_interface.h"
#include "frc971/zeroing/averager.h"
diff --git a/y2014/BUILD b/y2014/BUILD
index b2617da..6ff529a 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -36,7 +36,7 @@
"//aos/util:log_interval",
"//frc971/autonomous:auto_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
"//y2014/actors:shoot_action_lib",
"//y2014/control_loops/claw:claw_goal_fbs",
"//y2014/control_loops/claw:claw_status_fbs",
diff --git a/y2016/BUILD b/y2016/BUILD
index 520dd4b..a96ea5a 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -38,7 +38,7 @@
"//frc971/autonomous:auto_fbs",
"//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
- "//frc971/queues:gyro",
+ "//frc971/queues:gyro_fbs",
"//y2016/actors:autonomous_action_lib",
"//y2016/actors:superstructure_action_lib",
"//y2016/actors:vision_align_action_lib",
@@ -51,6 +51,9 @@
)
robot_downloader(
+ data = [
+ ":config.json",
+ ],
dirs = [
"//y2016/dashboard:www_files",
],
@@ -81,12 +84,17 @@
"//y2016/control_loops/superstructure:superstructure_position_fbs",
"//y2016/control_loops/superstructure:superstructure_status_fbs",
"//y2016/queues:ball_detector_fbs",
- "//y2017/vision:vision_fbs",
+ "//y2016/vision:vision_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2016/actors:vision_align_action_fbs",
+ "//y2016/actors:superstructure_action_fbs",
],
visibility = ["//visibility:public"],
deps = [
"//aos/robot_state:config",
+ "//frc971/autonomous:config",
"//frc971/control_loops/drivetrain:config",
+ "//frc971/wpilib:config",
],
)
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index a665c28..0b11bdd 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -64,7 +64,7 @@
actors::VisionAlignActor::MakeFactory(event_loop)),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- "/superstructure")),
+ "/vision")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
"/superstructure")),
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index ae05c70..1761a72 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -24,7 +24,7 @@
event_loop, "/vision_align_action"),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- "/superstructure")),
+ "/vision")),
drivetrain_goal_sender_(
event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
"/drivetrain")) {}
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 8aec0de..3361e96 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -53,13 +53,13 @@
: event_loop_(event_loop),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- "/superstructure")),
+ "/vision")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
"/superstructure")),
autonomous_mode_fetcher_(
event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
- "/aos")),
+ "/autonomous")),
superstructure_status_fetcher_(
event_loop
->MakeFetcher<::y2016::control_loops::superstructure::Status>(
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index e9a2e1d..93ec131 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -305,7 +305,7 @@
::aos::ShmEventLoop event_loop(&config.message());
::aos::Sender<::y2016::vision::VisionStatus> vision_status_sender =
- event_loop.MakeSender<::y2016::vision::VisionStatus>("/superstructure");
+ event_loop.MakeSender<::y2016::vision::VisionStatus>("/vision");
StereoGeometry stereo(constants::GetValues().vision_name);
AOS_LOG(INFO, "calibration: %s\n",
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
index 3e29085..41da462 100644
--- a/y2016/vision/target_sender.cc
+++ b/y2016/vision/target_sender.cc
@@ -226,7 +226,7 @@
using namespace y2016::vision;
StereoGeometry stereo("./stereo_rig.calib");
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
std::thread cam0([stereo]() {
RunCamera(0, GetCameraParams(stereo.calibration()),
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index dc828f3..5c2fcd6 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -153,7 +153,7 @@
"/superstructure")),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- "/aos")),
+ "/autonomous")),
shooter_position_sender_(
event_loop->MakeSender<shooter::Position>("/shooter")),
superstructure_position_sender_(
diff --git a/y2016/y2016.json b/y2016/y2016.json
index eb8ecd5..9ad4f20 100644
--- a/y2016/y2016.json
+++ b/y2016/y2016.json
@@ -45,10 +45,45 @@
"name": "/superstructure",
"type": "y2016.sensors.BallDetector",
"frequency": 200
+ },
+ {
+ "name": "/vision",
+ "type": "y2016.vision.VisionStatus",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure_action",
+ "type": "aos.common.actions.Status"
+ },
+ {
+ "name": "/vision_align_action",
+ "type": "aos.common.actions.Status"
+ },
+ {
+ "name": "/autonomous",
+ "type": "aos.common.actions.Status"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.Goal"
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2019.control_loops.drivetrain.TargetSelectorHint"
+ },
+ {
+ "name": "/vision_align_action",
+ "type": "y2016.actors.vision_align_action.Goal"
+ },
+ {
+ "name": "/superstructure_action",
+ "type": "y2016.actors.superstructure_action.Goal"
}
],
"imports": [
"../aos/robot_state/robot_state_config.json",
- "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ "../frc971/control_loops/drivetrain/drivetrain_config.json",
+ "../frc971/autonomous/autonomous_config.json",
+ "../frc971/wpilib/wpilib_config.json"
]
}
diff --git a/y2017/vision/target_sender.cc b/y2017/vision/target_sender.cc
index 261c397..88655c7 100644
--- a/y2017/vision/target_sender.cc
+++ b/y2017/vision/target_sender.cc
@@ -220,7 +220,7 @@
using namespace y2017::vision;
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stdout));
VisionConfig cfg;
if (ReadConfiguration("ConfigFile.pb.ascii", &cfg)) {
diff --git a/y2018/vision/image_streamer.cc b/y2018/vision/image_streamer.cc
index b0c1f86..fa5f4b5 100644
--- a/y2018/vision/image_streamer.cc
+++ b/y2018/vision/image_streamer.cc
@@ -289,7 +289,7 @@
int main(int argc, char ** argv) {
gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
TCPServer<MjpegDataSocket> tcp_server_(80);
diff --git a/y2019/image_streamer/image_streamer.cc b/y2019/image_streamer/image_streamer.cc
index 30bea8f..6f8027c 100644
--- a/y2019/image_streamer/image_streamer.cc
+++ b/y2019/image_streamer/image_streamer.cc
@@ -308,7 +308,7 @@
int main(int argc, char **argv) {
gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
TCPServer<MjpegDataSocket> tcp_server_(80);
aos::vision::CameraParams params0;
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
index 857f20f..c707d1f 100644
--- a/y2019/vision/debug_serial.cc
+++ b/y2019/vision/debug_serial.cc
@@ -24,7 +24,7 @@
using namespace frc971::jevois;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
int flags = fcntl(0, F_GETFL, 0);
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index e2c4490..52a6e52 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -103,7 +103,7 @@
};
::aos::logging::Init();
- ::aos::logging::AddImplementation(
+ ::aos::logging::SetImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
TargetFinder target_finder;
diff --git a/y2020/BUILD b/y2020/BUILD
new file mode 100644
index 0000000..17d3020
--- /dev/null
+++ b/y2020/BUILD
@@ -0,0 +1,125 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+robot_downloader(
+ data = [
+ ":config.json",
+ ],
+ start_binaries = [
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//y2020/control_loops/drivetrain:drivetrain",
+ "//y2020/control_loops/superstructure:superstructure",
+ "//y2020/actors:binaries",
+ ],
+)
+
+cc_library(
+ name = "constants",
+ srcs = [
+ "constants.cc",
+ ],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/logging",
+ "//aos/mutex",
+ "//aos/network:team_number",
+ "//frc971:constants",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//y2020/control_loops/drivetrain:polydrivetrain_plants",
+ "@com_google_absl//absl/base",
+ ],
+)
+
+cc_binary(
+ name = "wpilib_interface",
+ srcs = [
+ "wpilib_interface.cc",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ ":constants",
+ "//aos:init",
+ "//aos:make_unique",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//aos/robot_state:robot_state_fbs",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/util:phased_loop",
+ "//aos/util:wrapping_counter",
+ "//frc971/autonomous:auto_mode_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/wpilib:ADIS16448",
+ "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:drivetrain_writer",
+ "//frc971/wpilib:encoder_and_potentiometer",
+ "//frc971/wpilib:interrupt_edge_counting",
+ "//frc971/wpilib:joystick_sender",
+ "//frc971/wpilib:logging_fbs",
+ "//frc971/wpilib:loop_output_handler",
+ "//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_interface",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ "//y2020/control_loops/superstructure:superstructure_output_fbs",
+ "//y2020/control_loops/superstructure:superstructure_position_fbs",
+ ],
+)
+
+cc_binary(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+aos_config(
+ name = "config",
+ src = "y2020.json",
+ flatbuffers = [
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_output_fbs",
+ "//y2020/control_loops/superstructure:superstructure_position_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/autonomous:config",
+ "//frc971/control_loops/drivetrain:config",
+ "//frc971/wpilib:config",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2020/__init__.py b/y2020/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2020/__init__.py
diff --git a/y2020/actors/BUILD b/y2020/actors/BUILD
new file mode 100644
index 0000000..668c502
--- /dev/null
+++ b/y2020/actors/BUILD
@@ -0,0 +1,53 @@
+filegroup(
+ name = "binaries.stripped",
+ srcs = [
+ ":autonomous_action.stripped",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+filegroup(
+ name = "binaries",
+ srcs = [
+ ":autonomous_action",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "autonomous_action_lib",
+ srcs = [
+ "auto_splines.cc",
+ "autonomous_actor.cc",
+ ],
+ hdrs = [
+ "auto_splines.h",
+ "autonomous_actor.h",
+ ],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/logging",
+ "//aos/util:phased_loop",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_binary(
+ name = "autonomous_action",
+ srcs = [
+ "autonomous_actor_main.cc",
+ ],
+ deps = [
+ ":autonomous_action_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/autonomous:auto_fbs",
+ ],
+)
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
new file mode 100644
index 0000000..86eed14
--- /dev/null
+++ b/y2020/actors/auto_splines.cc
@@ -0,0 +1,103 @@
+#include "y2020/actors/auto_splines.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+
+namespace y2020 {
+namespace actors {
+
+void MaybeFlipSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
+ bool is_left) {
+ flatbuffers::Vector<float> *spline_y =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+
+ if (!is_left) {
+ for (size_t i = 0; i < spline_y->size(); i++) {
+ spline_y->Mutate(i, -spline_y->Get(i));
+ }
+ }
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
+
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
+
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::VOLTAGE);
+ voltage_constraint_builder.add_value(6.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset});
+
+ const float startx = 0.4;
+ const float starty = 3.4;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+ 0.6f + startx, 0.4f + startx,
+ 0.4f + startx, 1.0f + startx});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+ starty - 0.3f, starty - 0.7f,
+ starty - 1.0f, starty - 1.0f});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
+}
+
+} // namespace actors
+} // namespace y2020
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
new file mode 100644
index 0000000..471b7b9
--- /dev/null
+++ b/y2020/actors/auto_splines.h
@@ -0,0 +1,28 @@
+#ifndef y2020_ACTORS_AUTO_SPLINES_H_
+#define y2020_ACTORS_AUTO_SPLINES_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+/*
+
+ The cooridinate system for the autonomous splines is the same as the spline
+ python generator and drivetrain spline systems.
+
+*/
+
+namespace y2020 {
+namespace actors {
+
+class AutonomousSplines {
+ public:
+ static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+};
+
+} // namespace actors
+} // namespace y2020
+
+#endif // y2020_ACTORS_AUTO_SPLINES_H_
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
new file mode 100644
index 0000000..d015c65
--- /dev/null
+++ b/y2020/actors/autonomous_actor.cc
@@ -0,0 +1,38 @@
+#include "y2020/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include <chrono>
+#include <cmath>
+
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace actors {
+
+using ::aos::monotonic_clock;
+using ::frc971::ProfileParametersT;
+using frc971::control_loops::drivetrain::LocalizerControl;
+namespace chrono = ::std::chrono;
+
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+ : frc971::autonomous::BaseAutonomousActor(
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+void AutonomousActor::Reset() {
+ InitializeEncoders();
+ ResetDrivetrain();
+}
+
+bool AutonomousActor::RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) {
+ Reset();
+
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
+ return true;
+}
+
+} // namespace actors
+} // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
new file mode 100644
index 0000000..3bfa610
--- /dev/null
+++ b/y2020/actors/autonomous_actor.h
@@ -0,0 +1,27 @@
+#ifndef y2020_ACTORS_AUTONOMOUS_ACTOR_H_
+#define y2020_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2020 {
+namespace actors {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit AutonomousActor(::aos::EventLoop *event_loop);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+ void Reset();
+};
+
+} // namespace actors
+} // namespace y2020
+
+#endif // y2020_ACTORS_AUTONOMOUS_ACTOR_H_
diff --git a/y2020/actors/autonomous_actor_main.cc b/y2020/actors/autonomous_actor_main.cc
new file mode 100644
index 0000000..99563ee
--- /dev/null
+++ b/y2020/actors/autonomous_actor_main.cc
@@ -0,0 +1,20 @@
+#include <stdio.h>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2020/actors/autonomous_actor.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init(-1);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2020::actors::AutonomousActor autonomous(&event_loop);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2020/constants.cc b/y2020/constants.cc
new file mode 100644
index 0000000..70876c3
--- /dev/null
+++ b/y2020/constants.cc
@@ -0,0 +1,84 @@
+#include "y2020/constants.h"
+
+#include <inttypes.h>
+
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "aos/logging/logging.h"
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2020 {
+namespace constants {
+
+const int Values::kZeroingSampleSize;
+
+namespace {
+
+const uint16_t kCompTeamNumber = 971;
+const uint16_t kPracticeTeamNumber = 9971;
+const uint16_t kCodingRobotTeamNumber = 7971;
+
+const Values *DoGetValuesForTeam(uint16_t team) {
+ Values *const r = new Values();
+
+ switch (team) {
+ // A set of constants for tests.
+ case 1:
+ break;
+
+ case kCompTeamNumber:
+ break;
+
+ case kPracticeTeamNumber:
+ break;
+
+ case kCodingRobotTeamNumber:
+ break;
+
+ default:
+ AOS_LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ }
+
+ return r;
+}
+
+void DoGetValues(const Values **result) {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ AOS_LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ *result = DoGetValuesForTeam(team);
+}
+
+} // namespace
+
+const Values &GetValues() {
+ static absl::once_flag once;
+ static const Values *result;
+ absl::call_once(once, DoGetValues, &result);
+ return *result;
+}
+
+const Values &GetValuesForTeam(uint16_t team_number) {
+ static ::aos::Mutex mutex;
+ ::aos::MutexLocker locker(&mutex);
+
+ // 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) {
+ values[team_number] = DoGetValuesForTeam(team_number);
+#if __has_feature(address_sanitizer)
+ __lsan_ignore_object(values[team_number]);
+#endif
+ }
+ return *values[team_number];
+}
+
+} // namespace constants
+} // namespace y2020
diff --git a/y2020/constants.h b/y2020/constants.h
new file mode 100644
index 0000000..2e915f9
--- /dev/null
+++ b/y2020/constants.h
@@ -0,0 +1,44 @@
+#ifndef y2020_CONSTANTS_H_
+#define y2020_CONSTANTS_H_
+
+#include <math.h>
+#include <stdint.h>
+
+#include <array>
+
+#include "frc971/constants.h"
+#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"
+
+namespace y2020 {
+namespace constants {
+
+struct Values {
+ static const int kZeroingSampleSize = 200;
+
+ static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+ static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+ return kDrivetrainCyclesPerRevolution() * 4;
+ }
+ static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+ static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+ return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::drivetrain::kHighOutputRatio /
+ constants::Values::kDrivetrainEncoderRatio() *
+ kDrivetrainEncoderCountsPerRevolution();
+ }
+};
+
+// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
+// returns a reference to it.
+const Values &GetValues();
+
+// Creates Values instances for each team number it is called with and returns
+// them.
+const Values &GetValuesForTeam(uint16_t team_number);
+
+} // namespace constants
+} // namespace y2020
+
+#endif // y2020_CONSTANTS_H_
diff --git a/y2020/control_loops/BUILD b/y2020/control_loops/BUILD
new file mode 100644
index 0000000..c0aa1ee
--- /dev/null
+++ b/y2020/control_loops/BUILD
@@ -0,0 +1,6 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2020:python_init"],
+)
diff --git a/y2020/control_loops/__init__.py b/y2020/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2020/control_loops/__init__.py
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..d03c1b9
--- /dev/null
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -0,0 +1,83 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
+
+genrule(
+ name = "genrule_drivetrain",
+ outs = [
+ "drivetrain_dog_motor_plant.h",
+ "drivetrain_dog_motor_plant.cc",
+ "kalman_drivetrain_motor_plant.h",
+ "kalman_drivetrain_motor_plant.cc",
+ ],
+ cmd = "$(location //y2020/control_loops/python:drivetrain) $(OUTS)",
+ tools = [
+ "//y2020/control_loops/python:drivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_polydrivetrain",
+ outs = [
+ "polydrivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.cc",
+ "polydrivetrain_cim_plant.h",
+ "polydrivetrain_cim_plant.cc",
+ "hybrid_velocity_drivetrain.h",
+ "hybrid_velocity_drivetrain.cc",
+ ],
+ cmd = "$(location //y2020/control_loops/python:polydrivetrain) $(OUTS)",
+ tools = [
+ "//y2020/control_loops/python:polydrivetrain",
+ ],
+)
+
+cc_library(
+ name = "polydrivetrain_plants",
+ srcs = [
+ "drivetrain_dog_motor_plant.cc",
+ "hybrid_velocity_drivetrain.cc",
+ "kalman_drivetrain_motor_plant.cc",
+ "polydrivetrain_dog_motor_plant.cc",
+ ],
+ hdrs = [
+ "drivetrain_dog_motor_plant.h",
+ "hybrid_velocity_drivetrain.h",
+ "kalman_drivetrain_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_base",
+ srcs = [
+ "drivetrain_base.cc",
+ ],
+ hdrs = [
+ "drivetrain_base.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":polydrivetrain_plants",
+ "//frc971:shifter_hall_effect",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ ],
+)
+
+cc_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":drivetrain_base",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ ],
+)
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..c3597f5
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,63 @@
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2020/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2020/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ static DrivetrainConfig<double> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+
+ drivetrain::MakeDrivetrainLoop,
+ drivetrain::MakeVelocityDrivetrainLoop,
+ drivetrain::MakeKFDrivetrainLoop,
+ drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio,
+ drivetrain::kLowGearRatio,
+ drivetrain::kJ,
+ drivetrain::kMass,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */
+ ,
+ 0.7 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */,
+ 1.2 /* wheel_multiplier */,
+ true /*pistol_grip_shift_enables_line_follow*/,
+ };
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.h b/y2020/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..c220088
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
+
+#endif // y2020_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..66b9cc7
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,25 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main() {
+ ::aos::InitNRT(true);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ &event_loop, ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
+ DrivetrainLoop drivetrain(
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
new file mode 100644
index 0000000..edc72f5
--- /dev/null
+++ b/y2020/control_loops/python/BUILD
@@ -0,0 +1,57 @@
+package(default_visibility = ["//y2020:__subpackages__"])
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:drivetrain",
+ ],
+)
+
+py_binary(
+ name = "polydrivetrain",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+py_library(
+ name = "polydrivetrain_lib",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ "//frc971/control_loops/python:drivetrain",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2020/control_loops:python_init"],
+)
diff --git a/y2020/control_loops/python/__init__.py b/y2020/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2020/control_loops/python/__init__.py
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..54745dd
--- /dev/null
+++ b/y2020/control_loops/python/drivetrain.py
@@ -0,0 +1,42 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import drivetrain
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+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,
+ 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)
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ # 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/polydrivetrain.py b/y2020/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..6481af1
--- /dev/null
+++ b/y2020/control_loops/python/polydrivetrain.py
@@ -0,0 +1,31 @@
+#!/usr/bin/python
+
+import sys
+from y2020.control_loops.python import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def main(argv):
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2020',
+ drivetrain.kDrivetrain)
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..533a369
--- /dev/null
+++ b/y2020/control_loops/superstructure/BUILD
@@ -0,0 +1,78 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
+ srcs = [
+ "superstructure_goal.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+cc_library(
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/controls:control_loop",
+ "//aos/events:event_loop",
+ "//y2020:constants",
+ ],
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..a3978a4
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,43 @@
+#include "y2020/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
+ const Position * /*position*/,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ if (WasReset()) {
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ }
+
+
+ if (output != nullptr) {
+ OutputT output_struct;
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_zeroed(true);
+ status_builder.add_estopped(false);
+
+ status->Send(status_builder.Finish());
+}
+
+} // namespace control_loops
+} // namespace y2020
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..9aaca8e
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -0,0 +1,35 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/controls/control_loop.h"
+#include "aos/events/event_loop.h"
+#include "y2020/constants.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"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
+
+ protected:
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..8a38b95
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2020.control_loops.superstructure;
+
+table Goal {
+
+}
+
+root_type Goal;
diff --git a/y2020/control_loops/superstructure/superstructure_main.cc b/y2020/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..101a0c7
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,20 @@
+#include "y2020/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::InitNRT(true);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2020::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2020/control_loops/superstructure/superstructure_output.fbs b/y2020/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..3682db6
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,7 @@
+namespace y2020.control_loops.superstructure;
+
+table Output {
+
+}
+
+root_type Output;
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..c4b0a9a
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2020.control_loops.superstructure;
+
+table Position {
+
+}
+
+root_type Position;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..d0e9266
--- /dev/null
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,15 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2020.control_loops.superstructure;
+
+table Status {
+ // All subsystems know their location.
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+}
+
+root_type Status;
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
new file mode 100644
index 0000000..105506e
--- /dev/null
+++ b/y2020/joystick_reader.cc
@@ -0,0 +1,79 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+using aos::input::driver_station::ButtonLocation;
+using aos::input::driver_station::ControlBit;
+using aos::input::driver_station::JoystickAxis;
+using aos::input::driver_station::POVLocation;
+
+namespace y2020 {
+namespace input {
+namespace joysticks {
+
+namespace superstructure = y2020::control_loops::superstructure;
+
+class Reader : public ::aos::input::ActionJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::aos::input::ActionJoystickInput(
+ event_loop,
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig(),
+ ::aos::input::DrivetrainInputReader::InputType::kPistol, {}),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<superstructure::Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<superstructure::Status>(
+ "/superstructure")) {}
+
+ void AutoEnded() override {
+ AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
+ }
+
+ void HandleTeleop(
+ const ::aos::input::driver_station::Data & /*data*/) override {
+ superstructure_status_fetcher_.Fetch();
+ if (!superstructure_status_fetcher_.get()) {
+ AOS_LOG(ERROR, "Got no superstructure status message.\n");
+ return;
+ }
+ }
+
+ private:
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2020
+
+int main() {
+ ::aos::InitNRT(true);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2020::input::joysticks::Reader reader(&event_loop);
+
+ event_loop.Run();
+
+ ::aos::Cleanup();
+}
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
new file mode 100644
index 0000000..4968c9f
--- /dev/null
+++ b/y2020/wpilib_interface.cc
@@ -0,0 +1,248 @@
+#include <inttypes.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+#include "ctre/phoenix/CANifier.h"
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "aos/commonmath.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/make_unique.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
+
+using ::aos::monotonic_clock;
+using ::y2020::constants::Values;
+namespace superstructure = ::y2020::control_loops::superstructure;
+namespace chrono = ::std::chrono;
+using aos::make_unique;
+
+namespace y2020 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
+// 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()) *
+ (2.0 * M_PI)) *
+ Values::kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius;
+}
+
+double drivetrain_velocity_translate(double in) {
+ return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
+ (2.0 * M_PI)) *
+ Values::kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius;
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond =
+ Values::kMaxDrivetrainEncoderPulsesPerSecond();
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+constexpr double kMaxMediumEncoderPulsesPerSecond = kMaxFastEncoderPulsesPerSecond;
+
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+ "medium encoders are too fast");
+
+} // namespace
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader(::aos::ShmEventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop),
+ auto_mode_sender_(
+ event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+ "/autonomous")),
+ superstructure_position_sender_(
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
+ drivetrain_position_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
+ // Set to filter out anything shorter than 1/4 of the minimum pulse width
+ // we should ever see.
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
+ }
+
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
+ void RunIteration() override {
+ {
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ drivetrain_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+ drivetrain_builder.add_right_encoder(
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
+
+ builder.Send(drivetrain_builder.Finish());
+ }
+
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+ builder.Send(position_builder.Finish());
+ }
+
+ {
+ auto builder = auto_mode_sender_.MakeBuilder();
+
+ uint32_t mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+ mode |= 1 << i;
+ }
+ }
+
+ auto auto_mode_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+ auto_mode_builder.add_mode(mode);
+
+ builder.Send(auto_mode_builder.Finish());
+ }
+ }
+
+ private:
+ ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
+ drivetrain_position_sender_;
+
+ ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+};
+
+class SuperstructureWriter
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
+ public:
+ SuperstructureWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure") {}
+
+ private:
+ void Write(const superstructure::Output & /*output*/) override {}
+
+ void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+
+ void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ // Thread 1.
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+ ::frc971::wpilib::JoystickSender joystick_sender(
+ &joystick_sender_event_loop);
+ AddLoop(&joystick_sender_event_loop);
+
+ // Thread 2.
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
+ ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+ AddLoop(&pdp_fetcher_event_loop);
+
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+ SensorReader sensor_reader(&sensor_reader_event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ // Thread 4.
+ ::aos::ShmEventLoop output_event_loop(&config.message());
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
+
+ SuperstructureWriter superstructure_writer(&output_event_loop);
+
+ AddLoop(&output_event_loop);
+
+ RunLoops();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2020
+
+AOS_ROBOT_CLASS(::y2020::wpilib::WPILibRobot);
diff --git a/y2020/y2020.json b/y2020/y2020.json
new file mode 100644
index 0000000..411e3ff
--- /dev/null
+++ b/y2020/y2020.json
@@ -0,0 +1,36 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2020.control_loops.superstructure.Position",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json",
+ "../frc971/autonomous/autonomous_config.json",
+ "../frc971/wpilib/wpilib_config.json"
+ ]
+}