Merge "Add y2023_bot3 folder"
diff --git a/BUILD b/BUILD
index d2c1658..5427fd5 100644
--- a/BUILD
+++ b/BUILD
@@ -69,6 +69,8 @@
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule_response //scouting/webserver/requests/messages:submit_shift_schedule_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking //scouting/webserver/requests/messages:submit_driver_ranking_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking_response //scouting/webserver/requests/messages:submit_driver_ranking_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting //scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response //scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs
gazelle(
name = "gazelle",
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 9a5fe19..132da04 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -408,7 +408,8 @@
void set_timing_report(timing::Watcher *watcher);
void ResetReport();
- virtual void Startup(EventLoop *event_loop) = 0;
+ virtual void Construct() = 0;
+ virtual void Startup() = 0;
protected:
const int channel_index_;
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index c33606c..a9c8497 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -183,18 +183,18 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":file_operations",
":boot_timestamp",
- ":snappy_encoder",
":buffer_encoder",
- ":logger_fbs",
+ ":file_operations",
":log_backend",
- "//aos:sha256",
- "//aos:uuid",
+ ":logger_fbs",
+ ":snappy_encoder",
"//aos:configuration",
- "//aos/containers:error_list",
"//aos:flatbuffer_merge",
"//aos:flatbuffers",
+ "//aos:sha256",
+ "//aos:uuid",
+ "//aos/containers:error_list",
"//aos/containers:resizeable_buffer",
"//aos/events:event_loop",
"//aos/network:remote_message_fbs",
diff --git a/aos/events/logging/logfile_validator.cc b/aos/events/logging/logfile_validator.cc
index 19d11c8..05c881c 100644
--- a/aos/events/logging/logfile_validator.cc
+++ b/aos/events/logging/logfile_validator.cc
@@ -37,12 +37,12 @@
multinode_estimator.set_reboot_found(
[config](distributed_clock::time_point reboot_time,
const std::vector<logger::BootTimestamp> &node_times) {
- LOG(INFO) << "Rebooted at distributed " << reboot_time;
+ VLOG(1) << "Rebooted at distributed " << reboot_time;
size_t node_index = 0;
for (const logger::BootTimestamp &time : node_times) {
- LOG(INFO) << " "
- << config->nodes()->Get(node_index)->name()->string_view()
- << " " << time;
+ VLOG(1) << " "
+ << config->nodes()->Get(node_index)->name()->string_view()
+ << " " << time;
++node_index;
}
});
@@ -94,7 +94,7 @@
// logger on purpose. It loads in *all* the timestamps in 1 go per node,
// ignoring memory usage.
for (const Node *node : configuration::GetNodes(config)) {
- LOG(INFO) << "Reading all data for " << node->name()->string_view();
+ VLOG(1) << "Reading all data for " << node->name()->string_view();
const size_t node_index = configuration::GetNodeIndex(config, node);
TimestampMapper *timestamp_mapper = mappers[node_index].get();
if (timestamp_mapper == nullptr) {
@@ -117,11 +117,11 @@
if (!next_timestamp.has_value() || !next_timestamp.value().has_value()) {
return preempt_destructor(false);
}
- LOG(INFO) << "Starting at:";
+ VLOG(1) << "Starting at:";
for (const Node *node : configuration::GetNodes(config)) {
const size_t node_index = configuration::GetNodeIndex(config, node);
- LOG(INFO) << " " << node->name()->string_view() << " -> "
- << std::get<1>(*next_timestamp.value().value())[node_index].time;
+ VLOG(1) << " " << node->name()->string_view() << " -> "
+ << std::get<1>(*next_timestamp.value().value())[node_index].time;
}
std::vector<monotonic_clock::time_point> just_monotonic(
@@ -148,7 +148,7 @@
std::get<0>(*next_timestamp.value().value()));
}
- LOG(INFO) << "Done";
+ VLOG(1) << "Done";
return preempt_destructor(true);
}
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 3126265..7f06ee0 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -159,7 +159,7 @@
}
}
- bool FetchNext() { return FetchNextIf(std::ref(should_fetch_)); }
+ bool FetchNext() { return FetchNextIf(should_fetch_); }
bool FetchNextIf(std::function<bool(const Context &)> fn) {
const ipc_lib::LocklessQueueReader::Result read_result =
@@ -192,7 +192,7 @@
return read_result == ipc_lib::LocklessQueueReader::Result::GOOD;
}
- bool Fetch() { return FetchIf(std::ref(should_fetch_)); }
+ bool Fetch() { return FetchIf(should_fetch_); }
Context context() const { return context_; }
@@ -557,10 +557,14 @@
event_loop_->RemoveEvent(&event_);
}
- void Startup(EventLoop *event_loop) override {
+ void Construct() override {
+ event_loop_->CheckCurrentThread();
+ CHECK(RegisterWakeup(event_loop_->runtime_realtime_priority()));
+ }
+
+ void Startup() override {
event_loop_->CheckCurrentThread();
simple_shm_fetcher_.PointAtNextQueueIndex();
- CHECK(RegisterWakeup(event_loop->runtime_realtime_priority()));
}
// Returns true if there is new data available.
@@ -1048,6 +1052,16 @@
if (!CPU_EQUAL(&affinity_, &default_affinity)) {
::aos::SetCurrentThreadAffinity(affinity_);
}
+
+ // Construct the watchers, but don't update the next pointer. This also
+ // cleans up any watchers that previously died, and puts the nonrt work
+ // before going realtime. After this happens, we will start queueing
+ // signals (which may be a bit of extra work to process, but won't cause any
+ // messages to be lost).
+ for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
+ watcher->Construct();
+ }
+
// Now, all the callbacks are setup. Lock everything into memory and go RT.
if (priority_ != 0) {
::aos::InitRT();
@@ -1059,9 +1073,10 @@
set_is_running(true);
// Now that we are realtime (but before the OnRun handlers run), snap the
- // queue index.
+ // queue index pointer to the newest message. This happens in RT so that we
+ // minimize the risk of losing messages.
for (::std::unique_ptr<WatcherState> &watcher : watchers_) {
- watcher->Startup(this);
+ watcher->Startup();
}
// Now that we are RT, run all the OnRun handlers.
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 6b40b9d..2382f07 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -26,12 +26,12 @@
}
// Clean up anything left there before.
- unlink((FLAGS_shm_base + "/test/aos.TestMessage.v5").c_str());
- unlink((FLAGS_shm_base + "/test1/aos.TestMessage.v5").c_str());
- unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v5").c_str());
- unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v5").c_str());
- unlink((FLAGS_shm_base + "/aos/aos.timing.Report.v5").c_str());
- unlink((FLAGS_shm_base + "/aos/aos.logging.LogMessageFbs.v5").c_str());
+ unlink((FLAGS_shm_base + "/test/aos.TestMessage.v6").c_str());
+ unlink((FLAGS_shm_base + "/test1/aos.TestMessage.v6").c_str());
+ unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v6").c_str());
+ unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v6").c_str());
+ unlink((FLAGS_shm_base + "/aos/aos.timing.Report.v6").c_str());
+ unlink((FLAGS_shm_base + "/aos/aos.logging.LogMessageFbs.v6").c_str());
}
~ShmEventLoopTestFactory() { FLAGS_override_hostname = ""; }
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 7b84380..5cb67b4 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -100,7 +100,8 @@
void Handle() noexcept override;
- void Startup(EventLoop * /*event_loop*/) override {}
+ void Construct() override {}
+ void Startup() override {}
void Schedule(std::shared_ptr<SimulatedMessage> message);
diff --git a/aos/events/simulated_network_bridge.cc b/aos/events/simulated_network_bridge.cc
index c8a8150..3ed39fa 100644
--- a/aos/events/simulated_network_bridge.cc
+++ b/aos/events/simulated_network_bridge.cc
@@ -234,7 +234,7 @@
// channel within the same callback.
LOG(WARNING) << "Not forwarding message on "
<< configuration::CleanedChannelToString(fetcher_->channel())
- << " because we aren't running. Set at "
+ << " because we aren't running. Sent at "
<< fetcher_->context().monotonic_event_time << " now is "
<< fetch_node_factory_->monotonic_now();
sent_ = true;
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index d67616f..75bbd4d 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -193,10 +193,12 @@
"lockless_queue.cc",
"lockless_queue_memory.h",
"memory_mapped_queue.cc",
+ "robust_ownership_tracker.cc",
],
hdrs = [
"lockless_queue.h",
"memory_mapped_queue.h",
+ "robust_ownership_tracker.h",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -210,6 +212,7 @@
"//aos/events:context",
"//aos/time",
"//aos/util:compiler_memory_barrier",
+ "//aos/util:top",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
"@com_google_absl//absl/types:span",
@@ -250,6 +253,16 @@
)
cc_test(
+ name = "robust_ownership_tracker_test",
+ srcs = ["robust_ownership_tracker_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":lockless_queue",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_test(
name = "lockless_queue_test",
timeout = "eternal",
srcs = ["lockless_queue_test.cc"],
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index fd3a305..f033e60 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -172,9 +172,7 @@
size_t valid_senders = 0;
for (size_t i = 0; i < num_senders; ++i) {
Sender *sender = memory->GetSender(i);
- const uint32_t tid =
- __atomic_load_n(&(sender->tid.futex), __ATOMIC_ACQUIRE);
- if (!(tid & FUTEX_OWNER_DIED)) {
+ if (!sender->ownership_tracker.OwnerIsDefinitelyAbsolutelyDead()) {
// Not dead.
++valid_senders;
continue;
@@ -227,7 +225,7 @@
// always be a NOP if it's in 1), verified by a DCHECK.
memory->GetMessage(scratch_index)->header.queue_index.RelaxedInvalidate();
- __atomic_store_n(&(sender->tid.futex), 0, __ATOMIC_RELEASE);
+ sender->ownership_tracker.ForceClear();
++valid_senders;
continue;
}
@@ -245,7 +243,7 @@
aos_compiler_memory_barrier();
// And mark that we succeeded.
- __atomic_store_n(&(sender->tid.futex), 0, __ATOMIC_RELEASE);
+ sender->ownership_tracker.ForceClear();
++valid_senders;
continue;
}
@@ -259,13 +257,11 @@
// read it before it's set.
for (size_t i = 0; i < num_pinners; ++i) {
Pinner *const pinner = memory->GetPinner(i);
- const uint32_t tid =
- __atomic_load_n(&(pinner->tid.futex), __ATOMIC_ACQUIRE);
- if (!(tid & FUTEX_OWNER_DIED)) {
+ if (!pinner->ownership_tracker.OwnerIsDefinitelyAbsolutelyDead()) {
continue;
}
pinner->pinned.Invalidate();
- __atomic_store_n(&(pinner->tid.futex), 0, __ATOMIC_RELEASE);
+ pinner->ownership_tracker.ForceClear();
}
// If all the senders are (or were made) good, there is no need to do the hard
@@ -284,9 +280,7 @@
num_missing = 0;
for (size_t i = 0; i < num_senders; ++i) {
Sender *const sender = memory->GetSender(i);
- const uint32_t tid =
- __atomic_load_n(&(sender->tid.futex), __ATOMIC_ACQUIRE);
- if (tid & FUTEX_OWNER_DIED) {
+ if (sender->ownership_tracker.OwnerIsDefinitelyAbsolutelyDead()) {
if (!need_recovery[i]) {
return false;
}
@@ -331,9 +325,7 @@
const size_t starting_num_missing = num_missing;
for (size_t i = 0; i < num_senders; ++i) {
Sender *sender = memory->GetSender(i);
- const uint32_t tid =
- __atomic_load_n(&(sender->tid.futex), __ATOMIC_ACQUIRE);
- if (!(tid & FUTEX_OWNER_DIED)) {
+ if (!sender->ownership_tracker.OwnerIsDefinitelyAbsolutelyDead()) {
CHECK(!need_recovery[i]) << ": Somebody else recovered a sender: " << i;
continue;
}
@@ -368,7 +360,7 @@
->header.queue_index.RelaxedInvalidate();
// And then mark this sender clean.
- __atomic_store_n(&(sender->tid.futex), 0, __ATOMIC_RELEASE);
+ sender->ownership_tracker.ForceClear();
need_recovery[i] = false;
// And account for scratch_index.
@@ -394,7 +386,7 @@
sender->to_replace.Invalidate();
// And then mark this sender clean.
- __atomic_store_n(&(sender->tid.futex), 0, __ATOMIC_RELEASE);
+ sender->ownership_tracker.ForceClear();
need_recovery[i] = false;
// And account for to_replace.
@@ -437,6 +429,14 @@
} // namespace
+bool PretendThatOwnerIsDeadForTesting(aos_mutex *mutex, pid_t tid) {
+ if (static_cast<pid_t>(mutex->futex & FUTEX_TID_MASK) == tid) {
+ mutex->futex = FUTEX_OWNER_DIED;
+ return true;
+ }
+ return false;
+}
+
size_t LocklessQueueConfiguration::message_size() const {
// Round up the message size so following data is aligned appropriately.
// Make sure to leave space to align the message data. It will be aligned
@@ -685,11 +685,10 @@
CHECK_NE(watcher_index_, -1);
// Make sure we still own the slot we are supposed to.
- CHECK(
- death_notification_is_held(&(memory_->GetWatcher(watcher_index_)->tid)));
+ CHECK(memory_->GetWatcher(watcher_index_)->ownership_tracker.IsHeldBySelf());
// The act of unlocking invalidates the entry. Invalidate it.
- death_notification_release(&(memory_->GetWatcher(watcher_index_)->tid));
+ memory_->GetWatcher(watcher_index_)->ownership_tracker.Release();
// And internally forget the slot.
watcher_index_ = -1;
@@ -699,7 +698,7 @@
// And confirm that nothing is owned by us.
const int num_watchers = memory_->num_watchers();
for (int i = 0; i < num_watchers; ++i) {
- CHECK(!death_notification_is_held(&(memory_->GetWatcher(i)->tid)))
+ CHECK(!memory_->GetWatcher(i)->ownership_tracker.IsHeldBySelf())
<< ": " << i;
}
}
@@ -732,14 +731,15 @@
for (int i = 0; i < num_watchers; ++i) {
// If we see a slot the kernel has marked as dead, everything we do reusing
// it needs to happen-after whatever that process did before dying.
- auto *const futex = &(memory_->GetWatcher(i)->tid.futex);
- const uint32_t tid = __atomic_load_n(futex, __ATOMIC_ACQUIRE);
- if (tid == 0 || (tid & FUTEX_OWNER_DIED)) {
+ auto *const ownership_tracker =
+ &(memory_->GetWatcher(i)->ownership_tracker);
+ if (ownership_tracker->LoadAcquire().IsUnclaimed() ||
+ ownership_tracker->OwnerIsDefinitelyAbsolutelyDead()) {
watcher_index_ = i;
// Relaxed is OK here because we're the only task going to touch it
// between here and the write in death_notification_init below (other
// recovery is blocked by us holding the setup lock).
- __atomic_store_n(futex, 0, __ATOMIC_RELAXED);
+ ownership_tracker->ForceClear();
break;
}
}
@@ -756,7 +756,7 @@
// Grabbing a mutex is a compiler and memory barrier, so nothing before will
// get rearranged afterwords.
- death_notification_init(&(w->tid));
+ w->ownership_tracker.Acquire();
}
LocklessQueueWakeUpper::LocklessQueueWakeUpper(LocklessQueue queue)
@@ -778,25 +778,25 @@
// creating a pidfd is likely not RT.
for (size_t i = 0; i < num_watchers; ++i) {
const Watcher *w = memory_->GetWatcher(i);
- watcher_copy_[i].tid = __atomic_load_n(&(w->tid.futex), __ATOMIC_RELAXED);
+ watcher_copy_[i].ownership_snapshot = w->ownership_tracker.LoadRelaxed();
// Force the load of the TID to come first.
aos_compiler_memory_barrier();
watcher_copy_[i].pid = w->pid.load(std::memory_order_relaxed);
watcher_copy_[i].priority = w->priority.load(std::memory_order_relaxed);
// Use a priority of -1 to mean an invalid entry to make sorting easier.
- if (watcher_copy_[i].tid & FUTEX_OWNER_DIED || watcher_copy_[i].tid == 0) {
+ if (watcher_copy_[i].ownership_snapshot.OwnerIsDead() ||
+ watcher_copy_[i].ownership_snapshot.IsUnclaimed()) {
watcher_copy_[i].priority = -1;
} else {
// Ensure all of this happens after we're done looking at the pid+priority
// in shared memory.
aos_compiler_memory_barrier();
- if (watcher_copy_[i].tid != static_cast<pid_t>(__atomic_load_n(
- &(w->tid.futex), __ATOMIC_RELAXED))) {
+ if (watcher_copy_[i].ownership_snapshot !=
+ w->ownership_tracker.LoadRelaxed()) {
// Confirm that the watcher hasn't been re-used and modified while we
// read it. If it has, mark it invalid again.
watcher_copy_[i].priority = -1;
- watcher_copy_[i].tid = 0;
}
}
}
@@ -835,8 +835,8 @@
// Send the signal. Target just the thread that sent it so that we can
// support multiple watchers in a process (when someone creates multiple
// event loops in different threads).
- rt_tgsigqueueinfo(watcher_copy.pid, watcher_copy.tid, kWakeupSignal,
- &uinfo);
+ rt_tgsigqueueinfo(watcher_copy.pid, watcher_copy.ownership_snapshot.tid(),
+ kWakeupSignal, &uinfo);
++count;
}
@@ -872,8 +872,7 @@
// This doesn't need synchronization because we're the only process doing
// initialization right now, and nobody else will be touching senders which
// we're interested in.
- const uint32_t tid = __atomic_load_n(&(s->tid.futex), __ATOMIC_RELAXED);
- if (tid == 0) {
+ if (s->ownership_tracker.LoadRelaxed().IsUnclaimed()) {
sender_index_ = i;
break;
}
@@ -888,7 +887,7 @@
// Indicate that we are now alive by taking over the slot. If the previous
// owner died, we still want to do this.
- death_notification_init(&(sender->tid));
+ sender->ownership_tracker.Acquire();
const Index scratch_index = sender->scratch_index.RelaxedLoad();
Message *const message = memory_->GetMessage(scratch_index);
@@ -899,7 +898,7 @@
LocklessQueueSender::~LocklessQueueSender() {
if (sender_index_ != -1) {
CHECK(memory_ != nullptr);
- death_notification_release(&(memory_->GetSender(sender_index_)->tid));
+ memory_->GetSender(sender_index_)->ownership_tracker.Release();
}
}
@@ -1183,8 +1182,7 @@
// This doesn't need synchronization because we're the only process doing
// initialization right now, and nobody else will be touching pinners which
// we're interested in.
- const uint32_t tid = __atomic_load_n(&(p->tid.futex), __ATOMIC_RELAXED);
- if (tid == 0) {
+ if (p->ownership_tracker.LoadRelaxed().IsUnclaimed()) {
pinner_index_ = i;
break;
}
@@ -1200,7 +1198,7 @@
// Indicate that we are now alive by taking over the slot. If the previous
// owner died, we still want to do this.
- death_notification_init(&(p->tid));
+ p->ownership_tracker.Acquire();
}
LocklessQueuePinner::~LocklessQueuePinner() {
@@ -1208,7 +1206,7 @@
CHECK(memory_ != nullptr);
memory_->GetPinner(pinner_index_)->pinned.Invalidate();
aos_compiler_memory_barrier();
- death_notification_release(&(memory_->GetPinner(pinner_index_)->tid));
+ memory_->GetPinner(pinner_index_)->ownership_tracker.Release();
}
}
@@ -1622,8 +1620,8 @@
for (size_t i = 0; i < memory->num_senders(); ++i) {
const Sender *s = memory->GetSender(i);
::std::cout << " [" << i << "] -> Sender {" << ::std::endl;
- ::std::cout << " aos_mutex tid = " << PrintMutex(&s->tid)
- << ::std::endl;
+ ::std::cout << " RobustOwnershipTracker ownership_tracker = "
+ << s->ownership_tracker.DebugString() << ::std::endl;
::std::cout << " AtomicIndex scratch_index = "
<< s->scratch_index.Load().DebugString() << ::std::endl;
::std::cout << " AtomicIndex to_replace = "
@@ -1637,8 +1635,8 @@
for (size_t i = 0; i < memory->num_pinners(); ++i) {
const Pinner *p = memory->GetPinner(i);
::std::cout << " [" << i << "] -> Pinner {" << ::std::endl;
- ::std::cout << " aos_mutex tid = " << PrintMutex(&p->tid)
- << ::std::endl;
+ ::std::cout << " RobustOwnershipTracker ownership_tracker = "
+ << p->ownership_tracker.DebugString() << ::std::endl;
::std::cout << " AtomicIndex scratch_index = "
<< p->scratch_index.Load().DebugString() << ::std::endl;
::std::cout << " AtomicIndex pinned = "
@@ -1653,8 +1651,8 @@
for (size_t i = 0; i < memory->num_watchers(); ++i) {
const Watcher *w = memory->GetWatcher(i);
::std::cout << " [" << i << "] -> Watcher {" << ::std::endl;
- ::std::cout << " aos_mutex tid = " << PrintMutex(&w->tid)
- << ::std::endl;
+ ::std::cout << " RobustOwnershipTracker ownership_tracker = "
+ << w->ownership_tracker.DebugString() << ::std::endl;
::std::cout << " pid_t pid = " << w->pid << ::std::endl;
::std::cout << " int priority = " << w->priority << ::std::endl;
::std::cout << " }" << ::std::endl;
diff --git a/aos/ipc_lib/lockless_queue.h b/aos/ipc_lib/lockless_queue.h
index 01f2b7c..14a11b6 100644
--- a/aos/ipc_lib/lockless_queue.h
+++ b/aos/ipc_lib/lockless_queue.h
@@ -14,6 +14,7 @@
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/data_alignment.h"
#include "aos/ipc_lib/index.h"
+#include "aos/ipc_lib/robust_ownership_tracker.h"
#include "aos/time/time.h"
#include "aos/uuid.h"
@@ -29,7 +30,7 @@
// Note: this is only modified with the queue_setup_lock lock held, but may
// always be read.
// Any state modification should happen before the lock is acquired.
- aos_mutex tid;
+ RobustOwnershipTracker ownership_tracker;
// PID of the watcher.
std::atomic<pid_t> pid;
@@ -46,7 +47,7 @@
//
// Note: this is only modified with the queue_setup_lock lock held, but may
// always be read.
- aos_mutex tid;
+ RobustOwnershipTracker ownership_tracker;
// Index of the message we will be filling out.
AtomicIndex scratch_index;
@@ -59,7 +60,7 @@
// Structure to hold the state required to pin messages.
struct Pinner {
// The same as Sender::tid. See there for docs.
- aos_mutex tid;
+ RobustOwnershipTracker ownership_tracker;
// Queue index of the message we have pinned, or Invalid if there isn't one.
AtomicQueueIndex pinned;
@@ -200,6 +201,10 @@
const static unsigned int kWakeupSignal = SIGRTMIN + 2;
+// Sets FUTEX_OWNER_DIED if the owner was tid. This fakes what the kernel does
+// with a robust mutex.
+bool PretendThatOwnerIsDeadForTesting(aos_mutex *mutex, pid_t tid);
+
// A convenient wrapper for accessing a lockless queue.
class LocklessQueue {
public:
@@ -268,7 +273,7 @@
// up. This isn't a copy of Watcher since tid is simpler to work with here
// than the futex above.
struct WatcherCopy {
- pid_t tid;
+ ThreadOwnerStatusSnapshot ownership_snapshot;
pid_t pid;
int priority;
};
diff --git a/aos/ipc_lib/lockless_queue_death_test.cc b/aos/ipc_lib/lockless_queue_death_test.cc
index 2a95b31..92ad8a8 100644
--- a/aos/ipc_lib/lockless_queue_death_test.cc
+++ b/aos/ipc_lib/lockless_queue_death_test.cc
@@ -102,12 +102,13 @@
bool print = false;
// TestShmRobustness doesn't handle robust futexes. It is happy to just
- // not crash with them. We know what they are, and what the tid of the
- // holder is. So go pretend to be the kernel and fix it for it.
- PretendOwnerDied(&memory->queue_setup_lock, tid.Get());
+ // not crash with them. We know what the futexes are, and what the tid
+ // of the corresponding holder is. So go pretend to be the kernel and
+ // fix the futex.
+ PretendThatOwnerIsDeadForTesting(&memory->queue_setup_lock, tid.Get());
for (size_t i = 0; i < config.num_senders; ++i) {
- if (PretendOwnerDied(&memory->GetSender(i)->tid, tid.Get())) {
+ if (memory->GetSender(i)->ownership_tracker.IsHeldBy(tid.Get())) {
// Print out before and after results if a sender died. That is the
// more fun case.
print = true;
diff --git a/aos/ipc_lib/lockless_queue_stepping.cc b/aos/ipc_lib/lockless_queue_stepping.cc
index 0ec5214..e819795 100644
--- a/aos/ipc_lib/lockless_queue_stepping.cc
+++ b/aos/ipc_lib/lockless_queue_stepping.cc
@@ -458,14 +458,6 @@
pid_t SharedTid::Get() { return *tid_; }
-bool PretendOwnerDied(aos_mutex *mutex, pid_t tid) {
- if ((mutex->futex & FUTEX_TID_MASK) == tid) {
- mutex->futex = FUTEX_OWNER_DIED;
- return true;
- }
- return false;
-}
-
} // namespace testing
} // namespace ipc_lib
} // namespace aos
diff --git a/aos/ipc_lib/lockless_queue_stepping.h b/aos/ipc_lib/lockless_queue_stepping.h
index b3eb6e3..7aab193 100644
--- a/aos/ipc_lib/lockless_queue_stepping.h
+++ b/aos/ipc_lib/lockless_queue_stepping.h
@@ -140,10 +140,6 @@
pid_t *tid_;
};
-// Sets FUTEX_OWNER_DIED if the owner was tid. This fakes what the kernel does
-// with a robust mutex.
-bool PretendOwnerDied(aos_mutex *mutex, pid_t tid);
-
#endif
} // namespace testing
diff --git a/aos/ipc_lib/lockless_queue_test.cc b/aos/ipc_lib/lockless_queue_test.cc
index 58e093f..14701e2 100644
--- a/aos/ipc_lib/lockless_queue_test.cc
+++ b/aos/ipc_lib/lockless_queue_test.cc
@@ -538,7 +538,7 @@
::aos::ipc_lib::LocklessQueueMemory *const memory =
reinterpret_cast<::aos::ipc_lib::LocklessQueueMemory *>(raw_memory);
LocklessQueue queue(memory, memory, config);
- PretendOwnerDied(&memory->queue_setup_lock, tid.Get());
+ PretendThatOwnerIsDeadForTesting(&memory->queue_setup_lock, tid.Get());
if (VLOG_IS_ON(1)) {
PrintLocklessQueueMemory(memory);
diff --git a/aos/ipc_lib/memory_mapped_queue.cc b/aos/ipc_lib/memory_mapped_queue.cc
index 79f27d9..594febb 100644
--- a/aos/ipc_lib/memory_mapped_queue.cc
+++ b/aos/ipc_lib/memory_mapped_queue.cc
@@ -17,7 +17,7 @@
std::string ShmPath(std::string_view shm_base, const Channel *channel) {
CHECK(channel->has_type());
- return ShmFolder(shm_base, channel) + channel->type()->str() + ".v5";
+ return ShmFolder(shm_base, channel) + channel->type()->str() + ".v6";
}
void PageFaultDataWrite(char *data, size_t size) {
diff --git a/aos/ipc_lib/robust_ownership_tracker.cc b/aos/ipc_lib/robust_ownership_tracker.cc
new file mode 100644
index 0000000..f106113
--- /dev/null
+++ b/aos/ipc_lib/robust_ownership_tracker.cc
@@ -0,0 +1,25 @@
+#include "aos/ipc_lib/robust_ownership_tracker.h"
+
+#include "aos/ipc_lib/lockless_queue.h"
+
+namespace aos {
+namespace ipc_lib {
+
+::std::string RobustOwnershipTracker::DebugString() const {
+ ::std::stringstream s;
+ s << "{.tid=aos_mutex(" << ::std::hex << mutex_.futex;
+
+ if (mutex_.futex != 0) {
+ s << ":";
+ if (mutex_.futex & FUTEX_OWNER_DIED) {
+ s << "FUTEX_OWNER_DIED|";
+ }
+ s << "tid=" << (mutex_.futex & FUTEX_TID_MASK);
+ }
+
+ s << "),}";
+ return s.str();
+}
+
+} // namespace ipc_lib
+} // namespace aos
diff --git a/aos/ipc_lib/robust_ownership_tracker.h b/aos/ipc_lib/robust_ownership_tracker.h
new file mode 100644
index 0000000..a8bbca3
--- /dev/null
+++ b/aos/ipc_lib/robust_ownership_tracker.h
@@ -0,0 +1,177 @@
+#ifndef AOS_IPC_LIB_ROBUST_OWNERSHIP_TRACKER_H_
+#define AOS_IPC_LIB_ROBUST_OWNERSHIP_TRACKER_H_
+
+#include <linux/futex.h>
+#include <sys/syscall.h>
+
+#include <string>
+
+#include "aos/ipc_lib/aos_sync.h"
+#include "aos/util/top.h"
+
+namespace aos::ipc_lib {
+namespace testing {
+class RobustOwnershipTrackerTest;
+} // namespace testing
+
+// Results of atomically loading the ownership state via RobustOwnershipTracker
+// below. This allows the state to be compared and queried later.
+class ThreadOwnerStatusSnapshot {
+ public:
+ ThreadOwnerStatusSnapshot() : futex_(0) {}
+ ThreadOwnerStatusSnapshot(aos_futex futex) : futex_(futex) {}
+ ThreadOwnerStatusSnapshot(const ThreadOwnerStatusSnapshot &) = default;
+ ThreadOwnerStatusSnapshot &operator=(const ThreadOwnerStatusSnapshot &) =
+ default;
+ ThreadOwnerStatusSnapshot(ThreadOwnerStatusSnapshot &&) = default;
+ ThreadOwnerStatusSnapshot &operator=(ThreadOwnerStatusSnapshot &&) = default;
+
+ // Returns if the owner died as noticed by the robust futex using Acquire
+ // memory ordering.
+ bool OwnerIsDead() const { return (futex_ & FUTEX_OWNER_DIED) != 0; }
+
+ // Returns true if no one has claimed ownership.
+ bool IsUnclaimed() const { return futex_ == 0; }
+
+ // Returns the thread ID (a.k.a. "tid") of the owning thread. Use this when
+ // trying to access the /proc entry that corresponds to the owning thread for
+ // example. Do not use the futex value directly.
+ pid_t tid() const { return futex_ & FUTEX_TID_MASK; }
+
+ bool operator==(const ThreadOwnerStatusSnapshot &other) const {
+ return other.futex_ == futex_;
+ }
+
+ private:
+ aos_futex futex_;
+};
+
+// This object reliably tracks a thread owning a resource. A single thread may
+// possess multiple resources like senders and receivers. Each resource can have
+// its own instance of this class. These instances are responsible for
+// monitoring the thread that owns them. Each resource can use its instance of
+// this class to reliably check whether the owning thread is no longer alive.
+//
+// All methods other than Load* must be accessed under a mutex.
+class RobustOwnershipTracker {
+ public:
+ static constexpr uint64_t kNoStartTimeTicks =
+ std::numeric_limits<uint64_t>::max();
+
+ static uint64_t ReadStartTimeTicks(pid_t tid) {
+ if (tid == 0) {
+ return kNoStartTimeTicks;
+ }
+ std::optional<aos::util::ProcStat> proc_stat = util::ReadProcStat(tid);
+ if (!proc_stat.has_value()) {
+ return kNoStartTimeTicks;
+ }
+ return proc_stat->start_time_ticks;
+ }
+
+ // Loads the realtime-compatible contents of the ownership tracker with
+ // Acquire memory ordering.
+ ThreadOwnerStatusSnapshot LoadAcquire() const {
+ return ThreadOwnerStatusSnapshot(
+ __atomic_load_n(&(mutex_.futex), __ATOMIC_ACQUIRE));
+ }
+
+ // Loads all the realtime-compatible contents of the ownership tracker with
+ // Relaxed memory order.
+ ThreadOwnerStatusSnapshot LoadRelaxed() const {
+ return ThreadOwnerStatusSnapshot(
+ __atomic_load_n(&(mutex_.futex), __ATOMIC_RELAXED));
+ }
+
+ // Checks both the robust futex and dredges through /proc to see if the thread
+ // is alive. As per the class description, this must only be called under a
+ // mutex. This must not be called in a realtime context and it is slow.
+ bool OwnerIsDefinitelyAbsolutelyDead() const {
+ auto loaded = LoadAcquire();
+ if (loaded.OwnerIsDead()) {
+ return true;
+ }
+ if (loaded.IsUnclaimed()) {
+ return false;
+ }
+ const uint64_t proc_start_time_ticks = ReadStartTimeTicks(loaded.tid());
+ if (proc_start_time_ticks == kNoStartTimeTicks) {
+ LOG(ERROR) << "Detected that PID " << loaded.tid() << " died.";
+ return true;
+ }
+
+ if (proc_start_time_ticks != start_time_ticks_) {
+ LOG(ERROR) << "Detected that PID " << loaded.tid()
+ << " died from a starttime missmatch.";
+ return true;
+ }
+ return false;
+ }
+
+ // Clears all ownership state.
+ //
+ // This should only really be called if you are 100% certain that the owner is
+ // dead. Use `LoadAquire().OwnerIsDead()` to determine this.
+ void ForceClear() {
+ // Must be opposite order of Acquire.
+ // We only deal with the futex here because we don't want to change anything
+ // about the linked list. We just want to release ownership here. We still
+ // want the kernel to know about this element via the linked list the next
+ // time someone takes ownership.
+ __atomic_store_n(&(mutex_.futex), 0, __ATOMIC_RELEASE);
+ start_time_ticks_ = kNoStartTimeTicks;
+ }
+
+ // Returns true if this thread holds ownership.
+ bool IsHeldBySelf() { return death_notification_is_held(&mutex_); }
+
+ // Returns true if the mutex is held by the provided tid. This is primarily
+ // intended for testing. There should be no need to call this in production
+ // code.
+ bool IsHeldBy(pid_t tid) { return LoadRelaxed().tid() == tid; }
+
+ // Acquires ownership. Other threads will know that this thread holds the
+ // ownership or be notified if this thread dies.
+ void Acquire() {
+ pid_t tid = syscall(SYS_gettid);
+ assert(tid > 0);
+ const uint64_t proc_start_time_ticks = ReadStartTimeTicks(tid);
+ CHECK_NE(proc_start_time_ticks, kNoStartTimeTicks);
+
+ start_time_ticks_ = proc_start_time_ticks;
+ death_notification_init(&mutex_);
+ }
+
+ // Releases ownership.
+ //
+ // This should only be called from the owning thread.
+ void Release() {
+ // Must be opposite order of Acquire.
+ death_notification_release(&mutex_);
+ start_time_ticks_ = kNoStartTimeTicks;
+ }
+
+ // Returns a string representing this object.
+ std::string DebugString() const;
+
+ private:
+ friend class testing::RobustOwnershipTrackerTest;
+
+ // Robust futex to track ownership the normal way. The futex is inside the
+ // mutex here. We use the wrapper mutex because the death_notification_*
+ // functions operate on that instead of the futex directly.
+ //
+ // We use a futex here because:
+ // - futexes are fast.
+ // - The kernel can atomically clean up a dead thread and mark the futex
+ // appropriately.
+ // - Owners can clean up after dead threads.
+ aos_mutex mutex_;
+
+ // Thread's start time ticks.
+ std::atomic<uint64_t> start_time_ticks_;
+};
+
+} // namespace aos::ipc_lib
+
+#endif // AOS_IPC_LIB_ROBUST_OWNERSHIP_TRACKER_H_
diff --git a/aos/ipc_lib/robust_ownership_tracker_test.cc b/aos/ipc_lib/robust_ownership_tracker_test.cc
new file mode 100644
index 0000000..b204886
--- /dev/null
+++ b/aos/ipc_lib/robust_ownership_tracker_test.cc
@@ -0,0 +1,155 @@
+#include "aos/ipc_lib/robust_ownership_tracker.h"
+
+#include <sys/mman.h>
+#include <sys/wait.h>
+
+#include "gtest/gtest.h"
+
+namespace aos::ipc_lib::testing {
+
+// Capture RobustOwnershipTracker in shared memory so it is shared across a
+// fork.
+class SharedRobustOwnershipTracker {
+ public:
+ SharedRobustOwnershipTracker() {
+ tracker_ = static_cast<RobustOwnershipTracker *>(
+ mmap(nullptr, sizeof(RobustOwnershipTracker), PROT_READ | PROT_WRITE,
+ MAP_SHARED | MAP_ANONYMOUS, -1, 0));
+ PCHECK(MAP_FAILED != tracker_);
+ };
+ ~SharedRobustOwnershipTracker() {
+ PCHECK(munmap(tracker_, sizeof(RobustOwnershipTracker)) != -1);
+ }
+
+ // Captures the tid.
+ RobustOwnershipTracker &tracker() const { return *tracker_; }
+
+ private:
+ RobustOwnershipTracker *tracker_;
+};
+
+class RobustOwnershipTrackerTest : public ::testing::Test {
+ public:
+ // Runs a function in a child process, and then exits afterwards. Waits for
+ // the child to finish before resuming.
+ template <typename T>
+ void RunInChildAndBlockUntilComplete(T fn) {
+ pid_t pid = fork();
+ if (pid == 0) {
+ fn();
+ LOG(INFO) << "Child exiting normally.";
+ exit(0);
+ return;
+ }
+
+ LOG(INFO) << "Child has pid " << pid;
+
+ while (true) {
+ LOG(INFO) << "Waiting for child.";
+ int status;
+ const pid_t waited_on = waitpid(pid, &status, 0);
+ // Check for failure.
+ if (waited_on == -1) {
+ if (errno == EINTR) continue;
+ PLOG(FATAL) << ": waitpid(" << pid << ", " << &status << ", 0) failed";
+ }
+ CHECK_EQ(waited_on, pid)
+ << ": waitpid() got child " << waited_on << " instead of " << pid;
+ CHECK(WIFEXITED(status));
+ LOG(INFO) << "Status " << WEXITSTATUS(status);
+ CHECK(WEXITSTATUS(status) == 0);
+ return;
+ }
+ }
+
+ // Returns the robust mutex.
+ aos_mutex &GetMutex(RobustOwnershipTracker &tracker) {
+ return tracker.mutex_;
+ }
+
+ // Returns the current start time in ticks.
+ uint64_t GetStartTimeTicks(RobustOwnershipTracker &tracker) {
+ return tracker.start_time_ticks_.load();
+ }
+
+ // Sets the current start time in ticks.
+ void SetStartTimeTicks(RobustOwnershipTracker &tracker, uint64_t start_time) {
+ tracker.start_time_ticks_ = start_time;
+ }
+};
+
+// Tests that acquiring the futex doesn't erroneously report the owner (i.e.
+// "us") as dead.
+TEST_F(RobustOwnershipTrackerTest, AcquireWorks) {
+ SharedRobustOwnershipTracker shared_tracker;
+
+ EXPECT_FALSE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+
+ // Run acquire in the this process, and expect it should not be dead until
+ // after the test finishes.
+ shared_tracker.tracker().Acquire();
+
+ // We have ownership. Since we are alive, the owner should not be marked as
+ // dead. We can use relaxed ordering since we are the only ones touching the
+ // data here.
+ EXPECT_FALSE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_FALSE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+}
+
+// Tests that child death without unlocking results in the futex being marked as
+// dead, and the owner being very dead.
+TEST_F(RobustOwnershipTrackerTest, FutexRecovers) {
+ SharedRobustOwnershipTracker shared_tracker;
+
+ RunInChildAndBlockUntilComplete(
+ [&]() { shared_tracker.tracker().Acquire(); });
+
+ // Since the child that took ownership died, we expect that death to be
+ // reported.
+ EXPECT_TRUE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_TRUE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+}
+
+// Tests that a PID which doesn't exist results in the process being noticed as
+// dead when we inspect /proc.
+TEST_F(RobustOwnershipTrackerTest, NoMatchingPID) {
+ SharedRobustOwnershipTracker shared_tracker;
+
+ shared_tracker.tracker().Acquire();
+ EXPECT_FALSE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_FALSE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wignored-attributes"
+ GetMutex(shared_tracker.tracker()).futex =
+ std::numeric_limits<aos_futex>::max() & FUTEX_TID_MASK;
+#pragma GCC diagnostic pop
+
+ // Since we're only pretending that the owner died (by changing the TID in the
+ // futex), we only notice that the owner is dead when spending the time
+ // walking through /proc.
+ EXPECT_FALSE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_TRUE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+}
+
+// Tests that a mismatched start time results in the process being marked as
+// dead.
+TEST_F(RobustOwnershipTrackerTest, NoMatchingStartTime) {
+ SharedRobustOwnershipTracker shared_tracker;
+
+ shared_tracker.tracker().Acquire();
+ EXPECT_FALSE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_FALSE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+
+ EXPECT_NE(GetStartTimeTicks(shared_tracker.tracker()), 0);
+ EXPECT_NE(GetStartTimeTicks(shared_tracker.tracker()),
+ RobustOwnershipTracker::kNoStartTimeTicks);
+ SetStartTimeTicks(shared_tracker.tracker(), 1);
+
+ // Since we're only pretending that the owner died (by changing the tracked
+ // start time ticks in the tracker), we only notice that the owner is dead
+ // when spending the time walking through /proc.
+ EXPECT_FALSE(shared_tracker.tracker().LoadRelaxed().OwnerIsDead());
+ EXPECT_TRUE(shared_tracker.tracker().OwnerIsDefinitelyAbsolutelyDead());
+}
+
+} // namespace aos::ipc_lib::testing
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index d2b88f0..a145364 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -41,7 +41,11 @@
printf("Back to string via TypeTable: %s\n", back_typetable.c_str());
printf("Back to string via reflection: %s\n", back_reflection.c_str());
- return back_typetable == out && back_reflection == out;
+ const bool as_expected = back_typetable == out && back_reflection == out;
+ if (!as_expected) {
+ printf("But expected: %s\n", out.c_str());
+ }
+ return as_expected;
}
};
@@ -230,9 +234,33 @@
EXPECT_TRUE(JsonAndBack("{ }"));
}
-// Tests that comments get stripped.
-TEST_F(JsonToFlatbufferTest, Comments) {
- EXPECT_TRUE(JsonAndBack("{ /* foo */ \"vector_foo_double\": [ 9, 7, 1 ] }",
+// Tests that C style comments get stripped.
+TEST_F(JsonToFlatbufferTest, CStyleComments) {
+ EXPECT_TRUE(JsonAndBack(R"({
+ /* foo */
+ "vector_foo_double": [ 9, 7, 1 ] /* foo */
+} /* foo */)",
+ "{ \"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
+}
+
+// Tests that C++ style comments get stripped.
+TEST_F(JsonToFlatbufferTest, CppStyleComments) {
+ EXPECT_TRUE(JsonAndBack(R"({
+ // foo
+ "vector_foo_double": [ 9, 7, 1 ] // foo
+} // foo)",
+ "{ \"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
+}
+
+// Tests that mixed style comments get stripped.
+TEST_F(JsonToFlatbufferTest, MixedStyleComments) {
+ // Weird comments do not throw us off.
+ EXPECT_TRUE(JsonAndBack(R"({
+ // foo /* foo */
+ "vector_foo_double": [ 9, 7, 1 ] /* // foo */
+}
+// foo
+/* foo */)",
"{ \"vector_foo_double\": [ 9.0, 7.0, 1.0 ] }"));
}
diff --git a/aos/json_tokenizer.cc b/aos/json_tokenizer.cc
index b3c6620..eab7fcc 100644
--- a/aos/json_tokenizer.cc
+++ b/aos/json_tokenizer.cc
@@ -23,6 +23,19 @@
}
ConsumeChar();
}
+ } else if (Consume("//")) {
+ // C++ style comment. Keep consuming chars until newline, or until the
+ // end of the file if this is the last line (no newline at end of file).
+ while (true) {
+ ConsumeChar();
+ if (AtEnd()) {
+ return;
+ }
+ if (Char() == '\n') {
+ ++linenumber_;
+ break;
+ }
+ }
} else {
// There is no fail. Once we are out of whitespace (including 0 of it),
// declare success.
diff --git a/aos/network/BUILD b/aos/network/BUILD
index d1a79d0..e8dd75f 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -194,6 +194,7 @@
tags = [
# Fakeroot is required to enable "net.sctp.auth_enable".
"requires-fakeroot",
+ "no-remote-exec",
],
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 85ee12c..55b6b02 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -459,7 +459,8 @@
return result;
}
-Eigen::VectorXd NewtonSolver::Newton(const Problem::Derivatives &derivatives,
+Eigen::VectorXd NewtonSolver::Newton(const Eigen::Ref<const Eigen::VectorXd> y,
+ const Problem::Derivatives &derivatives,
size_t iteration) {
// https://www.cs.purdue.edu/homes/jhonorio/16spring-cs52000-equality.pdf
// https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf chapter 10 is good
@@ -479,6 +480,10 @@
Eigen::VectorXd b = Eigen::VectorXd::Zero(a.rows());
b.block(0, 0, derivatives.gradient.rows(), 1) = -derivatives.gradient;
if (derivatives.Axmb.rows()) {
+ const Eigen::Ref<const Eigen::VectorXd> v =
+ y.block(derivatives.hessian.rows(), 0, derivatives.A.rows(), 1);
+ b.block(0, 0, derivatives.gradient.rows(), 1) -=
+ derivatives.A.transpose() * v;
b.block(derivatives.gradient.rows(), 0, derivatives.Axmb.rows(), 1) =
-derivatives.Axmb;
}
@@ -508,6 +513,34 @@
return step;
}
+double NewtonSolver::RSquaredUnconstrained(
+ const Problem::Derivatives &derivatives,
+ Eigen::Ref<const Eigen::VectorXd> y, Eigen::Ref<const Eigen::VectorXd> step,
+ double t) {
+ // See https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf section 10.3.1
+ //
+ // Note: all the prints are transposed so they fit on one line instead of
+ // having a ton of massive column vectors being printed all the time.
+ SOLVE_VLOG(my_solve_number_, 2)
+ << " r_dual: " << std::setprecision(12) << std::fixed
+ << std::setfill(' ')
+ << derivatives.gradient.transpose().format(kHeavyFormat) << " + "
+ << ((y.block(derivatives.gradient.rows(), 0, 1, 1) +
+ t * step.block(derivatives.gradient.rows(), 0, 1, 1))
+ .transpose() *
+ derivatives.A)
+ .format(kHeavyFormat);
+ SOLVE_VLOG(my_solve_number_, 2)
+ << " r_primal: " << std::setprecision(12) << std::fixed
+ << std::setfill(' ') << derivatives.Axmb.transpose().format(kHeavyFormat);
+ return (derivatives.gradient +
+ derivatives.A.transpose() *
+ (y.block(derivatives.gradient.rows(), 0, 1, 1) +
+ t * step.block(derivatives.gradient.rows(), 0, 1, 1)))
+ .squaredNorm() +
+ derivatives.Axmb.squaredNorm();
+}
+
std::tuple<Eigen::VectorXd, size_t, size_t> NewtonSolver::SolveNewton(
Problem *problem, const size_t max_iterations) {
SOLVE_VLOG(my_solve_number_, 2)
@@ -515,25 +548,35 @@
problem->Prepare(my_solve_number_);
- Eigen::VectorXd data = Eigen::VectorXd::Zero(problem->states());
+ Eigen::VectorXd y = Eigen::VectorXd::Zero(problem->states() + 1);
size_t iteration = 0;
size_t solution_node;
+
+ // The derivatives and squared norm of r(y).
+ Problem::Derivatives derivatives = problem->ComputeDerivatives(
+ my_solve_number_, y, false, false, absl::Span<const size_t>());
+ double r_orig_squared = RSquaredUnconstrained(derivatives, y, y, 0.0);
while (true) {
- const Problem::Derivatives derivatives = problem->ComputeDerivatives(
- my_solve_number_, data, false, false, absl::Span<const size_t>());
- const Eigen::VectorXd step = Newton(derivatives, iteration);
+ SOLVE_VLOG(my_solve_number_, 1) << "Starting iteration " << iteration;
+
+ const Eigen::VectorXd step = Newton(y, derivatives, iteration);
solution_node = derivatives.solution_node;
+ SOLVE_VLOG(my_solve_number_, 1)
+ << " y(" << iteration << ") is " << y.transpose().format(kHeavyFormat);
+ PrintDerivatives(derivatives, y, "", 3);
SOLVE_VLOG(my_solve_number_, 2)
- << "Step " << iteration << " -> " << std::setprecision(12) << std::fixed
- << std::setfill(' ') << step.transpose().format(kHeavyFormat);
+ << " initial step(" << iteration << ") -> " << std::setprecision(12)
+ << std::fixed << std::setfill(' ')
+ << step.transpose().format(kHeavyFormat);
// We now have a search direction. Line search and go.
+ double t = 1.0;
- // We got there if the max step is small (this is strongly correlated to the
- // gradient since the Hessian is constant), and our solution node's time is
- // also close.
+ // We got there if the max step is small (this is strongly correlated to
+ // the gradient since the Hessian is constant), and our solution node's
+ // time is also close.
// Grad is < epsilon.
// Dual is < epsilon too.
@@ -543,7 +586,44 @@
break;
}
- data += step.block(0, 0, problem->states(), 1);
+ SOLVE_VLOG(my_solve_number_, 2)
+ << " Not close enough, "
+ << step.block(0, 0, problem->states(), 1).lpNorm<Eigen::Infinity>()
+ << " >= 1e-4 || " << derivatives.Axmb.squaredNorm() << " >= 1e-8";
+
+ // Now, do line search.
+ while (true) {
+ Problem::Derivatives new_derivatives =
+ problem->ComputeDerivatives(my_solve_number_, y + t * step, false,
+ false, absl::Span<const size_t>());
+ const double r_squared =
+ RSquaredUnconstrained(new_derivatives, y, step, t);
+ SOLVE_VLOG(my_solve_number_, 2)
+ << std::setprecision(12) << std::setfill(' ') << " |r| < |r+1| "
+ << r_orig_squared << " < " << r_squared << " for step size " << t;
+ // See Boyd, section 10.3.2, algorithm 10.2
+ if (r_squared <=
+ std::pow(1.0 - kUnconstrainedAlpha * t, 2.0) * r_orig_squared ||
+ t < kLineSearchStopThreshold) {
+ // This is modified because we have seen cases where the way we handle
+ // equality constraints sometimes ends up increasing the cost slightly.
+ // We are better off taking a small step and trying again than shrinking
+ // our step forever.
+ //
+ // Save the derivatives and norm computed for the next iteration to save
+ // CPU.
+ derivatives = std::move(new_derivatives);
+ r_orig_squared = r_squared;
+ SOLVE_VLOG(my_solve_number_, 3)
+ << " Line search terminated with a step of " << t;
+ break;
+ } else {
+ CHECK_NE(t, 0.0) << ": Failed on solve " << my_solve_number_;
+ }
+ t *= kBeta;
+ }
+
+ y += t * step;
++iteration;
@@ -555,8 +635,8 @@
// true for the first solution. Because we control the solver, as we
// determine that the double is getting too big, we can move that
// information to the int64 base clock. Threshold this to not be *too* big
- // since it makes it hard to debug as the data keeps jumping around.
- problem->Update(my_solve_number_, data);
+ // since it makes it hard to debug as y keeps jumping around.
+ problem->Update(my_solve_number_, y);
// And finally, don't let us iterate forever. If it isn't converging,
// report back.
@@ -571,7 +651,8 @@
SOLVE_VLOG(my_solve_number_, 1)
<< "Took " << iteration << " iterations to solve.";
- return std::make_tuple(std::move(data), solution_node, iteration);
+ return std::make_tuple(y.block(0, 0, problem->states(), 1), solution_node,
+ iteration);
}
Problem::Derivatives NewtonSolver::AddConstraintSlackVariable(
@@ -693,6 +774,10 @@
const Eigen::Ref<const Eigen::VectorXd> lambda =
y.block(x.rows(), 0, derivatives.f.rows(), 1);
+ CHECK_LT(0, lambda.rows())
+ << ": You are calling the unconstrained Newton solver without inequality "
+ "constraints. This is not supported.";
+
const Eigen::Ref<const Eigen::VectorXd> v =
y.block(x.rows() + lambda.rows(), 0, derivatives.A.rows(), 1);
@@ -770,25 +855,32 @@
const Eigen::Ref<const Eigen::VectorXd> v =
y.block(x.rows() + lambda.rows(), 0, derivatives.A.rows(), 1);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "x: " << x.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "x: " << x.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "lambda: " << lambda.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "lambda: " << lambda.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "v: " << v.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "v: " << v.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix
+ << std::setprecision(12) << " " << prefix
<< "hessian: " << derivatives.hessian.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix
+ << std::setprecision(12) << " " << prefix
<< "gradient: " << derivatives.gradient.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "A: " << derivatives.A.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "A: " << derivatives.A.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "Ax-b: " << derivatives.Axmb.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "Ax-b: " << derivatives.Axmb.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "f: " << derivatives.f.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "f: " << derivatives.f.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "df: " << derivatives.df.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "df: " << derivatives.df.format(heavy);
}
}
@@ -1022,6 +1114,13 @@
PrintDerivatives(derivatives, y, "", 1);
+ if (derivatives.f.rows() == 0) {
+ LOG(ERROR) << "No inequality constraints provided in constrained solver. "
+ "This suggests an inconsistency in the solver code, please "
+ "investigate.";
+ return std::nullopt;
+ }
+
// Figure out our descent direction.
Eigen::VectorXd dy;
Eigen::VectorXd rt_orig;
@@ -1275,7 +1374,8 @@
}
SOLVE_VLOG(solve_number, 2)
- << " live " << points_[j].time << " vs solution "
+ << std::setprecision(12) << std::fixed << " live "
+ << points_[j].time << " vs solution "
<< base_clock_[j].time +
std::chrono::nanoseconds(static_cast<int64_t>(
std::round(data(NodeToFullSolutionIndex(j)))))
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 959fe85..828c489 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -84,6 +84,10 @@
// Ratio to let the cost increase when line searching. A small increase is
// fine since we aren't perfectly convex.
static constexpr double kAlpha = -0.15;
+ // Ratio to require the cost to decrease when line searching.
+ static constexpr double kUnconstrainedAlpha = 0.5;
+ // Unconstrained min line search distance to guarantee forward progress.
+ static constexpr double kLineSearchStopThreshold = 0.4;
// Line search step parameter.
static constexpr double kBeta = 0.5;
static constexpr double kMu = 2.0;
@@ -138,6 +142,14 @@
Eigen::VectorXd Rt(const Problem::Derivatives &derivatives,
Eigen::Ref<const Eigen::VectorXd> y, double t);
+ // Returns the squared norm of r for the unconstrained problem.
+ // (10.3.1 in Convex Optimization,
+ // https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf)
+ double RSquaredUnconstrained(const Problem::Derivatives &derivatives,
+ Eigen::Ref<const Eigen::VectorXd> y,
+ Eigen::Ref<const Eigen::VectorXd> step,
+ double t);
+
// Returns the constrained newtons step, t_inverse, and Rt.
std::tuple<Eigen::VectorXd, double, Eigen::VectorXd> ConstrainedNewton(
const Eigen::Ref<const Eigen::VectorXd> y,
@@ -152,7 +164,8 @@
// used for the equality constraint. The last term is the scalar on the
// equality constraint. This needs to be removed from the solution to get the
// actual newton step.
- Eigen::VectorXd Newton(const Problem::Derivatives &derivatives,
+ Eigen::VectorXd Newton(const Eigen::Ref<const Eigen::VectorXd> y,
+ const Problem::Derivatives &derivatives,
size_t iteration);
// The solve number for this instance of the problem.
diff --git a/aos/network/sctp_lib.cc b/aos/network/sctp_lib.cc
index c3c6e23..0922d6b 100644
--- a/aos/network/sctp_lib.cc
+++ b/aos/network/sctp_lib.cc
@@ -715,8 +715,11 @@
<< "SCTP Authentication key requested, but authentication isn't "
"enabled... Use `sysctl -w net.sctp.auth_enable=1` to enable";
// Set up the key with id `1`.
- std::unique_ptr<sctp_authkey> authkey(
- (sctp_authkey *)malloc(sizeof(sctp_authkey) + auth_key.size()));
+ // NOTE: `sctp_authkey` is a variable-sized struct which is why it needs
+ // to be heap allocated. Regardless, this object doesn't have to persist past
+ // the `setsockopt` call below.
+ std::unique_ptr<sctp_authkey> authkey(static_cast<sctp_authkey *>(
+ ::operator new(sizeof(sctp_authkey) + auth_key.size())));
authkey->sca_keynumber = 1;
authkey->sca_keylength = auth_key.size();
diff --git a/aos/network/sctp_test.cc b/aos/network/sctp_test.cc
index 8e332e4..bce5c19 100644
--- a/aos/network/sctp_test.cc
+++ b/aos/network/sctp_test.cc
@@ -11,6 +11,7 @@
#include "aos/network/sctp_client.h"
#include "aos/network/sctp_lib.h"
#include "aos/network/sctp_server.h"
+#include "sctp_lib.h"
DECLARE_bool(disable_ipv6);
@@ -28,8 +29,10 @@
namespace {
void EnableSctpAuthIfAvailable() {
#if HAS_SCTP_AUTH
- CHECK(system("/usr/sbin/sysctl net.sctp.auth_enable=1 || /sbin/sysctl "
- "net.sctp.auth_enable=1") == 0)
+ // Open an SCTP socket to bring the kernel SCTP module
+ SctpServer server(1, "localhost");
+ CHECK(system("/usr/sbin/sysctl net.sctp.auth_enable=1 || "
+ "/sbin/sysctl net.sctp.auth_enable=1") == 0)
<< "Couldn't enable sctp authentication.";
#endif
}
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 4a29dfc..9736679 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -531,6 +531,13 @@
if (!use_other) {
return std::make_pair(pointer, std::make_pair(t0, t1));
}
+
+ // The invariant of pointer is that other_points is bounded by t0, t1. Confirm
+ // it before we return things depending on it since it is easy.
+ CHECK_GT(std::get<0>(pointer.other_points_[0].second), std::get<0>(t0));
+ CHECK_LT(std::get<0>(
+ pointer.other_points_[pointer.other_points_.size() - 1].second),
+ std::get<0>(t1));
// We have 2 timestamps bookending everything, and a list of points in the
// middle.
//
@@ -660,11 +667,14 @@
const size_t index = std::distance(timestamps_.begin(), it);
+ // Now, update the pointer cache.
pointer.index_ = index - 1;
t0 = timestamp(index - 1);
pointer.t0_ = t0;
t1 = timestamp(index);
pointer.t1_ = t1;
+ // And clear out the point cache since the points changed.
+ pointer.other_points_.clear();
if (other != nullptr && !other->timestamps_empty()) {
// Ok, we now need to find all points within our range in the matched
@@ -688,8 +698,6 @@
if (std::get<0>(*other_t0_it) + std::get<1>(*other_t0_it) <
std::get<0>(pointer.t1_)) {
- pointer.other_points_.clear();
-
// Now, we've got a range. [other_t0_it, other_t1_it).
for (auto other_it = other_t0_it; other_it != other_t1_it; ++other_it) {
const std::tuple<monotonic_clock::time_point,
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 8b597bd..b1aff86 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -306,7 +306,12 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t1_ =
std::make_tuple(monotonic_clock::min_time, std::chrono::nanoseconds(0));
- // List of points and their associated times going the other way.
+ // List of points and their associated times going the other way. This lets
+ // us handle when there is a large gap in timestamps, but time drifts in
+ // such a way to create an impossible situation when the points are
+ // connected by straight lines.
+ //
+ // These need to be between t0 and t1.
std::vector<std::pair<size_t, std::tuple<monotonic_clock::time_point,
std::chrono::nanoseconds>>>
other_points_;
@@ -869,6 +874,14 @@
return result;
}
+ // Interpolates between two points for time ta using the provided pointer, t0,
+ // and t1. If use_other is true, then we use other_points_ inside pointer to
+ // also interpolate with. This lets us handle cases where we have
+ // observations only going one way, but the filter lines cross because time
+ // drifted.
+ //
+ // Returns the 2 points which define the line we should interpolate along, and
+ // an updated pointer caching what points were actually used.
static std::pair<
Pointer,
std::pair<
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 732cfe3..1873166 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -93,6 +93,24 @@
],
)
+# Similar to subprocess_test, but here are all the tests that are not flaky.
+cc_test(
+ name = "subprocess_reliable_test",
+ srcs = ["subprocess_reliable_test.cc"],
+ data = [
+ "//aos/events:pingpong_config",
+ ],
+ # The roborio compiler doesn't support <filesystem>.
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":subprocess",
+ "//aos/events:shm_event_loop",
+ "//aos/testing:googletest",
+ "//aos/testing:path",
+ "//aos/testing:tmpdir",
+ ],
+)
+
cc_test(
name = "starter_test",
srcs = ["starter_test.cc"],
diff --git a/aos/starter/subprocess.cc b/aos/starter/subprocess.cc
index 7b5fce6..4d21c61 100644
--- a/aos/starter/subprocess.cc
+++ b/aos/starter/subprocess.cc
@@ -12,6 +12,25 @@
namespace aos::starter {
+// Blocks all signals while an instance of this class is in scope.
+class ScopedCompleteSignalBlocker {
+ public:
+ ScopedCompleteSignalBlocker() {
+ sigset_t mask;
+ sigfillset(&mask);
+ // Remember the current mask.
+ PCHECK(sigprocmask(SIG_SETMASK, &mask, &old_mask_) == 0);
+ }
+
+ ~ScopedCompleteSignalBlocker() {
+ // Restore the remembered mask.
+ PCHECK(sigprocmask(SIG_SETMASK, &old_mask_, nullptr) == 0);
+ }
+
+ private:
+ sigset_t old_mask_;
+};
+
// RAII class to become root and restore back to the original user and group
// afterwards.
class Sudo {
@@ -141,11 +160,13 @@
restart_timer_(event_loop_->AddTimer([this] { DoStart(); })),
stop_timer_(event_loop_->AddTimer([this] {
if (kill(pid_, SIGKILL) == 0) {
- LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
+ LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Failed to stop, sending SIGKILL to '" << name_
<< "' pid: " << pid_;
} else {
- PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
+ PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Failed to send SIGKILL to '" << name_ << "' pid: " << pid_;
stop_timer_->Schedule(event_loop_->monotonic_now() +
std::chrono::seconds(1));
@@ -209,34 +230,56 @@
pipe_timer_->Schedule(event_loop_->monotonic_now(),
std::chrono::milliseconds(100));
- const pid_t pid = fork();
+ {
+ // Block all signals during the fork() call. Together with the default
+ // signal handler restoration below, This prevents signal handlers from
+ // getting called in the child and accidentally affecting the parent. In
+ // particular, the exit handler for shm_event_loop could be called here if
+ // we don't exec() quickly enough.
+ ScopedCompleteSignalBlocker signal_blocker;
- if (pid != 0) {
- if (pid == -1) {
- PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
- << "Failed to fork '" << name_ << "'";
- stop_reason_ = aos::starter::LastStopReason::FORK_ERR;
- status_ = aos::starter::State::STOPPED;
- } else {
- pid_ = pid;
- id_ = next_id_++;
- start_time_ = event_loop_->monotonic_now();
- status_ = aos::starter::State::STARTING;
- latest_timing_report_version_.reset();
- LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo)
- << "Starting '" << name_ << "' pid " << pid_;
+ const pid_t pid = fork();
- // Set up timer which moves application to RUNNING state if it is still
- // alive in 1 second.
- start_timer_->Schedule(event_loop_->monotonic_now() +
- std::chrono::seconds(1));
- // Since we are the parent process, clear our write-side of all the pipes.
- status_pipes_.write.reset();
- stdout_pipes_.write.reset();
- stderr_pipes_.write.reset();
+ if (pid != 0) {
+ if (pid == -1) {
+ PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
+ << "Failed to fork '" << name_ << "'";
+ stop_reason_ = aos::starter::LastStopReason::FORK_ERR;
+ status_ = aos::starter::State::STOPPED;
+ } else {
+ pid_ = pid;
+ id_ = next_id_++;
+ start_time_ = event_loop_->monotonic_now();
+ status_ = aos::starter::State::STARTING;
+ latest_timing_report_version_.reset();
+ LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo)
+ << "Starting '" << name_ << "' pid " << pid_;
+
+ // Set up timer which moves application to RUNNING state if it is still
+ // alive in 1 second.
+ start_timer_->Schedule(event_loop_->monotonic_now() +
+ std::chrono::seconds(1));
+ // Since we are the parent process, clear our write-side of all the
+ // pipes.
+ status_pipes_.write.reset();
+ stdout_pipes_.write.reset();
+ stderr_pipes_.write.reset();
+ }
+ OnChange();
+ return;
}
- OnChange();
- return;
+
+ // Clear any signal handlers so that they don't accidentally interfere with
+ // the parent process. Is there a better way to iterate over all the
+ // signals? Right now we're just dealing with the most common ones.
+ for (int signal : {SIGINT, SIGHUP, SIGTERM}) {
+ struct sigaction action;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = 0;
+ action.sa_handler = SIG_DFL;
+ PCHECK(sigaction(signal, &action, nullptr) == 0);
+ }
}
if (memory_cgroup_) {
@@ -323,7 +366,8 @@
// If we got here, something went wrong
status_pipes_.write->Write(
static_cast<uint32_t>(aos::starter::LastStopReason::EXECV_ERR));
- PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
+ PLOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Could not execute " << name_ << " (" << path_ << ')';
_exit(EXIT_FAILURE);
@@ -371,12 +415,18 @@
switch (status_) {
case aos::starter::State::STARTING:
case aos::starter::State::RUNNING: {
- LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo)
+ LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Stopping '" << name_ << "' pid: " << pid_ << " with signal "
<< SIGINT;
status_ = aos::starter::State::STOPPING;
- kill(pid_, SIGINT);
+ if (kill(pid_, SIGINT) != 0) {
+ PLOG_IF(INFO, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
+ << "Failed to send signal " << SIGINT << " to '" << name_
+ << "' pid: " << pid_;
+ }
// Watchdog timer to SIGKILL application if it is still running 1 second
// after SIGINT
@@ -591,7 +641,8 @@
<< "Application '" << name_ << "' pid " << pid_
<< " exited with status " << exit_code_.value();
} else {
- LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
+ LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Failed to start '" << name_ << "' on pid " << pid_
<< " : Exited with status " << exit_code_.value();
}
@@ -609,7 +660,8 @@
<< "Application '" << name_ << "' pid " << pid_
<< " exited with status " << exit_code_.value();
} else {
- if (quiet_flag_ == QuietLogging::kNo) {
+ if (quiet_flag_ == QuietLogging::kNo ||
+ quiet_flag_ == QuietLogging::kNotForDebugging) {
std::string version_string =
latest_timing_report_version_.has_value()
? absl::StrCat("'", latest_timing_report_version_.value(),
diff --git a/aos/starter/subprocess.h b/aos/starter/subprocess.h
index 784c544..9630e00 100644
--- a/aos/starter/subprocess.h
+++ b/aos/starter/subprocess.h
@@ -68,7 +68,14 @@
// automatically.
class Application {
public:
- enum class QuietLogging { kYes, kNo };
+ enum class QuietLogging {
+ kYes,
+ kNo,
+ // For debugging child processes not behaving as expected. When a child
+ // experiences an event such as exiting with an error code or dying to due a
+ // signal, this option will cause a log statement to be printed.
+ kNotForDebugging,
+ };
Application(const aos::Application *application, aos::EventLoop *event_loop,
std::function<void()> on_change,
QuietLogging quiet_flag = QuietLogging::kNo);
@@ -127,6 +134,8 @@
bool autorestart() const { return autorestart_; }
void set_autorestart(bool autorestart) { autorestart_ = autorestart; }
+ LastStopReason stop_reason() const { return stop_reason_; }
+
const std::string &GetStdout();
const std::string &GetStderr();
std::optional<int> exit_code() const { return exit_code_; }
diff --git a/aos/starter/subprocess_reliable_test.cc b/aos/starter/subprocess_reliable_test.cc
new file mode 100644
index 0000000..0460bc3
--- /dev/null
+++ b/aos/starter/subprocess_reliable_test.cc
@@ -0,0 +1,127 @@
+#include <signal.h>
+#include <sys/types.h>
+
+#include <filesystem>
+
+#include "gtest/gtest.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/starter/subprocess.h"
+#include "aos/testing/path.h"
+#include "aos/testing/tmpdir.h"
+#include "aos/util/file.h"
+
+namespace aos::starter::testing {
+
+namespace {
+void Wait(pid_t pid) {
+ int status;
+ if (waitpid(pid, &status, 0) != pid) {
+ if (errno != ECHILD) {
+ PLOG(ERROR) << "Failed to wait for PID " << pid << ": " << status;
+ FAIL();
+ }
+ }
+ LOG(INFO) << "Succesfully waited for PID " << pid;
+}
+
+} // namespace
+
+// Validates that killing a child process right after startup doesn't have any
+// unexpected consequences. The child process should exit even if it hasn't
+// `exec()`d yet.
+TEST(SubprocessTest, KillDuringStartup) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+
+ // Run an application that takes a really long time to exit. The exact details
+ // here don't matter. We just need to to survive long enough until we can call
+ // Terminate() below.
+ auto application =
+ std::make_unique<Application>("sleep", "sleep", &event_loop, []() {});
+ application->set_args({"100"});
+
+ // Track whether we exit via our own timer callback. We don't want to exit
+ // because of any strange interactions with the child process.
+ bool exited_as_expected = false;
+
+ // Here's the sequence of events that we expect to see:
+ // 1. Start child process.
+ // 2. Stop child process (via `Terminate()`).
+ // 3. Wait 1 second.
+ // 4. Set `exited_as_expected` to `true`.
+ // 5. Exit the event loop.
+ //
+ // At the end, if `exited_as_expected` is `false`, then something unexpected
+ // happened and we failed the test here.
+ aos::TimerHandler *shutdown_timer = event_loop.AddTimer([&]() {
+ exited_as_expected = true;
+ event_loop.Exit();
+ });
+ aos::TimerHandler *trigger_timer = event_loop.AddTimer([&]() {
+ application->Start();
+ application->Terminate();
+ shutdown_timer->Schedule(event_loop.monotonic_now() +
+ std::chrono::seconds(1));
+ });
+ trigger_timer->Schedule(event_loop.monotonic_now());
+ event_loop.Run();
+ application->Stop();
+ Wait(application->get_pid());
+
+ EXPECT_TRUE(exited_as_expected) << "It looks like we killed ourselves.";
+}
+
+// Validates that the code in subprocess.cc doesn't accidentally block signals
+// in the child process.
+TEST(SubprocessTest, CanKillAfterStartup) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+
+ // We create a directory in which we create some files so this test here and
+ // the subsequently created child process can "signal" one another. We roughly
+ // expect the following sequence of events:
+ // 1. Start the child process.
+ // 2. Test waits for "startup" file to be created by child.
+ // 3. Child process creates the "startup" file.
+ // 4. Test sees "startup" file being created, sends SIGTERM to child.
+ // 5. Child sees SIGTERM, creates "shutdown" file, and exits.
+ // 6. Test waits for child to exit.
+ // 7. Test validates that the "shutdown" file was created by the child.
+ auto signal_dir = std::filesystem::path(aos::testing::TestTmpDir()) /
+ "startup_file_signals";
+ ASSERT_TRUE(std::filesystem::create_directory(signal_dir));
+ auto startup_signal_file = signal_dir / "startup";
+ auto shutdown_signal_file = signal_dir / "shutdown";
+
+ auto application = std::make_unique<Application>("/bin/bash", "/bin/bash",
+ &event_loop, []() {});
+ application->set_args(
+ {"-c", absl::StrCat("cleanup() { touch ", shutdown_signal_file.string(),
+ "; exit 0; }; trap cleanup SIGTERM; touch ",
+ startup_signal_file.string(),
+ "; while true; do sleep 0.1; done;")});
+
+ // Wait for the child process to create the "startup" file.
+ ASSERT_FALSE(std::filesystem::exists(startup_signal_file));
+ application->Start();
+ while (!std::filesystem::exists(startup_signal_file)) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+ ASSERT_FALSE(std::filesystem::exists(shutdown_signal_file));
+
+ // Manually kill the application here. The Stop() and Terminate() helpers
+ // trigger some timeout behaviour that interferes with the test here. This
+ // should cause the child to exit and create the "shutdown" file.
+ PCHECK(kill(application->get_pid(), SIGTERM) == 0);
+ Wait(application->get_pid());
+ ASSERT_TRUE(std::filesystem::exists(shutdown_signal_file));
+}
+
+} // namespace aos::starter::testing
diff --git a/aos/util/config_validator_lib.cc b/aos/util/config_validator_lib.cc
index 0f90ed6..e388bb7 100644
--- a/aos/util/config_validator_lib.cc
+++ b/aos/util/config_validator_lib.cc
@@ -177,6 +177,13 @@
const std::string log_path = aos::testing::TestTmpDir() + "/logs/";
for (const bool send_data_on_channels : {false, true}) {
SCOPED_TRACE(send_data_on_channels);
+ // Single nodes (multi-nodes with node count = 1) will not produce readable
+ // logs in the absense of data.
+ if (!send_data_on_channels && (configuration::NodesCount(config) == 1u)) {
+ continue;
+ }
+ // Send timing report when we are sending data.
+ const bool do_skip_timing_report = !send_data_on_channels;
for (const LoggerNodeSetValidation *logger_set :
*validation_config->logging()->logger_sets()) {
SCOPED_TRACE(aos::FlatbufferToJson(logger_set));
@@ -187,9 +194,11 @@
for (const auto &node : *logger_set->loggers()) {
logger_nodes.push_back(node->str());
}
- loggers = MakeLoggersForNodes(&factory, logger_nodes, log_path);
+ loggers = MakeLoggersForNodes(&factory, logger_nodes, log_path,
+ do_skip_timing_report);
} else {
- loggers = MakeLoggersForAllNodes(&factory, log_path);
+ loggers =
+ MakeLoggersForAllNodes(&factory, log_path, do_skip_timing_report);
}
std::vector<std::unique_ptr<EventLoop>> test_loops;
diff --git a/aos/util/simulation_logger.cc b/aos/util/simulation_logger.cc
index 24691ef..513fa65 100644
--- a/aos/util/simulation_logger.cc
+++ b/aos/util/simulation_logger.cc
@@ -4,13 +4,16 @@
namespace aos::util {
LoggerState::LoggerState(aos::SimulatedEventLoopFactory *factory,
- const aos::Node *node, std::string_view output_folder)
+ const aos::Node *node, std::string_view output_folder,
+ bool do_skip_timing_report)
: event_loop_(factory->MakeEventLoop("logger", node)),
namer_(std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(output_folder, "/", logger::MaybeNodeName(node), "/"),
event_loop_.get())),
logger_(std::make_unique<aos::logger::Logger>(event_loop_.get())) {
- event_loop_->SkipTimingReport();
+ if (do_skip_timing_report) {
+ event_loop_->SkipTimingReport();
+ }
event_loop_->SkipAosLog();
event_loop_->OnRun([this]() { logger_->StartLogging(std::move(namer_)); });
}
@@ -18,23 +21,24 @@
std::vector<std::unique_ptr<LoggerState>> MakeLoggersForNodes(
aos::SimulatedEventLoopFactory *factory,
const std::vector<std::string> &nodes_to_log,
- std::string_view output_folder) {
+ std::string_view output_folder, bool do_skip_timing_report) {
std::vector<std::unique_ptr<LoggerState>> loggers;
for (const std::string &node : nodes_to_log) {
loggers.emplace_back(std::make_unique<LoggerState>(
factory, aos::configuration::GetNode(factory->configuration(), node),
- output_folder));
+ output_folder, do_skip_timing_report));
}
return loggers;
}
std::vector<std::unique_ptr<LoggerState>> MakeLoggersForAllNodes(
- aos::SimulatedEventLoopFactory *factory, std::string_view output_folder) {
+ aos::SimulatedEventLoopFactory *factory, std::string_view output_folder,
+ bool do_skip_timing_report) {
std::vector<std::unique_ptr<LoggerState>> loggers;
for (const aos::Node *node :
configuration::GetNodes(factory->configuration())) {
- loggers.emplace_back(
- std::make_unique<LoggerState>(factory, node, output_folder));
+ loggers.emplace_back(std::make_unique<LoggerState>(
+ factory, node, output_folder, do_skip_timing_report));
}
return loggers;
}
diff --git a/aos/util/simulation_logger.h b/aos/util/simulation_logger.h
index f431b4c..af43d02 100644
--- a/aos/util/simulation_logger.h
+++ b/aos/util/simulation_logger.h
@@ -9,7 +9,8 @@
class LoggerState {
public:
LoggerState(aos::SimulatedEventLoopFactory *factory, const aos::Node *node,
- std::string_view output_folder);
+ std::string_view output_folder,
+ bool do_skip_timing_report = true);
private:
std::unique_ptr<aos::EventLoop> event_loop_;
@@ -23,11 +24,12 @@
std::vector<std::unique_ptr<LoggerState>> MakeLoggersForNodes(
aos::SimulatedEventLoopFactory *factory,
const std::vector<std::string> &nodes_to_log,
- std::string_view output_folder);
+ std::string_view output_folder, bool do_skip_timing_report = true);
// Creates loggers for all of the nodes.
std::vector<std::unique_ptr<LoggerState>> MakeLoggersForAllNodes(
- aos::SimulatedEventLoopFactory *factory, std::string_view output_folder);
+ aos::SimulatedEventLoopFactory *factory, std::string_view output_folder,
+ bool do_skip_timing_report = true);
} // namespace aos::util
#endif // AOS_UTIL_SIMULATION_LOGGER_H_
diff --git a/debian/curl.BUILD b/debian/curl.BUILD
index 95edb78..c6c2d2c 100644
--- a/debian/curl.BUILD
+++ b/debian/curl.BUILD
@@ -175,6 +175,8 @@
name = "curl",
srcs = [
"include/curl_config.h",
+ "lib/altsvc.c",
+ "lib/altsvc.h",
"lib/amigaos.h",
"lib/arpa_telnet.h",
"lib/asyn.h",
@@ -198,6 +200,8 @@
"lib/curl_endian.h",
"lib/curl_fnmatch.c",
"lib/curl_fnmatch.h",
+ "lib/curl_get_line.c",
+ "lib/curl_get_line.h",
"lib/curl_gethostname.c",
"lib/curl_gethostname.h",
"lib/curl_gssapi.h",
@@ -227,6 +231,8 @@
"lib/curl_threads.h",
"lib/curlx.h",
"lib/dict.h",
+ "lib/doh.c",
+ "lib/doh.h",
"lib/dotdot.c",
"lib/dotdot.h",
"lib/easy.c",
@@ -295,16 +301,18 @@
"lib/nwos.c",
"lib/parsedate.c",
"lib/parsedate.h",
- "lib/pingpong.h",
"lib/pingpong.c",
+ "lib/pingpong.h",
"lib/pop3.h",
"lib/progress.c",
"lib/progress.h",
+ "lib/psl.c",
+ "lib/psl.h",
"lib/quic.h",
"lib/rand.c",
"lib/rand.h",
- "lib/rename.h",
"lib/rename.c",
+ "lib/rename.h",
"lib/rtsp.c",
"lib/rtsp.h",
"lib/security.c",
@@ -325,8 +333,8 @@
"lib/smb.h",
"lib/smtp.h",
"lib/sockaddr.h",
- "lib/socketpair.h",
"lib/socketpair.c",
+ "lib/socketpair.h",
"lib/socks.c",
"lib/socks.h",
"lib/speedcheck.c",
@@ -352,6 +360,8 @@
"lib/transfer.h",
"lib/url.c",
"lib/url.h",
+ "lib/urlapi.c",
+ "lib/urlapi-int.h",
"lib/urldata.h",
"lib/vauth/cleartext.c",
"lib/vauth/cram.c",
@@ -367,9 +377,12 @@
"lib/vtls/gskit.h",
"lib/vtls/gtls.h",
"lib/vtls/mbedtls.h",
+ "lib/vtls/mesalink.c",
+ "lib/vtls/mesalink.h",
"lib/vtls/nssg.h",
"lib/vtls/openssl.h",
"lib/vtls/schannel.h",
+ "lib/vtls/sectransp.h",
"lib/vtls/vtls.c",
"lib/vtls/vtls.h",
"lib/vtls/wolfssl.h",
@@ -378,19 +391,6 @@
"lib/wildcard.c",
"lib/wildcard.h",
"lib/x509asn1.h",
- "lib/psl.h",
- "lib/psl.c",
- "lib/vtls/sectransp.h",
- "lib/vtls/mesalink.h",
- "lib/vtls/mesalink.c",
- "lib/curl_get_line.h",
- "lib/curl_get_line.c",
- "lib/urlapi-int.h",
- "lib/urlapi.c",
- "lib/altsvc.h",
- "lib/altsvc.c",
- "lib/doh.h",
- "lib/doh.c",
] + select({
":macos": [
"lib/vtls/sectransp.c",
@@ -463,7 +463,7 @@
visibility = ["//visibility:public"],
deps = [
# Use the same version of zlib that gRPC does.
- "@zlib//:zlib",
+ "@zlib",
":define-ca-bundle-location",
] + select({
":windows": [],
diff --git a/debian/gstreamer.BUILD b/debian/gstreamer.BUILD
index f81f206..a9a28ef 100644
--- a/debian/gstreamer.BUILD
+++ b/debian/gstreamer.BUILD
@@ -262,8 +262,8 @@
"usr/include",
"usr/include/glib-2.0",
"usr/include/gstreamer-1.0",
- "usr/include/libsoup-2.4",
"usr/include/json-glib-1.0",
+ "usr/include/libsoup-2.4",
"usr/include/opencv4",
],
linkopts = [
diff --git a/debian/opencv.BUILD b/debian/opencv.BUILD
index 141a286..d6b4378 100644
--- a/debian/opencv.BUILD
+++ b/debian/opencv.BUILD
@@ -263,10 +263,10 @@
name = "opencv",
srcs = select({
"@platforms//cpu:x86_64": [s % "x86_64-linux-gnu" if "%" in s else s for s in _common_srcs_list] + [
- "usr/lib/x86_64-linux-gnu/libmfx.so.1",
- "usr/lib/x86_64-linux-gnu/libquadmath.so.0",
- "usr/lib/x86_64-linux-gnu/libnuma.so.1",
"usr/lib/x86_64-linux-gnu/libcrypto.so.1.1",
+ "usr/lib/x86_64-linux-gnu/libmfx.so.1",
+ "usr/lib/x86_64-linux-gnu/libnuma.so.1",
+ "usr/lib/x86_64-linux-gnu/libquadmath.so.0",
"usr/lib/x86_64-linux-gnu/libssl.so.1.1",
],
"@platforms//cpu:armv7": [s % "arm-linux-gnueabihf" if "%" in s else s for s in _common_srcs_list],
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 60528a9..442a7de 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -58,6 +58,7 @@
"//y2022/control_loops/superstructure:turret_plotter",
"//y2022/localizer:localizer_plotter",
"//y2022/vision:vision_plotter",
+ "//y2023/control_loops/superstructure:superstructure_plotter",
"//y2023/localizer:corrections_plotter",
"//y2023/localizer:localizer_plotter",
],
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 57d5041..1ee85d1 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -44,6 +44,8 @@
'../../y2022/control_loops/superstructure/turret_plotter'
import {plotSuperstructure as plot2022Superstructure} from
'../../y2022/control_loops/superstructure/superstructure_plotter'
+import {plotSuperstructure as plot2023Superstructure} from
+ '../../y2023/control_loops/superstructure/superstructure_plotter'
import {plotCatapult as plot2022Catapult} from
'../../y2022/control_loops/superstructure/catapult_plotter'
import {plotIntakeFront as plot2022IntakeFront, plotIntakeBack as plot2022IntakeBack} from
@@ -118,6 +120,7 @@
['Robot State', new PlotState(plotDiv, plotRobotState)],
['2023 Vision', new PlotState(plotDiv, plot2023Corrections)],
['2023 Localizer', new PlotState(plotDiv, plot2023Localizer)],
+ ['2023 Superstructure', new PlotState(plotDiv, plot2023Superstructure)],
['2020 Finisher', new PlotState(plotDiv, plot2020Finisher)],
['2020 Accelerator', new PlotState(plotDiv, plot2020Accelerator)],
['2020 Hood', new PlotState(plotDiv, plot2020Hood)],
diff --git a/frc971/constants.h b/frc971/constants.h
index 4f7cb36..cf53287 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -70,6 +70,28 @@
struct RelativeEncoderZeroingConstants {};
+struct ContinuousAbsoluteEncoderZeroingConstants {
+ // The number of samples in the moving average filter.
+ size_t average_filter_size;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ // It is presumed that this will always be 2 * pi for any subsystem using this
+ // class, unless you have a continuous system that for some reason doesn't
+ // have a logical period of 1 revolution in radians.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_position;
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ double zeroing_threshold;
+ // Buffer size for deciding if we are moving.
+ size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of a revolution
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
struct AbsoluteEncoderZeroingConstants {
// The number of samples in the moving average filter.
size_t average_filter_size;
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 3cdb6ab..31b245b 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -416,6 +416,7 @@
"//aos/util:trapezoid_profile",
"//frc971/control_loops:control_loop",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_index",
],
)
@@ -661,6 +662,9 @@
":static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_fbs",
"//aos/testing:googletest",
"//frc971/control_loops:control_loop_test",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
],
)
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 9823f0c..355710c 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -257,7 +257,7 @@
relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
abs_state(3, 0), abs_state(4, 0))
.finished();
- if (velocity_sign_ * goal_velocity_ < 0) {
+ if (velocity_sign_ * goal_velocity_ < -0.1) {
goal_theta = rel_state(2, 0);
}
controls_goal_ << goal_theta, goal_velocity_, 0.0;
diff --git a/frc971/control_loops/drivetrain/robot_state_plotter.ts b/frc971/control_loops/drivetrain/robot_state_plotter.ts
index 8cffd76..7a9a346 100644
--- a/frc971/control_loops/drivetrain/robot_state_plotter.ts
+++ b/frc971/control_loops/drivetrain/robot_state_plotter.ts
@@ -34,6 +34,9 @@
const brownOut = robotStatePlot.addMessageLine(robotState, ['browned_out']);
brownOut.setColor(BROWN);
brownOut.setDrawLine(false);
+ robotStatePlot.addMessageLine(robotState, ['outputs_enabled'])
+ .setColor(CYAN)
+ .setDrawLine(false);
const enabled = robotStatePlot.addMessageLine(joystickState, ['enabled']);
enabled.setColor(GREEN);
enabled.setDrawLine(false);
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index dff5aad..1fa8ac2 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -15,6 +15,7 @@
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pot_and_index.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
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 c886775..ace528e 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
@@ -16,6 +16,8 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_pot_and_absolute_position_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
using ::frc971::control_loops::PositionSensorSimulator;
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 7778da1..c4af163 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -224,7 +224,7 @@
: target_constraints_(target_constraints),
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
- vis_robot_(cv::Size(1280, 1000)) {
+ vis_robot_(cv::Size(kImageWidth_, kImageHeight_)) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
@@ -243,7 +243,7 @@
target_constraints_(target_constraints),
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
- vis_robot_(cv::Size(1280, 1000)) {
+ vis_robot_(cv::Size(kImageWidth_, kImageHeight_)) {
CountConstraints();
}
@@ -369,9 +369,10 @@
TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
// Set up robot visualization.
vis_robot_.ClearImage();
- constexpr int kImageWidth = 1280;
- constexpr double kFocalLength = 500.0;
- vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ // Compute focal length so that image shows field with viewpoint at 10m above
+ // it (default for viewer)
+ const double kFocalLength = kImageWidth_ * 10.0 / kFieldWidth_;
+ vis_robot_.SetDefaultViewpoint(kImageWidth_, kFocalLength);
const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
// Translation and rotation error for each target
@@ -428,6 +429,7 @@
CHECK(SolveOptimizationProblem(&target_pose_problem_2))
<< "The target pose solve 2 was not successful, exiting.";
+ LOG(INFO) << "Solving the overall map's best alignment to the previous map";
ceres::Problem map_fitting_problem(
{.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP});
std::unique_ptr<ceres::CostFunction> map_fitting_cost_function =
@@ -503,7 +505,6 @@
}
namespace {
-
// Hacks to extract a double from a scalar, which is either a ceres jet or a
// double. Only used for debugging and displaying.
template <typename S>
@@ -587,8 +588,11 @@
kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z();
if (FLAGS_visualize_solver) {
+ LOG(INFO) << std::to_string(id) + std::string("-est") << " at "
+ << ScalarAffineToDouble(H_world_actual).matrix();
vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual),
- std::to_string(id), cv::Scalar(0, 255, 0));
+ std::to_string(id) + std::string("-est"),
+ cv::Scalar(0, 255, 0));
vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_ideal),
std::to_string(id), cv::Scalar(255, 255, 255));
}
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index cfefcc7..dc4a85a 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -116,6 +116,10 @@
Eigen::Translation3d T_frozen_actual_;
Eigen::Quaterniond R_frozen_actual_;
+ const double kFieldWidth_ = 20.0; // 20 meters across
+ const int kImageWidth_ = 1000;
+ const int kImageHeight_ =
+ kImageWidth_ * 3.0 / 4.0; // Roughly matches field aspect ratio
mutable VisualizeRobot vis_robot_;
Stats stats_with_outliers_;
diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
index 6be83aa..c3ff26d 100644
--- a/frc971/wpilib/falcon.cc
+++ b/frc971/wpilib/falcon.cc
@@ -3,11 +3,12 @@
using frc971::wpilib::Falcon;
using frc971::wpilib::kMaxBringupPower;
-Falcon::Falcon(int device_id, std::string canbus,
+Falcon::Falcon(int device_id, bool inverted, std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit)
: talon_(device_id, canbus),
device_id_(device_id),
+ inverted_(inverted),
device_temp_(talon_.GetDeviceTemp()),
supply_voltage_(talon_.GetSupplyVoltage()),
supply_current_(talon_.GetSupplyCurrent()),
@@ -37,6 +38,12 @@
signals->push_back(&duty_cycle_);
}
+Falcon::Falcon(FalconParams params, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit)
+ : Falcon(params.device_id, params.inverted, canbus, signals,
+ stator_current_limit, supply_current_limit) {}
+
void Falcon::PrintConfigs() {
ctre::phoenix6::configs::TalonFXConfiguration configuration;
ctre::phoenix::StatusCode status =
@@ -48,9 +55,7 @@
AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
}
-void Falcon::WriteConfigs(ctre::phoenix6::signals::InvertedValue invert) {
- inverted_ = invert;
-
+void Falcon::WriteConfigs() {
ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
current_limits.StatorCurrentLimit = stator_current_limit_;
current_limits.StatorCurrentLimitEnable = true;
@@ -94,7 +99,8 @@
return status;
}
-void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb) {
+void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio) {
control_loops::CANFalcon::Builder builder(*fbb);
builder.add_id(device_id_);
builder.add_device_temp(device_temp());
@@ -102,7 +108,7 @@
builder.add_supply_current(supply_current());
builder.add_torque_current(torque_current());
builder.add_duty_cycle(duty_cycle());
- builder.add_position(position());
+ builder.add_position(position() * gear_ratio);
last_position_offset_ = builder.Finish();
}
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
index 8f0f1f0..6ea8735 100644
--- a/frc971/wpilib/falcon.h
+++ b/frc971/wpilib/falcon.h
@@ -18,24 +18,35 @@
namespace frc971 {
namespace wpilib {
+struct FalconParams {
+ int device_id;
+ bool inverted;
+};
+
static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
static constexpr double kMaxBringupPower = 12.0;
// Gets info from and writes to falcon motors using the TalonFX controller.
class Falcon {
public:
- Falcon(int device_id, std::string canbus,
+ Falcon(int device_id, bool inverted, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
+
+ Falcon(FalconParams params, std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit);
void PrintConfigs();
- void WriteConfigs(ctre::phoenix6::signals::InvertedValue invert);
+ void WriteConfigs();
ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
- void SerializePosition(flatbuffers::FlatBufferBuilder *fbb);
+ // The position of the Falcon output shaft is multiplied by gear_ratio
+ void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio);
std::optional<flatbuffers::Offset<control_loops::CANFalcon>> TakeOffset();
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
index 2b6ef9e..bfc5d73 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
@@ -37,8 +37,8 @@
void DrivetrainWriter::WriteConfigs() {
for (auto module : {front_left_, front_right_, back_left_, back_right_}) {
- module->rotation->WriteConfigs(false);
- module->translation->WriteConfigs(false);
+ module->rotation->WriteConfigs();
+ module->translation->WriteConfigs();
}
}
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index 534f0ce..f449afa 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -8,14 +8,15 @@
namespace swerve {
struct SwerveModule {
- SwerveModule(int rotation_id, int translation_id, std::string canbus,
+ SwerveModule(FalconParams rotation_params, FalconParams translation_params,
+ std::string canbus,
std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
double stator_current_limit, double supply_current_limit)
- : rotation(std::make_shared<Falcon>(rotation_id, canbus, signals,
+ : rotation(std::make_shared<Falcon>(rotation_params, canbus, signals,
stator_current_limit,
supply_current_limit)),
- translation(std::make_shared<Falcon>(translation_id, canbus, signals,
- stator_current_limit,
+ translation(std::make_shared<Falcon>(translation_params, canbus,
+ signals, stator_current_limit,
supply_current_limit)) {}
void WriteModule(
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index d50f181..8ca7f67 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -61,20 +61,8 @@
cc_library(
name = "zeroing",
srcs = [
- "absolute_and_absolute_encoder.cc",
- "absolute_encoder.cc",
- "hall_effect_and_position.cc",
- "pot_and_absolute_encoder.cc",
- "pot_and_index.cc",
- "pulse_index.cc",
],
hdrs = [
- "absolute_and_absolute_encoder.h",
- "absolute_encoder.h",
- "hall_effect_and_position.h",
- "pot_and_absolute_encoder.h",
- "pot_and_index.h",
- "pulse_index.h",
"zeroing.h",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -88,19 +76,10 @@
],
)
-cc_test(
- name = "zeroing_test",
- srcs = [
- "absolute_and_absolute_encoder_test.cc",
- "absolute_encoder_test.cc",
- "hall_effect_and_position_test.cc",
- "pot_and_absolute_encoder_test.cc",
- "pot_and_index_test.cc",
- "pulse_index_test.cc",
- "relative_encoder_test.cc",
- "zeroing_test.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
+cc_library(
+ name = "zeroing_test_lib",
+ testonly = True,
+ hdrs = ["zeroing_test.h"],
deps = [
":zeroing",
"//aos/testing:googletest",
@@ -109,6 +88,46 @@
],
)
+[
+ (
+ cc_library(
+ name = lib,
+ srcs = [lib + ".cc"],
+ hdrs = [lib + ".h"],
+ deps = [
+ ":wrap",
+ ":zeroing",
+ "//aos/containers:error_list",
+ "//aos/logging",
+ "//frc971:constants",
+ "//frc971/control_loops:control_loops_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+ ),
+ cc_test(
+ name = lib + "_test",
+ srcs = [lib + "_test.cc"],
+ deps = [
+ lib,
+ ":zeroing",
+ ":zeroing_test_lib",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:position_sensor_sim",
+ ],
+ ),
+ )
+ for lib in [
+ "absolute_and_absolute_encoder",
+ "absolute_encoder",
+ "continuous_absolute_encoder",
+ "hall_effect_and_position",
+ "pot_and_absolute_encoder",
+ "pot_and_index",
+ "pulse_index",
+ ]
+]
+
cc_library(
name = "wrap",
srcs = [
diff --git a/frc971/zeroing/absolute_encoder.h b/frc971/zeroing/absolute_encoder.h
index df40ec3..9d730f2 100644
--- a/frc971/zeroing/absolute_encoder.h
+++ b/frc971/zeroing/absolute_encoder.h
@@ -5,6 +5,7 @@
#include "flatbuffers/flatbuffers.h"
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
diff --git a/frc971/zeroing/continuous_absolute_encoder.cc b/frc971/zeroing/continuous_absolute_encoder.cc
new file mode 100644
index 0000000..a47a491
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.cc
@@ -0,0 +1,169 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "aos/containers/error_list.h"
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+ContinuousAbsoluteEncoderZeroingEstimator::
+ ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void ContinuousAbsoluteEncoderZeroingEstimator::Reset() {
+ zeroed_ = false;
+ error_ = false;
+ first_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ position_ = 0.0;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ move_detector_.Reset();
+}
+
+// The math here is a bit backwards, but I think it'll be less error prone that
+// way and more similar to the version with a pot as well.
+//
+// We start by unwrapping the absolute encoder using the relative encoder. This
+// puts us in a non-wrapping space and lets us average a bit easier. From
+// there, we can compute an offset and wrap ourselves back such that we stay
+// close to the middle value.
+//
+// To guard against the robot moving while updating estimates, buffer a number
+// of samples and check that the buffered samples are not different than the
+// zeroing threshold. At any point that the samples differ too much, do not
+// update estimates based on those samples.
+void ContinuousAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsolutePosition &info) {
+ // Check for Abs Encoder NaN value that would mess up the rest of the zeroing
+ // code below. NaN values are given when the Absolute Encoder is disconnected.
+ if (::std::isnan(info.absolute_encoder())) {
+ if (zeroed_) {
+ VLOG(1) << "NAN on absolute encoder.";
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ } else {
+ ++nan_samples_;
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
+ if (nan_samples_ >= constants_.average_filter_size) {
+ errors_.Set(ZeroingError::LOST_ABSOLUTE_ENCODER);
+ error_ = true;
+ zeroed_ = true;
+ }
+ }
+ // Throw some dummy values in for now.
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ position_ = offset_ + info.encoder();
+ return;
+ }
+
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
+
+ if (!moving) {
+ const PositionStruct &sample = move_detector_.GetSample();
+
+ // adjusted_* numbers are nominally in the desired output frame.
+ const double adjusted_absolute_encoder =
+ sample.absolute_encoder - constants_.measured_absolute_position;
+
+ // Note: If are are near the breakpoint of the absolute encoder, this number
+ // will be jitter between numbers that are ~one_revolution_distance apart.
+ // For that reason, we rewrap it so that we are not near that boundary.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // To avoid the aforementioned jitter, choose a base value to use for
+ // wrapping. When we have no prior samples, just use the current offset.
+ // Otherwise, we use an arbitrary prior offset (the stored offsets will all
+ // already be wrapped).
+ const double relative_to_absolute_offset_wrap_base =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? relative_to_absolute_offset
+ : relative_to_absolute_offset_samples_[0];
+
+ const double relative_to_absolute_offset_wrapped =
+ UnWrap(relative_to_absolute_offset_wrap_base,
+ relative_to_absolute_offset, constants_.one_revolution_distance);
+
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset_wrapped);
+ } else {
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset_wrapped;
+ }
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ // Compute the average offset between the absolute encoder and relative
+ // encoder. Because we just pushed a value, the size() will never be zero.
+ offset_ =
+ ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(), 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ // To provide a value that can be used to estimate the
+ // measured_absolute_position when zeroing, we just need to output the
+ // current absolute encoder value. We could make use of the averaging
+ // implicit in offset_ to reduce the noise on this slightly.
+ filtered_absolute_encoder_ = sample.absolute_encoder;
+
+ if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(first_offset_ - offset_) >
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance) {
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
+ errors_.Set(ZeroingError::OFFSET_MOVED_TOO_FAR);
+ error_ = true;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position. Wrap it to reflect the fact that we do not have
+ // sufficient information to disambiguate which revolution we are on (also,
+ // since this value is primarily meant for debugging, this makes it easier to
+ // see that the device is actually at zero without having to divide by 2 *
+ // pi).
+ position_ =
+ Wrap(0.0, offset_ + info.encoder(), constants_.one_revolution_distance);
+}
+
+flatbuffers::Offset<ContinuousAbsoluteEncoderZeroingEstimator::State>
+ContinuousAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<flatbuffers::Vector<ZeroingError>> errors_offset =
+ errors_.ToFlatbuffer(fbb);
+
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ builder.add_errors(errors_offset);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/continuous_absolute_encoder.h b/frc971/zeroing/continuous_absolute_encoder.h
new file mode 100644
index 0000000..e11d866
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder.h
@@ -0,0 +1,99 @@
+#ifndef FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
+#define FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
+
+#include <vector>
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "aos/containers/error_list.h"
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an absolute encoder which spins continuously. The
+// absolute encoder must have a 1:1 ratio to the output.
+// The provided offset(), when added to incremental encoder, may return a value
+// outside of +/- pi. The user is responsible for handling wrapping.
+class ContinuousAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<
+ AbsolutePosition,
+ constants::ContinuousAbsoluteEncoderZeroingConstants,
+ AbsoluteEncoderEstimatorState> {
+ public:
+ explicit ContinuousAbsoluteEncoderZeroingEstimator(
+ const constants::ContinuousAbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const AbsolutePosition &info) override;
+
+ void TriggerError() override { error_ = true; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ struct PositionStruct {
+ PositionStruct(const AbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double absolute_encoder;
+ double encoder;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::ContinuousAbsoluteEncoderZeroingConstants constants_;
+
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ MoveDetector<PositionStruct, AbsolutePosition> move_detector_;
+
+ // Estimated start position of the mechanism
+ double offset_ = 0;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_ = 0;
+
+ // Number of NANs we've seen in a row.
+ size_t nan_samples_ = 0;
+
+ // The filtered position.
+ double position_ = 0.0;
+
+ // Marker to track what kind of error has occured.
+ aos::ErrorList<ZeroingError> errors_;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_CONTINUOUS_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/continuous_absolute_encoder_test.cc b/frc971/zeroing/continuous_absolute_encoder_test.cc
new file mode 100644
index 0000000..3869393
--- /dev/null
+++ b/frc971/zeroing/continuous_absolute_encoder_test.cc
@@ -0,0 +1,198 @@
+#include "frc971/zeroing/continuous_absolute_encoder.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/wrap.h"
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::ContinuousAbsoluteEncoderZeroingConstants;
+
+class ContinuousAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ ContinuousAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ flatbuffers::FlatBufferBuilder fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithoutMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that if the underlying mechanism moves by >1 revolution that the
+// continuous zeroing estimator handles it correctly.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ ContinuousEstimatorZeroesAcrossRevolution) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+
+ // Now, rotate by a full revolution, then stay still. We should stay zeroed.
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize; ++i) {
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+ }
+ ASSERT_TRUE(estimator.zeroed());
+ ASSERT_FALSE(estimator.error());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingIgnoresNAN) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 2.1;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ // Because the continuous estimator doesn't care about extra revolutions, it
+ // will have brought the offset down to less than index_diff.
+ EXPECT_NEAR(Wrap(0.0, start_pos, index_diff), estimator.offset(), 1e-12);
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithMovement) {
+ const double index_diff = 1.0;
+ PositionSensorSimulator sim(index_diff);
+
+ const double start_pos = 10 * index_diff;
+ double measured_absolute_position = 0.3 * index_diff;
+
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, index_diff, measured_absolute_position,
+ 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ sim.Initialize(start_pos, index_diff / 3.0, 0.0,
+ constants.measured_absolute_position);
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * index_diff);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * index_diff);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ContinuousAbsoluteEncoderZeroingTest,
+ TestContinuousAbsoluteEncoderZeroingWithNaN) {
+ ContinuousAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize, kIndexErrorFraction};
+
+ ContinuousAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+
+ estimator.UpdateEstimate(*sensor_values);
+ ASSERT_TRUE(estimator.error());
+
+ flatbuffers::FlatBufferBuilder fbb2;
+ fbb2.Finish(estimator.GetEstimatorState(&fbb2));
+
+ const AbsoluteEncoderEstimatorState *state =
+ flatbuffers::GetRoot<AbsoluteEncoderEstimatorState>(
+ fbb2.GetBufferPointer());
+
+ EXPECT_THAT(*state->errors(),
+ ::testing::ElementsAre(ZeroingError::LOST_ABSOLUTE_ENCODER));
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index f7b52a6..5d5b6eb 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -172,13 +172,4 @@
} // namespace zeroing
} // namespace frc971
-// TODO(Brian): Actually split these targets apart. Need to convert all the
-// reverse dependencies to #include what they actually need...
-#include "frc971/zeroing/absolute_and_absolute_encoder.h"
-#include "frc971/zeroing/absolute_encoder.h"
-#include "frc971/zeroing/hall_effect_and_position.h"
-#include "frc971/zeroing/pot_and_absolute_encoder.h"
-#include "frc971/zeroing/pot_and_index.h"
-#include "frc971/zeroing/pulse_index.h"
-
#endif // FRC971_ZEROING_ZEROING_H_
diff --git a/scouting/db/db.go b/scouting/db/db.go
index ac9a6e8..1a43634 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -199,6 +199,14 @@
return result.Error
}
+func (database *Database) DeleteFromActions(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ var actions []Action
+ result := database.
+ Where("comp_level = ? AND match_number = ? AND set_number = ? AND team_number = ?", compLevel_, matchNumber_, setNumber_, teamNumber_).
+ Delete(&actions)
+ return result.Error
+}
+
func (database *Database) AddOrUpdateRankings(r Ranking) error {
result := database.Clauses(clause.OnConflict{
UpdateAll: true,
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 8a4c0bc..d49e649 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -505,6 +505,75 @@
}
}
+func TestDeleteFromActions(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+
+ startingActions := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1239", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0004, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0005, CollectedBy: "",
+ },
+ }
+
+ correct := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), Timestamp: 0005, CollectedBy: "",
+ },
+ }
+
+ for _, action := range startingActions {
+ err := fixture.db.AddAction(action)
+ check(t, err, "Failed to add stat")
+ }
+
+ err := fixture.db.DeleteFromActions("quals", 94, 1, "1239")
+
+ got, err := fixture.db.ReturnActions()
+ check(t, err, "Failed ReturnActions()")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestQueryShiftDB(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 52d838b..b1276d9 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -8,6 +8,10 @@
// (index 3) which resolves to team 3990 in quals match 1.
const QUALS_MATCH_1_TEAM_3990 = 0 * 6 + 3;
+// On the 2st row of matches (index 1) click on the fourth team
+// (index 3) which resolves to team 4481 in quals match 1.
+const QUALS_MATCH_2_TEAM_4481 = 1 * 6 + 3;
+
function disableAlerts() {
cy.get('#block_alerts').check({force: true}).should('be.checked');
}
@@ -66,6 +70,54 @@
return element;
}
+function submitDataScouting(
+ matchButtonKey = SEMI_FINAL_2_MATCH_3_TEAM_5254,
+ teamNumber = 5254
+) {
+ // Click on a random team in the Match list. The exact details here are not
+ // important, but we need to know what they are. This could as well be any
+ // other team from any other match.
+ cy.get('button.match-item').eq(matchButtonKey).click();
+
+ // Select Starting Position.
+ headerShouldBe(teamNumber + ' Init ');
+ cy.get('[type="radio"]').first().check();
+ clickButton('Start Match');
+
+ // Pick and Place Cone in Auto.
+ clickButton('CONE');
+ clickButton('HIGH');
+
+ // Pick and Place Cube in Teleop.
+ clickButton('Start Teleop');
+ clickButton('CUBE');
+ clickButton('LOW');
+
+ // Robot dead and revive.
+ clickButton('DEAD');
+ clickButton('Revive');
+
+ // Endgame.
+ clickButton('Endgame');
+ cy.contains(/Docked & Engaged/).click();
+
+ clickButton('End Match');
+ headerShouldBe(teamNumber + ' Review and Submit ');
+ cy.get('#review_data li')
+ .eq(0)
+ .should('have.text', ' Started match at position 1 ');
+ cy.get('#review_data li').eq(1).should('have.text', ' Picked up kCone ');
+ cy.get('#review_data li')
+ .last()
+ .should(
+ 'have.text',
+ ' Ended Match; docked: false, engaged: true, attempted to dock and engage: false '
+ );
+
+ clickButton('Submit');
+ headerShouldBe(teamNumber + ' Success ');
+}
+
before(() => {
cy.visit('/');
disableAlerts();
@@ -139,48 +191,7 @@
//TODO(FILIP): Verify last action when the last action header gets added.
it('should: be able to submit data scouting.', () => {
- // Click on a random team in the Match list. The exact details here are not
- // important, but we need to know what they are. This could as well be any
- // other team from any other match.
- cy.get('button.match-item').eq(SEMI_FINAL_2_MATCH_3_TEAM_5254).click();
-
- // Select Starting Position.
- headerShouldBe('5254 Init ');
- cy.get('[type="radio"]').first().check();
- clickButton('Start Match');
-
- // Pick and Place Cone in Auto.
- clickButton('CONE');
- clickButton('HIGH');
-
- // Pick and Place Cube in Teleop.
- clickButton('Start Teleop');
- clickButton('CUBE');
- clickButton('LOW');
-
- // Robot dead and revive.
- clickButton('DEAD');
- clickButton('Revive');
-
- // Endgame.
- clickButton('Endgame');
- cy.contains(/Docked & Engaged/).click();
-
- clickButton('End Match');
- headerShouldBe('5254 Review and Submit ');
- cy.get('#review_data li')
- .eq(0)
- .should('have.text', ' Started match at position 1 ');
- cy.get('#review_data li').eq(1).should('have.text', ' Picked up kCone ');
- cy.get('#review_data li')
- .last()
- .should(
- 'have.text',
- ' Ended Match; docked: false, engaged: true, attempted to dock and engage: false '
- );
-
- clickButton('Submit');
- headerShouldBe('5254 Success ');
+ submitDataScouting();
// Now that the data is submitted, the button should be disabled.
switchToTab('Match List');
@@ -189,6 +200,30 @@
.should('be.disabled');
});
+ it('should: be able to delete data scouting entry', () => {
+ // Submit data to delete.
+ submitDataScouting(QUALS_MATCH_2_TEAM_4481, 4481);
+
+ switchToTab('View');
+
+ cy.get('[data-bs-toggle="dropdown"]').click();
+ cy.get('[id="stats_source_dropdown"]').click();
+
+ // Check that table contains data.
+ cy.get('table.table tbody td').should('contain', '4481');
+
+ // Find and click the delete button for the row containing team 4481.
+ cy.get('table.table tbody td')
+ .contains('4481')
+ .parent()
+ .find('[id^="delete_button_"]')
+ .click();
+ cy.on('window:confirm', () => true);
+
+ // Check that deleted data is not in table.
+ cy.get('table.table tbody').should('not.contain', '4481');
+ });
+
it('should: be able to return to correct screen with undo for pick and place.', () => {
cy.get('button.match-item').eq(QUALS_MATCH_1_TEAM_3990).click();
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index 2ff8b85..795d0ee 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -8,6 +8,8 @@
visibility = ["//visibility:public"],
deps = [
"//scouting/db",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:error_response_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
@@ -42,6 +44,7 @@
deps = [
"//scouting/db",
"//scouting/webserver/requests/debug",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_go_fbs",
diff --git a/scouting/webserver/requests/debug/BUILD b/scouting/webserver/requests/debug/BUILD
index d9bb030..ef14e5a 100644
--- a/scouting/webserver/requests/debug/BUILD
+++ b/scouting/webserver/requests/debug/BUILD
@@ -7,6 +7,7 @@
target_compatible_with = ["@platforms//cpu:x86_64"],
visibility = ["//visibility:public"],
deps = [
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:error_response_go_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_response_go_fbs",
diff --git a/scouting/webserver/requests/debug/debug.go b/scouting/webserver/requests/debug/debug.go
index acb9dd4..4837cfe 100644
--- a/scouting/webserver/requests/debug/debug.go
+++ b/scouting/webserver/requests/debug/debug.go
@@ -9,6 +9,7 @@
"log"
"net/http"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/error_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_driver_rankings_response"
@@ -164,3 +165,9 @@
server+"/requests/submit/submit_actions", requestBytes,
submit_actions_response.GetRootAsSubmitActionsResponse)
}
+
+func Delete2023DataScouting(server string, requestBytes []byte) (*delete_2023_data_scouting_response.Delete2023DataScoutingResponseT, error) {
+ return sendMessage[delete_2023_data_scouting_response.Delete2023DataScoutingResponseT](
+ server+"/requests/delete/delete_2023_data_scouting", requestBytes,
+ delete_2023_data_scouting_response.GetRootAsDelete2023DataScoutingResponse)
+}
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index c1cd999..db422ed 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -23,6 +23,8 @@
"submit_driver_ranking_response",
"submit_actions",
"submit_actions_response",
+ "delete_2023_data_scouting",
+ "delete_2023_data_scouting_response",
)
filegroup(
diff --git a/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs b/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs
new file mode 100644
index 0000000..a2a3ce6
--- /dev/null
+++ b/scouting/webserver/requests/messages/delete_2023_data_scouting.fbs
@@ -0,0 +1,10 @@
+namespace scouting.webserver.requests;
+
+table Delete2023DataScouting {
+ comp_level:string (id: 0);
+ match_number:int (id: 1);
+ set_number:int (id: 2);
+ team_number:string (id: 3);
+}
+
+root_type Delete2023DataScouting;
diff --git a/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs b/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs
new file mode 100644
index 0000000..fd07526
--- /dev/null
+++ b/scouting/webserver/requests/messages/delete_2023_data_scouting_response.fbs
@@ -0,0 +1,7 @@
+namespace scouting.webserver.requests;
+
+table Delete2023DataScoutingResponse {
+ // empty response
+}
+
+root_type Delete2023DataScoutingResponse;
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 37a6ff2..533959c 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -12,6 +12,8 @@
"strings"
"github.com/frc971/971-Robot-Code/scouting/db"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/error_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
@@ -58,6 +60,8 @@
type SubmitActions = submit_actions.SubmitActions
type Action = submit_actions.Action
type SubmitActionsResponseT = submit_actions_response.SubmitActionsResponseT
+type Delete2023DataScouting = delete_2023_data_scouting.Delete2023DataScouting
+type Delete2023DataScoutingResponseT = delete_2023_data_scouting_response.Delete2023DataScoutingResponseT
// The interface we expect the database abstraction to conform to.
// We use an interface here because it makes unit testing easier.
@@ -76,6 +80,8 @@
AddNotes(db.NotesData) error
AddDriverRanking(db.DriverRankingData) error
AddAction(db.Action) error
+ DeleteFromStats(string, int32, int32, string) error
+ DeleteFromActions(string, int32, int32, string) error
}
// Handles unknown requests. Just returns a 404.
@@ -871,6 +877,50 @@
w.Write(builder.FinishedBytes())
}
+type Delete2023DataScoutingHandler struct {
+ db Database
+}
+
+func (handler Delete2023DataScoutingHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
+ requestBytes, err := io.ReadAll(req.Body)
+ if err != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
+ return
+ }
+
+ request, success := parseRequest(w, requestBytes, "Delete2023DataScouting", delete_2023_data_scouting.GetRootAsDelete2023DataScouting)
+ if !success {
+ return
+ }
+
+ err = handler.db.DeleteFromStats(
+ string(request.CompLevel()),
+ request.MatchNumber(),
+ request.SetNumber(),
+ string(request.TeamNumber()))
+
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to delete from stats: %v", err))
+ return
+ }
+
+ err = handler.db.DeleteFromActions(
+ string(request.CompLevel()),
+ request.MatchNumber(),
+ request.SetNumber(),
+ string(request.TeamNumber()))
+
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprintf("Failed to delete from actions: %v", err))
+ return
+ }
+
+ var response Delete2023DataScoutingResponseT
+ builder := flatbuffers.NewBuilder(10)
+ builder.Finish((&response).Pack(builder))
+ w.Write(builder.FinishedBytes())
+}
+
func HandleRequests(db Database, scoutingServer server.ScoutingServer) {
scoutingServer.HandleFunc("/requests", unknown)
scoutingServer.Handle("/requests/request/all_matches", requestAllMatchesHandler{db})
@@ -883,4 +933,5 @@
scoutingServer.Handle("/requests/request/shift_schedule", requestShiftScheduleHandler{db})
scoutingServer.Handle("/requests/submit/submit_driver_ranking", SubmitDriverRankingHandler{db})
scoutingServer.Handle("/requests/submit/submit_actions", submitActionsHandler{db})
+ scoutingServer.Handle("/requests/delete/delete_2023_data_scouting", Delete2023DataScoutingHandler{db})
}
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index c1fe860..20a63ca 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -7,6 +7,7 @@
"github.com/frc971/971-Robot-Code/scouting/db"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/debug"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/delete_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_2023_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_driver_rankings"
@@ -907,6 +908,112 @@
}
}
+// Validates that we can delete stats.
+func TestDeleteFromStats(t *testing.T) {
+ database := MockDatabase{
+ stats2023: []db.Stats2023{
+ {
+ TeamNumber: "3634", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 3, LowCubesAuto: 10,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 0,
+ LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
+ ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
+ HighCubes: 2, CubesDropped: 1, LowCones: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
+ BalanceAttemptAuto: false, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "isaac",
+ },
+ {
+ TeamNumber: "2343", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 1, LowCubesAuto: 0,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 2,
+ LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
+ ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
+ HighCubes: 1, CubesDropped: 0, LowCones: 0,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 53, Mobility: false, DockedAuto: false, EngagedAuto: false,
+ BalanceAttemptAuto: true, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "unknown",
+ },
+ },
+ actions: []db.Action{
+ {
+ PreScouting: true,
+ TeamNumber: "3634",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 2400,
+ },
+ {
+ PreScouting: true,
+ TeamNumber: "2343",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 1009,
+ },
+ },
+ }
+ scoutingServer := server.NewScoutingServer()
+ HandleRequests(&database, scoutingServer)
+ scoutingServer.Start(8080)
+ defer scoutingServer.Stop()
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&delete_2023_data_scouting.Delete2023DataScoutingT{
+ CompLevel: "quals",
+ MatchNumber: 1,
+ SetNumber: 2,
+ TeamNumber: "2343",
+ }).Pack(builder))
+
+ _, err := debug.Delete2023DataScouting("http://localhost:8080", builder.FinishedBytes())
+ if err != nil {
+ t.Fatal("Failed to delete from data scouting ", err)
+ }
+
+ expectedActions := []db.Action{
+ {
+ PreScouting: true,
+ TeamNumber: "3634",
+ MatchNumber: 1,
+ SetNumber: 2,
+ CompLevel: "quals",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 2400,
+ },
+ }
+
+ expectedStats := []db.Stats2023{
+ {
+ TeamNumber: "3634", MatchNumber: 1, SetNumber: 2,
+ CompLevel: "quals", StartingQuadrant: 3, LowCubesAuto: 10,
+ MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 0,
+ LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
+ ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
+ HighCubes: 2, CubesDropped: 1, LowCones: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
+ BalanceAttemptAuto: false, Docked: false, Engaged: false,
+ BalanceAttempt: true, CollectedBy: "isaac",
+ },
+ }
+
+ if !reflect.DeepEqual(expectedActions, database.actions) {
+ t.Fatal("Expected ", expectedActions, ", but got:", database.actions)
+ }
+ if !reflect.DeepEqual(expectedStats, database.stats2023) {
+ t.Fatal("Expected ", expectedStats, ", but got:", database.stats2023)
+ }
+}
+
// A mocked database we can use for testing. Add functionality to this as
// needed for your tests.
@@ -995,3 +1102,29 @@
func (database *MockDatabase) ReturnActions() ([]db.Action, error) {
return database.actions, nil
}
+
+func (database *MockDatabase) DeleteFromStats(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ for i, stat := range database.stats2023 {
+ if stat.CompLevel == compLevel_ &&
+ stat.MatchNumber == matchNumber_ &&
+ stat.SetNumber == setNumber_ &&
+ stat.TeamNumber == teamNumber_ {
+ // Match found, remove the element from the array.
+ database.stats2023 = append(database.stats2023[:i], database.stats2023[i+1:]...)
+ }
+ }
+ return nil
+}
+
+func (database *MockDatabase) DeleteFromActions(compLevel_ string, matchNumber_ int32, setNumber_ int32, teamNumber_ string) error {
+ for i, action := range database.actions {
+ if action.CompLevel == compLevel_ &&
+ action.MatchNumber == matchNumber_ &&
+ action.SetNumber == setNumber_ &&
+ action.TeamNumber == teamNumber_ {
+ // Match found, remove the element from the array.
+ database.actions = append(database.actions[:i], database.actions[i+1:]...)
+ }
+ }
+ return nil
+}
diff --git a/scouting/www/view/BUILD b/scouting/www/view/BUILD
index d787e8f..67c7e3b 100644
--- a/scouting/www/view/BUILD
+++ b/scouting/www/view/BUILD
@@ -10,6 +10,8 @@
],
deps = [
":node_modules/@angular/forms",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_ts_fbs",
+ "//scouting/webserver/requests/messages:delete_2023_data_scouting_ts_fbs",
"//scouting/webserver/requests/messages:error_response_ts_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_response_ts_fbs",
"//scouting/webserver/requests/messages:request_2023_data_scouting_ts_fbs",
diff --git a/scouting/www/view/view.component.ts b/scouting/www/view/view.component.ts
index 561ae08..c75f83b 100644
--- a/scouting/www/view/view.component.ts
+++ b/scouting/www/view/view.component.ts
@@ -1,4 +1,6 @@
import {Component, OnInit} from '@angular/core';
+import {Builder, ByteBuffer} from 'flatbuffers';
+import {ErrorResponse} from '../../webserver/requests/messages/error_response_generated';
import {
Ranking,
RequestAllDriverRankingsResponse,
@@ -11,6 +13,8 @@
Note,
RequestAllNotesResponse,
} from '../../webserver/requests/messages/request_all_notes_response_generated';
+import {Delete2023DataScouting} from '../../webserver/requests/messages/delete_2023_data_scouting_generated';
+import {Delete2023DataScoutingResponse} from '../../webserver/requests/messages/delete_2023_data_scouting_response_generated';
import {ViewDataRequestor} from '../rpc';
@@ -104,16 +108,83 @@
}
// TODO(Filip): Add delete functionality.
- // Gets called when a user clicks the delete icon.
- async deleteData() {
+ // Gets called when a user clicks the delete icon (note scouting).
+ async deleteNoteData() {
const block_alerts = document.getElementById(
'block_alerts'
) as HTMLInputElement;
- if (!block_alerts.checked) {
- if (!window.confirm('Actually delete data?')) {
- this.errorMessage = 'Deleting data has not been implemented yet.';
- return;
- }
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ this.errorMessage = 'Deleting data has not been implemented yet.';
+ return;
+ }
+ }
+
+ // TODO(Filip): Add delete functionality.
+ // Gets called when a user clicks the delete icon (driver ranking).
+ async deleteDriverRankingData() {
+ const block_alerts = document.getElementById(
+ 'block_alerts'
+ ) as HTMLInputElement;
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ this.errorMessage = 'Deleting data has not been implemented yet.';
+ return;
+ }
+ }
+
+ // Gets called when a user clicks the delete icon.
+ async deleteDataScouting(
+ compLevel: string,
+ matchNumber: number,
+ setNumber: number,
+ teamNumber: string
+ ) {
+ const block_alerts = document.getElementById(
+ 'block_alerts'
+ ) as HTMLInputElement;
+ if (block_alerts.checked || window.confirm('Actually delete data?')) {
+ await this.requestDeleteDataScouting(
+ compLevel,
+ matchNumber,
+ setNumber,
+ teamNumber
+ );
+ await this.fetchStats2023();
+ }
+ }
+
+ async requestDeleteDataScouting(
+ compLevel: string,
+ matchNumber: number,
+ setNumber: number,
+ teamNumber: string
+ ) {
+ this.progressMessage = 'Deleting data. Please be patient.';
+ const builder = new Builder();
+ const compLevelData = builder.createString(compLevel);
+ const teamNumberData = builder.createString(teamNumber);
+
+ builder.finish(
+ Delete2023DataScouting.createDelete2023DataScouting(
+ builder,
+ compLevelData,
+ matchNumber,
+ setNumber,
+ teamNumberData
+ )
+ );
+
+ const buffer = builder.asUint8Array();
+ const res = await fetch('/requests/delete/delete_2023_data_scouting', {
+ method: 'POST',
+ body: buffer,
+ });
+
+ if (!res.ok) {
+ const resBuffer = await res.arrayBuffer();
+ const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
+ const parsedResponse = ErrorResponse.getRootAsErrorResponse(fbBuffer);
+ const errorMessage = parsedResponse.errorMessage();
+ this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
}
}
diff --git a/scouting/www/view/view.ng.html b/scouting/www/view/view.ng.html
index 667765f..ee2c62f 100644
--- a/scouting/www/view/view.ng.html
+++ b/scouting/www/view/view.ng.html
@@ -11,12 +11,22 @@
</button>
<ul class="dropdown-menu">
<li>
- <a class="dropdown-item" href="#" (click)="switchDataSource('Notes')">
+ <a
+ class="dropdown-item"
+ href="#"
+ (click)="switchDataSource('Notes')"
+ id="notes_source_dropdown"
+ >
Notes
</a>
</li>
<li>
- <a class="dropdown-item" href="#" (click)="switchDataSource('Stats2023')">
+ <a
+ class="dropdown-item"
+ href="#"
+ (click)="switchDataSource('Stats2023')"
+ id="stats_source_dropdown"
+ >
Stats
</a>
</li>
@@ -25,6 +35,7 @@
class="dropdown-item"
href="#"
(click)="switchDataSource('DriverRanking')"
+ id="driver_ranking_source_dropdown"
>
Driver Ranking
</a>
@@ -64,7 +75,7 @@
<td>{{parseKeywords(note)}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button class="btn btn-danger" (click)="deleteNoteData()">
<i class="bi bi-trash"></i>
</button>
</td>
@@ -87,20 +98,24 @@
</div>
</th>
<th scope="col">Team</th>
- <th scope="col">Set</th>
<th scope="col">Comp Level</th>
+ <th scope="col">Scout</th>
<th scope="col"></th>
</tr>
</thead>
<tbody>
<tr *ngFor="let stat2023 of statList; index as i;">
- <th scope="row">{{stat2023.match()}}</th>
- <td>{{stat2023.team()}}</td>
- <td>{{stat2023.setNumber()}}</td>
+ <th scope="row">{{stat2023.matchNumber()}}</th>
+ <td>{{stat2023.teamNumber()}}</td>
<td>{{COMP_LEVEL_LABELS[stat2023.compLevel()]}}</td>
+ <td>{{stat2023.collectedBy()}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button
+ class="btn btn-danger"
+ id="delete_button_{{i}}"
+ (click)="deleteDataScouting(stat2023.compLevel(), stat2023.matchNumber(), stat2023.setNumber(), stat2023.teamNumber())"
+ >
<i class="bi bi-trash"></i>
</button>
</td>
@@ -136,7 +151,7 @@
<td>{{ranking.rank3()}}</td>
<!-- Delete Icon. -->
<td>
- <button class="btn btn-danger" (click)="deleteData()">
+ <button class="btn btn-danger" (click)="deleteDriverRankingData()">
<i class="bi bi-trash"></i>
</button>
</td>
diff --git a/third_party/flatbuffers/src/util.cpp b/third_party/flatbuffers/src/util.cpp
index aabc23a..d3cf0d8 100644
--- a/third_party/flatbuffers/src/util.cpp
+++ b/third_party/flatbuffers/src/util.cpp
@@ -212,9 +212,16 @@
return g_file_exists_function(name);
}
-bool DirExists(const char *name) {
- // clang-format off
+#ifdef __clang__
+#define NO_MSAN_ATTRIBUTE __attribute__((no_sanitize("memory")))
+#else
+#define NO_MSAN_ATTRIBUTE
+#endif
+// For no obvious reason, clang's sanitizer thinks that the mode bits from
+// stat() are uninitialized in some circumstances.
+bool DirExists(const char *name) NO_MSAN_ATTRIBUTE {
+ // clang-format off
#ifdef _WIN32
#define flatbuffers_stat _stat
#define FLATBUFFERS_S_IFDIR _S_IFDIR
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 2d83482..6e1cc7f 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -71,6 +71,8 @@
"//frc971:constants",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:hall_effect_and_position",
+ "//frc971/zeroing:pulse_index",
"//frc971/zeroing:wrap",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 9acd190..b8a12f7 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -11,6 +11,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/profiled_subsystem.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pulse_index.h"
#include "y2017/control_loops/superstructure/column/column_integral_plant.h"
#include "y2017/control_loops/superstructure/column/stuck_column_integral_plant.h"
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
index a7fe47b..c07a504 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.h
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
#include "frc971/constants.h"
+#include "frc971/zeroing/hall_effect_and_position.h"
#include "frc971/zeroing/zeroing.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 8f162a7..9edcc8e 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -43,6 +43,7 @@
deps = [
":hood_plants",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/zeroing:pulse_index",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_goal_fbs",
"//y2017/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 8284e6e..47ace6e 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/zeroing/pulse_index.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 550c241..00642b0 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -43,6 +43,7 @@
deps = [
":intake_plants",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2017:constants",
"//y2017/control_loops/superstructure:superstructure_goal_fbs",
],
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index a91d2f0..29899fb 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -2,6 +2,7 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
#include "frc971/control_loops/profiled_subsystem.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 9f969ff..ec18f9d 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -15,6 +15,7 @@
"//frc971/control_loops/double_jointed_arm:graph",
"//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
"//y2018/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 0b0a6a4..a9cf614 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index fa4eb7e..50d9989 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -46,6 +46,7 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_output_fbs",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 09a7e4d..bec5ff6 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -5,6 +5,7 @@
#include "aos/commonmath.h"
#include "frc971/control_loops/control_loop.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/wrap.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
diff --git a/y2019/BUILD b/y2019/BUILD
index 2fba6d6..d388819 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -41,6 +41,8 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/control_loops/drivetrain:camera",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 10582ed..380899a 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -12,6 +12,8 @@
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/control_loops/superstructure/elevator/integral_elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
diff --git a/y2019/constants.h b/y2019/constants.h
index b6c1b55..a36e4b3 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -9,6 +9,8 @@
#include "frc971/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 80e9fe6..2ac689b 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -188,14 +188,14 @@
shard_count = 8,
target_compatible_with = ["@platforms//os:linux"],
deps = [
- ":localizer",
":drivetrain_base",
+ ":localizer",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"//aos/testing:test_shm",
+ "//frc971/control_loops/drivetrain:splinedrivetrain",
"//frc971/control_loops/drivetrain:trajectory",
"//y2019:constants",
- "//frc971/control_loops/drivetrain:splinedrivetrain",
"@com_github_gflags_gflags//:gflags",
] + cpu_select({
"amd64": [
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 9504c9f..f97c6f0 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -69,6 +69,8 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2019:constants",
"//y2019:status_light_fbs",
],
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index f8ad0fd..4d59132 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -5,6 +5,8 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2020/BUILD b/y2020/BUILD
index 32d392a..7c04d99 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -77,6 +77,9 @@
"//frc971:constants",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/control_panel:control_panel_plants",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 4d9d066..6218f9b 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,6 +12,9 @@
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.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"
diff --git a/y2020/constants.h b/y2020/constants.h
index 83b2cec..7332b4f 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -8,6 +8,9 @@
#include "frc971/constants.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.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"
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d1f20a2..83af834 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -83,6 +83,9 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2020:constants",
"//y2020/control_loops/superstructure/hood:hood_encoder_zeroing_estimator",
"//y2020/control_loops/superstructure/shooter",
diff --git a/y2020/control_loops/superstructure/hood/BUILD b/y2020/control_loops/superstructure/hood/BUILD
index 437c67a..7983e62 100644
--- a/y2020/control_loops/superstructure/hood/BUILD
+++ b/y2020/control_loops/superstructure/hood/BUILD
@@ -44,6 +44,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
"//frc971/zeroing",
+ "//frc971/zeroing:absolute_and_absolute_encoder",
"//y2020:constants",
],
)
diff --git a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
index dee4461..249dd00 100644
--- a/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
+++ b/y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.cc
@@ -2,6 +2,8 @@
#include <cmath>
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
namespace y2020::control_loops::superstructure::hood {
HoodEncoderZeroingEstimator::HoodEncoderZeroingEstimator(
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 5d371fc..8a00bf0 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -5,6 +5,9 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/input/joystick_state_generated.h"
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/hood/hood_encoder_zeroing_estimator.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
diff --git a/y2022/BUILD b/y2022/BUILD
index f25925e..8d9d901 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -224,6 +224,7 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
"//frc971/wpilib:wpilib_utils",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index ff1c738..8ba3367 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -13,6 +13,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "frc971/wpilib/wpilib_utils.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
diff --git a/y2022/constants.h b/y2022/constants.h
index 62b9b4a..f27b6cd 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/climber_plant.h"
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e0db64c..e370f0e 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -86,6 +86,7 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022:constants",
"//y2022/control_loops/superstructure/catapult",
"//y2022/control_loops/superstructure/turret:aiming",
diff --git a/y2022/control_loops/superstructure/catapult/BUILD b/y2022/control_loops/superstructure/catapult/BUILD
index a43925e..36e34bd 100644
--- a/y2022/control_loops/superstructure/catapult/BUILD
+++ b/y2022/control_loops/superstructure/catapult/BUILD
@@ -41,6 +41,7 @@
deps = [
":catapult_plants",
"//aos:realtime",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//third_party/osqp-cpp",
"//y2022:constants",
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index a4c82de..d30e8f5 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -5,6 +5,7 @@
#include "glog/logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "osqp++.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 5e3415c..36269e9 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/catapult/catapult.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
diff --git a/y2022_bot3/BUILD b/y2022_bot3/BUILD
index 29debe7..125ff44 100644
--- a/y2022_bot3/BUILD
+++ b/y2022_bot3/BUILD
@@ -116,6 +116,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022_bot3/control_loops/drivetrain:polydrivetrain_plants",
"//y2022_bot3/control_loops/superstructure/climber:climber_plants",
"//y2022_bot3/control_loops/superstructure/intake:intake_plants",
diff --git a/y2022_bot3/constants.cc b/y2022_bot3/constants.cc
index 7ec5187..82982aa 100644
--- a/y2022_bot3/constants.cc
+++ b/y2022_bot3/constants.cc
@@ -12,6 +12,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/control_loops/superstructure/climber/integral_climber_plant.h"
#include "y2022_bot3/control_loops/superstructure/intake/integral_intake_plant.h"
diff --git a/y2022_bot3/constants.h b/y2022_bot3/constants.h
index 2ced38d..a87930f 100644
--- a/y2022_bot3/constants.h
+++ b/y2022_bot3/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2022_bot3/control_loops/superstructure/climber/climber_plant.h"
#include "y2022_bot3/control_loops/superstructure/intake/intake_plant.h"
diff --git a/y2022_bot3/control_loops/superstructure/BUILD b/y2022_bot3/control_loops/superstructure/BUILD
index 7d89a0a..5b44e2a 100644
--- a/y2022_bot3/control_loops/superstructure/BUILD
+++ b/y2022_bot3/control_loops/superstructure/BUILD
@@ -75,6 +75,7 @@
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2022_bot3:constants",
],
)
diff --git a/y2022_bot3/control_loops/superstructure/superstructure.h b/y2022_bot3/control_loops/superstructure/superstructure.h
index 4f33c3c..13d5dec 100644
--- a/y2022_bot3/control_loops/superstructure/superstructure.h
+++ b/y2022_bot3/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2022_bot3/constants.h"
#include "y2022_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022_bot3/control_loops/superstructure/superstructure_output_generated.h"
diff --git a/y2023/BUILD b/y2023/BUILD
index e200f87..d934927 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -254,6 +254,8 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023/control_loops/drivetrain:polydrivetrain_plants",
"//y2023/control_loops/superstructure/arm:arm_constants",
"//y2023/control_loops/superstructure/roll:roll_plants",
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index bedbab7..8891223 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -15,6 +15,7 @@
DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
DEFINE_bool(charged_up, true, "If true run charged up autonomous mode");
DEFINE_bool(charged_up_cable, false, "If true run cable side autonomous mode");
+DEFINE_bool(do_balance, true, "If true run the balance.");
namespace y2023 {
namespace autonomous {
@@ -364,6 +365,7 @@
AOS_LOG(
INFO, "Placed second cube %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
InitializeEncoders();
const ProfileParametersT kDrive = MakeProfileParameters(2.0, 4.0);
@@ -381,6 +383,11 @@
INFO, "Done backing up %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ if (!FLAGS_do_balance) {
+ StopSpitting();
+ return;
+ }
+
const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.7, 8.0);
StartDrive(0.0, aos::math::NormalizeAngle(M_PI / 2.0 - Theta()), kDrive,
kInPlaceTurn);
diff --git a/y2023/autonomous/splines/spline.1.json b/y2023/autonomous/splines/spline.1.json
index 5240ad8..ae80ebe 100644
--- a/y2023/autonomous/splines/spline.1.json
+++ b/y2023/autonomous/splines/spline.1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [1.609310857796625, 2.5819120488556946, 3.506443404506549, 5.555694235956169, 5.989575501273337, 6.418880858416194], "spline_y": [0.6043502546533336, 0.69141924611354, 1.0213742193777775, 0.38712949808092717, 0.40845524312381665, 0.40845524312381665], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [1.609310857796625, 2.5819120488556946, 3.506443404506549, 5.571740737699863, 6.005622003017031, 6.434927360159888], "spline_y": [0.6043502546533336, 0.69141924611354, 1.0213742193777775, 0.47692519455739807, 0.49825093960028755, 0.49825093960028755], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline.2.json b/y2023/autonomous/splines/spline.2.json
index 297966c..4afc161 100644
--- a/y2023/autonomous/splines/spline.2.json
+++ b/y2023/autonomous/splines/spline.2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [6.418880858416194, 6.02775087044969, 5.275605833281384, 2.8053451665928835, 2.37026593061867, 1.5260719060059573], "spline_y": [0.40845524312381665, 0.40845524312381665, 0.4647376584428905, 1.3287637699876067, -0.026954142728124464, -0.5547144640218522], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.5}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [6.434927360159888, 6.043797372193384, 5.291652335025078, 2.8053451665928835, 2.37026593061867, 1.5260719060059573], "spline_y": [0.49825093960028755, 0.49825093960028755, 0.5545333549193614, 1.3287637699876067, -0.026954142728124464, -0.5547144640218522], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.5}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 5ab6407..56be337 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -84,31 +84,34 @@
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.911194143585562;
+ arm_proximal->zeroing.measured_absolute_position = 0.911747959388894;
arm_proximal->potentiometer_offset =
10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
- 0.167359305216504 + 0.135144500925909 - 0.214909475332252;
+ 0.167359305216504 + 0.135144500925909 - 0.214909475332252 +
+ 0.0377032255050543;
- arm_distal->zeroing.measured_absolute_position = 0.295329750530428;
+ arm_distal->zeroing.measured_absolute_position = 0.294291930885304;
arm_distal->potentiometer_offset =
7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
0.0143810684138064 + 0.00945555248207735 + 0.452446388633863 +
0.0194863477007102 + 0.235993332670562 + 0.00138417783482921 -
- 1.29562640607084;
+ 1.29562640607084 - 0.390356125757262 - 0.267002511437832 -
+ 0.611626839639182 + 2.55745730136924 + 0.503121678457021 +
+ 0.0440779746883177;
arm_distal->zeroing.one_revolution_distance =
M_PI * 2.0 * constants::Values::kDistalEncoderRatio() *
(3.12725165289659 + 0.002) / 3.1485739705977704;
- roll_joint->zeroing.measured_absolute_position = 1.79390317510529;
+ roll_joint->zeroing.measured_absolute_position = 1.82824749141201;
roll_joint->potentiometer_offset =
0.624713611895747 + 3.10458504917251 - 0.0966407797407789 +
0.0257708772364788 - 0.0395076737853459 - 6.87914956118006 -
0.097581301615046 + 3.3424421683095 - 3.97605190912604 +
- 0.709274294168941;
+ 0.709274294168941 - 0.0817908884966825;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 2.97717660361257;
+ 0.744036527649413;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index 32ed317..84d6499 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -8,6 +8,8 @@
#include "frc971/constants.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/arm/arm_constants.h"
#include "y2023/control_loops/superstructure/roll/roll_plant.h"
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 3c3ba38..fc21b74 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -4,13 +4,13 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-04-15.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json' %}
}
],
"robot": {
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index eb92ec8..52db9d8 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -231,7 +231,7 @@
))
points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.28, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.30, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -246,7 +246,7 @@
))
points['GroundPickupFrontCube'] = to_theta_with_circular_index_and_roll(
- 0.325603, 0.255189, np.pi / 2.0, circular_index=0)
+ 0.325603, 0.275189, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -261,7 +261,7 @@
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
- -1.45013, 1.00354, np.pi / 2.0, circular_index=1)
+ -1.45013, 1.02354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -333,7 +333,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1200539, 1.335, np.pi / 2.0, circular_index=0)
+ -1.1200539, 1.330, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -346,7 +346,7 @@
))
points['HPPickupFrontConeUp'] = np.array(
- (5.16514378449353, 1.25, -np.pi / 2.0))
+ (5.16514378449353, 1.2461538461538462, -np.pi / 2.0))
# to_theta_with_circular_index_and_roll(
# 0.265749, 1.28332, -np.pi / 2.0, circular_index=1)
@@ -476,7 +476,7 @@
))
points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.3712121, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.4012121, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -489,7 +489,7 @@
))
points['ScoreBackMidCube'] = to_theta_with_circular_index_and_roll(
- -1.27896, 0.84, -np.pi / 2.0, circular_index=1)
+ -1.27896, 0.89, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -515,7 +515,7 @@
#points['ScoreBackHighCube'] = to_theta_with_circular_index_and_roll(
# -1.60932, 1.16839, np.pi / 2.0, circular_index=0)
points['ScoreBackHighCube'] = np.array(
- (4.77284735761704, -1.19952193130714, -np.pi / 2.0))
+ (4.77284735761704, -1.130291162076371, -np.pi / 2.0))
named_segments.append(
ThetaSplineSegment(
@@ -538,7 +538,7 @@
))
points['GroundPickupFrontConeUp'] = to_theta_with_circular_index_and_roll(
- 0.313099, 0.380, -np.pi / 2.0, circular_index=0)
+ 0.313099, 0.39, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 861bbf8..3d35dae 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -106,6 +106,8 @@
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023:constants",
"//y2023/constants:constants_fbs",
"//y2023/constants:simulated_constants_sender",
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index a742c50..768a54e 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -16,6 +16,7 @@
"//frc971/control_loops/double_jointed_arm:graph",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/zeroing",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023:constants",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 1f97d80..6153400 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "frc971/zeroing/zeroing.h"
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/arm/generated_graph.h"
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 8628359..43f3d24 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -36,6 +36,9 @@
state_ = EndEffectorState::LOADED;
break;
case EndEffectorState::LOADED:
+ // In case we thought we had a cube, force it to cone.
+ game_piece_ = vision::Class::CONE_UP;
+ break;
case EndEffectorState::SPITTING:
break;
}
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index fdfef4e..bcee3ea 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -7,6 +7,8 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023/constants.h"
#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 26b88f3..926846e 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -9,6 +9,7 @@
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/subsystem_simulator.h"
#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/absolute_encoder.h"
#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
diff --git a/y2023/control_loops/superstructure/superstructure_plotter.ts b/y2023/control_loops/superstructure/superstructure_plotter.ts
index 2e32445..c7d14fd 100644
--- a/y2023/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2023/control_loops/superstructure/superstructure_plotter.ts
@@ -11,15 +11,15 @@
export function plotSuperstructure(conn: Connection, element: Element): void {
const aosPlotter = new AosPlotter(conn);
- const goal = aosPlotter.addMessageSource(
- '/superstructure', 'y2023.control_loops.superstructure.Goal');
- const output = aosPlotter.addMessageSource(
- '/superstructure', 'y2023.control_loops.superstructure.Output');
- const status = aosPlotter.addMessageSource(
- '/superstructure', 'y2023.control_loops.superstructure.Status');
+ //const goal = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2023.control_loops.superstructure.Goal');
+ //const output = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2023.control_loops.superstructure.Output');
+ //const status = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2023.control_loops.superstructure.Status');
const position = aosPlotter.addMessageSource(
'/superstructure', 'y2023.control_loops.superstructure.Position');
- const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ //const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
const positionPlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
@@ -28,88 +28,13 @@
positionPlot.plot.getAxisLabels().setYLabel('wonky state units');
positionPlot.plot.setDefaultYRange([-1.0, 2.0]);
- positionPlot.addMessageLine(position, ['turret_beambreak'])
+ positionPlot.addMessageLine(position, ['arm', 'distal', 'pot'])
.setColor(RED)
.setPointSize(4.0);
- positionPlot.addMessageLine(status, ['state'])
- .setColor(PINK)
- .setPointSize(4.0);
- positionPlot.addMessageLine(status, ['flippers_open'])
- .setColor(WHITE)
- .setPointSize(1.0);
- positionPlot.addMessageLine(status, ['reseating_in_catapult'])
- .setColor(BLUE)
- .setPointSize(1.0);
- positionPlot.addMessageLine(status, ['fire'])
- .setColor(BROWN)
- .setPointSize(1.0);
- positionPlot.addMessageLine(status, ['ready_to_fire'])
- .setColor(GREEN)
- .setPointSize(1.0);
- positionPlot.addMessageLine(status, ['collided'])
- .setColor(PINK)
- .setPointSize(1.0);
-
- const goalPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- goalPlot.plot.getAxisLabels().setTitle('Goal');
- goalPlot.plot.getAxisLabels().setXLabel(TIME);
- goalPlot.plot.getAxisLabels().setYLabel('value');
- goalPlot.plot.setDefaultYRange([-1.0, 2.0]);
- goalPlot.addMessageLine(goal, ['fire']).setColor(RED).setPointSize(1.0);
- goalPlot.addMessageLine(goal, ['auto_aim']).setColor(BLUE).setPointSize(1.0);
-
-
- const shotCountPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- shotCountPlot.plot.getAxisLabels().setTitle('Shot Count');
- shotCountPlot.plot.getAxisLabels().setXLabel(TIME);
- shotCountPlot.plot.getAxisLabels().setYLabel('balls');
- shotCountPlot.plot.setDefaultYRange([-1.0, 2.0]);
- shotCountPlot.addMessageLine(status, ['shot_count'])
- .setColor(RED)
- .setPointSize(1.0);
-
- const intakePlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- intakePlot.plot.getAxisLabels().setTitle('Intake');
- intakePlot.plot.getAxisLabels().setXLabel(TIME);
- intakePlot.plot.getAxisLabels().setYLabel('wonky state units');
- intakePlot.plot.setDefaultYRange([-1.0, 2.0]);
- intakePlot.addMessageLine(status, ['intake_state'])
- .setColor(RED)
- .setPointSize(1.0);
- intakePlot.addMessageLine(position, ['intake_beambreak_front'])
- .setColor(GREEN)
- .setPointSize(4.0);
- intakePlot.addMessageLine(position, ['intake_beambreak_back'])
- .setColor(PINK)
- .setPointSize(1.0);
- intakePlot.addMessageLine(output, ['transfer_roller_voltage'])
- .setColor(BROWN)
- .setPointSize(3.0);
-
-
- const otherPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- otherPlot.plot.getAxisLabels().setTitle('Position');
- otherPlot.plot.getAxisLabels().setXLabel(TIME);
- otherPlot.plot.getAxisLabels().setYLabel('rad');
- otherPlot.plot.setDefaultYRange([-1.0, 2.0]);
-
- otherPlot.addMessageLine(status, ['catapult', 'position'])
- .setColor(PINK)
- .setPointSize(4.0);
- otherPlot.addMessageLine(status, ['turret', 'position'])
- .setColor(WHITE)
- .setPointSize(4.0);
- otherPlot.addMessageLine(position, ['flipper_arm_left', 'encoder'])
+ positionPlot.addMessageLine(position, ['arm', 'distal', 'absolute_encoder'])
.setColor(BLUE)
.setPointSize(4.0);
- otherPlot.addMessageLine(position, ['flipper_arm_right', 'encoder'])
- .setColor(CYAN)
- .setPointSize(4.0);
- otherPlot.addMessageLine(output, ['flipper_arms_voltage'])
- .setColor(BROWN)
+ positionPlot.addMessageLine(position, ['arm', 'distal', 'encoder'])
+ .setColor(GREEN)
.setPointSize(4.0);
}
diff --git a/y2023/copy_logs.sh b/y2023/copy_logs.sh
new file mode 100755
index 0000000..4d5669c
--- /dev/null
+++ b/y2023/copy_logs.sh
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+# Helper script to copy most recent logs off of the pis
+
+set -e
+
+ROBOT_PREFIX="79" # ..71 (Should be one of 79, 89, 99, or 9)
+PI_LIST="2 3" # Should be some set of {1,2,3,4,5,6}
+
+LOG_FILE_PATH=/media/sda1/fbs_log-current
+if [[ -z $1 || ! -d $1 ]]; then
+ echo "Please specify the base directory to store the logs ('$1' not found)"
+ exit -1
+fi
+
+# Create output directory based on given directory + a timestamp
+OUTPUT_DIR=$1"/"`date +"%Y-%m-%dT%H-%M-%S"`
+mkdir ${OUTPUT_DIR}
+
+echo "Copying logs from the robot ${ROBOT_PREFIX}71 and pis ${PI_LIST}"
+echo "Storing logs in folder ${OUTPUT_DIR}"
+
+for pi in $PI_LIST; do
+ echo "========================================================"
+ echo "Copying logs from pi-${ROBOT_PREFIX}71-$pi"
+ echo "========================================================"
+ scp -r pi@10.${ROBOT_PREFIX}.71.10${pi}:${LOG_FILE_PATH} ${OUTPUT_DIR}/fbs_log-pi${pi}
+done
+
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 874e691..f4d9ea6 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -186,7 +186,7 @@
},
{
.index = arm::ScoreBackMidConeUpIndex(),
- .wrist_goal = kConeWrist,
+ .wrist_goal = kConeWrist + 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
{kMidConeScoreLeft, SpotSelectionHint::LEFT}},
diff --git a/y2023/pi_send_joystick.sh b/y2023/pi_send_joystick.sh
new file mode 100755
index 0000000..9b60e02
--- /dev/null
+++ b/y2023/pi_send_joystick.sh
@@ -0,0 +1,18 @@
+#!/usr/bin/sh
+
+# Helper script to spoof joystick state of robot enabled, to triggger image logging
+
+# Currently, this is going through pi6. Need to set the right IP address for the bot
+imu_pi6="pi@10.79.71.106"
+
+# TODO(milind): add logger in the future
+echo "Sending Joystick command '$1' to $imu_pi6"
+ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+
+if [ $1 = "false" ]
+then
+ # This extra sleep is necessary to make sure the logs rotate to a new file
+ sleep 6
+ echo "Sending Joystick command '$1' to $imu_pi6"
+ ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+fi
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json
index ad8ab23..051897b 100644
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json
+++ b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-09_ext_2023-03-05.json
@@ -1 +1 @@
-{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.191149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
+{ "node_name": "pi1", "team_number": 971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.231149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json
new file mode 100644
index 0000000..f353533
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-10_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 971, "intrinsics": [ 894.002502, 0.0, 636.431335, 0.0, 893.723816, 377.069672, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.84372, 0.233067, 0.483545, 0.188786, 0.499721,-0.0121191, -0.866102, -0.244606, -0.196001, 0.972385, -0.126693, 0.600451, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.446659, 0.244189, 0.000632, 0.000171, -0.074849 ], "calibration_timestamp": 1358503360377380613, "camera_id": "23-10" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json
new file mode 100644
index 0000000..d7851fa
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-11_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.484157, -0.154776, -0.861182, -0.0935153, -0.872577, -0.0125032, -0.488317, -0.225918, 0.0648126, 0.987871, -0.141107, 0.623926, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json
new file mode 100644
index 0000000..e12c22c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-12_ext_2023-09-23.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.864114, -0.173035, -0.472616, -0.111428, -0.477471, -0.0150962, 0.878518, -0.22396, -0.159149, 0.9848, -0.0695749, 0.696406, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
diff --git a/y2023_bot4/BUILD b/y2023_bot4/BUILD
new file mode 100644
index 0000000..1a4ed64
--- /dev/null
+++ b/y2023_bot4/BUILD
@@ -0,0 +1,234 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+
+config_validator_test(
+ name = "config_validator_test",
+ config = "//y2023_bot4:aos_config",
+)
+
+robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//aos/events:aos_timing_report_streamer",
+ ],
+ data = [
+ ":aos_config",
+ ":swerve_publisher_output_json",
+ "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ ":wpilib_interface",
+ ":swerve_publisher",
+ "//frc971/can_logger",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "pi_download",
+ binaries = [
+ "//aos/util:foxglove_websocket",
+ "//aos/events:aos_timing_report_streamer",
+ "//aos/events/logging:log_cat",
+ "//y2023/rockpi:imu_main",
+ "//frc971/image_streamer:image_streamer",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//aos/events/logging:logger_main",
+ ],
+ data = [
+ ":aos_config",
+ "//frc971/rockpi:rockpi_config.json",
+ ],
+ start_binaries = [
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "pi",
+)
+
+filegroup(
+ name = "swerve_publisher_output_json",
+ srcs = [
+ "swerve_drivetrain_output.json",
+ ],
+ visibility = ["//y2023_bot4:__subpackages__"],
+)
+
+cc_library(
+ name = "constants",
+ srcs = ["constants.cc"],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network:team_number",
+ "//frc971:constants",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ gen_reflections = 1,
+ deps = ["//frc971/control_loops:control_loops_fbs"],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_can_position_fbs",
+ srcs = ["drivetrain_can_position.fbs"],
+ gen_reflections = 1,
+ deps = ["//frc971/control_loops:can_falcon_fbs"],
+)
+
+cc_binary(
+ name = "swerve_publisher",
+ srcs = ["swerve_publisher_main.cc"],
+ deps = [
+ ":swerve_publisher_lib",
+ "//aos/events:shm_event_loop",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
+
+cc_library(
+ name = "swerve_publisher_lib",
+ srcs = ["swerve_publisher_lib.cc"],
+ hdrs = ["swerve_publisher_lib.h"],
+ deps = [
+ "//aos:init",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_test(
+ name = "swerve_publisher_lib_test",
+ srcs = [
+ "swerve_publisher_lib_test.cc",
+ ],
+ data = [
+ ":aos_config",
+ ":swerve_publisher_output_json",
+ ],
+ deps = [
+ ":swerve_publisher_lib",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_binary(
+ name = "wpilib_interface",
+ srcs = ["wpilib_interface.cc"],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":constants",
+ ":drivetrain_can_position_fbs",
+ ":drivetrain_position_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/wpilib:can_sensor_reader",
+ "//frc971/wpilib:falcon",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//frc971/wpilib/swerve:swerve_drivetrain_writer",
+ ],
+)
+
+aos_config(
+ name = "aos_config",
+ src = "y2023_bot4.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_imu",
+ ":config_logger",
+ ":config_roborio",
+ ],
+)
+
+aos_config(
+ name = "config_roborio",
+ src = "y2023_bot4_roborio.json",
+ flatbuffers = [
+ ":drivetrain_position_fbs",
+ ":drivetrain_can_position_fbs",
+ "//frc971:can_configuration_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain/swerve:swerve_drivetrain_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/autonomous:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ "//frc971/wpilib:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_imu",
+ src = "y2023_bot4_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_logger",
+ src = "y2023_bot4_logger.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ ],
+)
diff --git a/y2023_bot4/constants.cc b/y2023_bot4/constants.cc
new file mode 100644
index 0000000..aab1935
--- /dev/null
+++ b/y2023_bot4/constants.cc
@@ -0,0 +1,52 @@
+#include "y2023_bot4/constants.h"
+
+#include <cstdint>
+
+#include "glog/logging.h"
+
+#include "aos/network/team_number.h"
+
+namespace y2023_bot4 {
+namespace constants {
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+ Values r;
+ auto *const front_left_zeroing_constants = &r.front_left_zeroing_constants;
+ auto *const front_right_zeroing_constants = &r.front_right_zeroing_constants;
+ auto *const back_left_zeroing_constants = &r.back_left_zeroing_constants;
+ auto *const back_right_zeroing_constants = &r.back_right_zeroing_constants;
+
+ front_left_zeroing_constants->average_filter_size = 0;
+ front_left_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ front_left_zeroing_constants->measured_absolute_position = 0.76761395509829;
+ front_left_zeroing_constants->zeroing_threshold = 0.0;
+ front_left_zeroing_constants->moving_buffer_size = 0.0;
+ front_left_zeroing_constants->allowable_encoder_error = 0.0;
+
+ front_right_zeroing_constants->average_filter_size = 0;
+ front_right_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ front_right_zeroing_constants->measured_absolute_position = 0.779403958443922;
+ front_right_zeroing_constants->zeroing_threshold = 0.0;
+ front_right_zeroing_constants->moving_buffer_size = 0.0;
+ front_right_zeroing_constants->allowable_encoder_error = 0.0;
+
+ back_left_zeroing_constants->average_filter_size = 0;
+ back_left_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ back_left_zeroing_constants->measured_absolute_position = 0.053439698061417;
+ back_left_zeroing_constants->zeroing_threshold = 0.0;
+ back_left_zeroing_constants->moving_buffer_size = 0.0;
+ back_left_zeroing_constants->allowable_encoder_error = 0.0;
+
+ back_right_zeroing_constants->average_filter_size = 0;
+ back_right_zeroing_constants->one_revolution_distance = 2 * M_PI;
+ back_right_zeroing_constants->measured_absolute_position = 0.719329333121509;
+ back_right_zeroing_constants->zeroing_threshold = 0.0;
+ back_right_zeroing_constants->moving_buffer_size = 0.0;
+ back_right_zeroing_constants->allowable_encoder_error = 0.0;
+
+ return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+} // namespace constants
+} // namespace y2023_bot4
diff --git a/y2023_bot4/constants.h b/y2023_bot4/constants.h
new file mode 100644
index 0000000..676ae06
--- /dev/null
+++ b/y2023_bot4/constants.h
@@ -0,0 +1,55 @@
+#ifndef Y2023_BOT4_CONSTANTS_H
+#define Y2023_BOT4_CONSTANTS_H
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#include <cstdint>
+
+#include "frc971/constants.h"
+
+namespace y2023_bot4 {
+namespace constants {
+struct Values {
+ static const int kZeroingSampleSize = 200;
+
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
+ // TODO (maxwell): Make this the real value;
+ static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
+
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 35.0; }
+ static constexpr double kDrivetrainSupplyCurrentLimit() { return 60.0; }
+
+ // TODO (maxwell): Make this the real value
+ static constexpr double kFollowerWheelCountsPerRevolution() { return 512.0; }
+ static constexpr double kFollowerWheelEncoderRatio() { return 1.0; }
+ static constexpr double kFollowerWheelRadius() { return 3.25 / 2 * 0.0254; }
+ static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+ return 2048.0;
+ }
+
+ static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+ return 1200000;
+ }
+
+ frc971::constants::ContinuousAbsoluteEncoderZeroingConstants
+ front_left_zeroing_constants,
+ front_right_zeroing_constants, back_left_zeroing_constants,
+ back_right_zeroing_constants;
+};
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
+} // namespace constants
+} // namespace y2023_bot4
+
+#endif // Y2023_BOT4_CONSTANTS_H
diff --git a/y2023_bot4/drivetrain_can_position.fbs b/y2023_bot4/drivetrain_can_position.fbs
new file mode 100644
index 0000000..e8c1235
--- /dev/null
+++ b/y2023_bot4/drivetrain_can_position.fbs
@@ -0,0 +1,17 @@
+include "frc971/control_loops/can_falcon.fbs";
+namespace y2023_bot4;
+
+table SwerveModuleCANPosition {
+ rotation: frc971.control_loops.CANFalcon (id: 0);
+ translation: frc971.control_loops.CANFalcon (id: 1);
+}
+
+// CAN Readings from the CAN sensor reader loop for each swerve module
+table AbsoluteCANPosition {
+ front_left: SwerveModuleCANPosition (id: 0);
+ front_right: SwerveModuleCANPosition (id: 1);
+ back_left: SwerveModuleCANPosition (id: 2);
+ back_right: SwerveModuleCANPosition (id: 3);
+}
+
+root_type AbsoluteCANPosition;
diff --git a/y2023_bot4/drivetrain_position.fbs b/y2023_bot4/drivetrain_position.fbs
new file mode 100644
index 0000000..e5d0fc3
--- /dev/null
+++ b/y2023_bot4/drivetrain_position.fbs
@@ -0,0 +1,16 @@
+include "frc971/control_loops/control_loops.fbs";
+namespace y2023_bot4;
+
+table AbsoluteDrivetrainPosition {
+ // Position of the follower wheels from the encoders
+ follower_wheel_one_position:double (id: 0);
+ follower_wheel_two_position:double (id: 1);
+
+ // Position from the mag encoder on each module.
+ front_left_position: frc971.AbsolutePosition (id: 2);
+ front_right_position: frc971.AbsolutePosition (id: 3);
+ back_left_position: frc971.AbsolutePosition (id: 4);
+ back_right_position: frc971.AbsolutePosition (id: 5);
+}
+
+root_type AbsoluteDrivetrainPosition;
diff --git a/y2023_bot4/swerve_drivetrain_output.json b/y2023_bot4/swerve_drivetrain_output.json
new file mode 100644
index 0000000..bc152e2
--- /dev/null
+++ b/y2023_bot4/swerve_drivetrain_output.json
@@ -0,0 +1,18 @@
+{
+ "front_left_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "front_right_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "back_left_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ },
+ "back_right_output": {
+ "rotation_current": 0.0,
+ "translation_current": 0.0
+ }
+}
diff --git a/y2023_bot4/swerve_publisher_lib.cc b/y2023_bot4/swerve_publisher_lib.cc
new file mode 100644
index 0000000..78a778e
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib.cc
@@ -0,0 +1,51 @@
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+y2023_bot4::SwervePublisher::SwervePublisher(aos::EventLoop *event_loop,
+ aos::ExitHandle *exit_handle,
+ const std::string &filename,
+ double duration)
+ : drivetrain_output_sender_(
+ event_loop->MakeSender<drivetrain::swerve::Output>("/drivetrain")) {
+ event_loop
+ ->AddTimer([this, filename]() {
+ auto output_builder = drivetrain_output_sender_.MakeBuilder();
+
+ auto drivetrain_output =
+ aos::JsonFileToFlatbuffer<drivetrain::swerve::Output>(filename);
+
+ auto copied_flatbuffer =
+ aos::CopyFlatBuffer<drivetrain::swerve::Output>(
+ drivetrain_output, output_builder.fbb());
+ CHECK(drivetrain_output.Verify());
+
+ output_builder.CheckOk(output_builder.Send(copied_flatbuffer));
+ })
+ ->Schedule(event_loop->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop
+ ->AddTimer([this, exit_handle]() {
+ auto builder = drivetrain_output_sender_.MakeBuilder();
+ drivetrain::swerve::SwerveModuleOutput::Builder swerve_module_builder =
+ builder.MakeBuilder<drivetrain::swerve::SwerveModuleOutput>();
+
+ swerve_module_builder.add_rotation_current(0.0);
+ swerve_module_builder.add_translation_current(0.0);
+
+ auto swerve_module_offset = swerve_module_builder.Finish();
+
+ drivetrain::swerve::Output::Builder drivetrain_output_builder =
+ builder.MakeBuilder<drivetrain::swerve::Output>();
+
+ drivetrain_output_builder.add_front_left_output(swerve_module_offset);
+ drivetrain_output_builder.add_front_right_output(swerve_module_offset);
+ drivetrain_output_builder.add_back_left_output(swerve_module_offset);
+ drivetrain_output_builder.add_back_right_output(swerve_module_offset);
+
+ builder.CheckOk(builder.Send(drivetrain_output_builder.Finish()));
+
+ exit_handle->Exit();
+ })
+ ->Schedule(event_loop->monotonic_now() +
+ std::chrono::milliseconds((int)duration));
+}
diff --git a/y2023_bot4/swerve_publisher_lib.h b/y2023_bot4/swerve_publisher_lib.h
new file mode 100644
index 0000000..5969b7b
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib.h
@@ -0,0 +1,29 @@
+#ifndef Y2023_BOT4_SWERVE_PUBLISHER_H_
+#define Y2023_BOT4_SWERVE_PUBLISHER_H_
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/control_loops/drivetrain/swerve/swerve_drivetrain_output_generated.h"
+
+namespace y2023_bot4 {
+
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+class SwervePublisher {
+ public:
+ SwervePublisher(aos::EventLoop *event_loop, aos::ExitHandle *exit_handle,
+ const std::string &filename, double duration);
+
+ private:
+ aos::Sender<frc971::control_loops::drivetrain::swerve::Output>
+ drivetrain_output_sender_;
+};
+
+} // namespace y2023_bot4
+
+#endif // Y2023_BOT4_SWERVE_PUBLISHER_H_
diff --git a/y2023_bot4/swerve_publisher_lib_test.cc b/y2023_bot4/swerve_publisher_lib_test.cc
new file mode 100644
index 0000000..817f295
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_lib_test.cc
@@ -0,0 +1,54 @@
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+
+namespace y2023_bot4 {
+namespace testing {
+class SwervePublisherTest : public ::testing::Test {
+ public:
+ SwervePublisherTest()
+ : config_(aos::configuration::ReadConfig("y2023_bot4/aos_config.json")),
+ event_loop_factory_(&config_.message()),
+ roborio_(aos::configuration::GetNode(
+ event_loop_factory_.configuration(), "roborio")),
+ event_loop_(
+ event_loop_factory_.MakeEventLoop("swerve_publisher", roborio_)),
+ exit_handle_(event_loop_factory_.MakeExitHandle()),
+ drivetrain_swerve_output_fetcher_(
+ event_loop_->MakeFetcher<
+ frc971::control_loops::drivetrain::swerve::Output>(
+ "/drivetrain")),
+ swerve_publisher_(event_loop_.get(), exit_handle_.get(),
+ "y2023_bot4/swerve_drivetrain_output.json", 100) {}
+
+ void SendOutput() { event_loop_factory_.Run(); }
+
+ void CheckOutput() {
+ drivetrain_swerve_output_fetcher_.Fetch();
+
+ ASSERT_TRUE(drivetrain_swerve_output_fetcher_.get() != nullptr)
+ << ": No drivetrain output";
+ }
+
+ private:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_;
+
+ std::unique_ptr<aos::EventLoop> event_loop_;
+ std::unique_ptr<aos::ExitHandle> exit_handle_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::swerve::Output>
+ drivetrain_swerve_output_fetcher_;
+
+ y2023_bot4::SwervePublisher swerve_publisher_;
+};
+
+TEST_F(SwervePublisherTest, CheckSentFb) {
+ SendOutput();
+ CheckOutput();
+}
+} // namespace testing
+} // namespace y2023_bot4
diff --git a/y2023_bot4/swerve_publisher_main.cc b/y2023_bot4/swerve_publisher_main.cc
new file mode 100644
index 0000000..87470d7
--- /dev/null
+++ b/y2023_bot4/swerve_publisher_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "y2023_bot4/swerve_publisher_lib.h"
+
+DEFINE_double(duration, 100.0, "Length of time in Ms to apply current for");
+DEFINE_string(drivetrain_position, "swerve_drivetrain_output.json",
+ "The path to the json drivetrain position to apply");
+DEFINE_string(config, "aos_config.json", "The path to aos_config.json");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
+
+ y2023_bot4::SwervePublisher publisher(&event_loop, exit_handle.get(),
+ FLAGS_drivetrain_position,
+ FLAGS_duration);
+
+ event_loop.Run();
+}
diff --git a/y2023_bot4/wpilib_interface.cc b/y2023_bot4/wpilib_interface.cc
new file mode 100644
index 0000000..a744ae0
--- /dev/null
+++ b/y2023_bot4/wpilib_interface.cc
@@ -0,0 +1,325 @@
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+#include "ctre/phoenix6/TalonFX.hpp"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/wpilib/can_sensor_reader.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/swerve/swerve_drivetrain_writer.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2023_bot4/constants.h"
+#include "y2023_bot4/drivetrain_can_position_generated.h"
+#include "y2023_bot4/drivetrain_position_generated.h"
+
+DEFINE_bool(ctre_diag_server, false,
+ "If true, enable the diagnostics server for interacting with "
+ "devices on the CAN bus using Phoenix Tuner");
+
+using frc971::wpilib::CANSensorReader;
+using frc971::wpilib::Falcon;
+using frc971::wpilib::swerve::DrivetrainWriter;
+using frc971::wpilib::swerve::SwerveModule;
+
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+namespace y2023_bot4 {
+namespace wpilib {
+namespace {
+
+template <class T>
+T value_or_exit(std::optional<T> optional) {
+ CHECK(optional.has_value());
+ return optional.value();
+}
+
+flatbuffers::Offset<frc971::AbsolutePosition> module_offset(
+ frc971::AbsolutePosition::Builder builder,
+ frc971::wpilib::AbsoluteEncoder *module) {
+ builder.add_encoder(module->ReadRelativeEncoder());
+ builder.add_absolute_encoder(module->ReadAbsoluteEncoder());
+
+ return builder.Finish();
+}
+
+flatbuffers::Offset<SwerveModuleCANPosition> can_module_offset(
+ SwerveModuleCANPosition::Builder builder,
+ std::shared_ptr<SwerveModule> module) {
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>> rotation_offset =
+ module->rotation->TakeOffset();
+ std::optional<flatbuffers::Offset<control_loops::CANFalcon>>
+ translation_offset = module->translation->TakeOffset();
+
+ CHECK(rotation_offset.has_value());
+ CHECK(translation_offset.has_value());
+
+ builder.add_rotation(rotation_offset.value());
+ builder.add_translation(translation_offset.value());
+
+ return builder.Finish();
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+ constants::Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+} // namespace
+
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader(aos::ShmEventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values)
+ : ::frc971::wpilib::SensorReader(event_loop),
+ values_(values),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<AbsoluteDrivetrainPosition>("/drivetrain")) {
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ }
+
+ void RunIteration() override {
+ {
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+
+ auto front_left_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &front_left_encoder_);
+ auto front_right_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &front_right_encoder_);
+ auto back_left_offset = module_offset(
+ builder.MakeBuilder<frc971::AbsolutePosition>(), &back_left_encoder_);
+ auto back_right_offset =
+ module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
+ &back_right_encoder_);
+
+ AbsoluteDrivetrainPosition::Builder drivetrain_position_builder =
+ builder.MakeBuilder<AbsoluteDrivetrainPosition>();
+
+ drivetrain_position_builder.add_follower_wheel_one_position(
+ follower_wheel_one_encoder_->GetRaw());
+ drivetrain_position_builder.add_follower_wheel_two_position(
+ follower_wheel_two_encoder_->GetRaw());
+
+ drivetrain_position_builder.add_front_left_position(front_left_offset);
+ drivetrain_position_builder.add_front_right_position(front_right_offset);
+ drivetrain_position_builder.add_back_left_position(back_left_offset);
+ drivetrain_position_builder.add_back_right_position(back_right_offset);
+
+ builder.CheckOk(builder.Send(drivetrain_position_builder.Finish()));
+ }
+ }
+
+ void set_follower_wheel_one_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ follower_wheel_one_encoder_ = std::move(encoder);
+ follower_wheel_one_encoder_->SetMaxPeriod(0.005);
+ }
+ void set_follower_wheel_two_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ follower_wheel_two_encoder_ = std::move(encoder);
+ follower_wheel_two_encoder_->SetMaxPeriod(0.005);
+ }
+
+ void set_front_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ front_left_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_front_left_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ front_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_front_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ front_right_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_front_right_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ front_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_back_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ back_left_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_back_left_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ back_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ void set_back_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ back_right_encoder_.set_encoder(std::move(encoder));
+ }
+ void set_back_right_absolute_pwm(
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ back_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ private:
+ std::shared_ptr<const constants::Values> values_;
+
+ aos::Sender<AbsoluteDrivetrainPosition> drivetrain_position_sender_;
+
+ std::unique_ptr<frc::Encoder> follower_wheel_one_encoder_,
+ follower_wheel_two_encoder_;
+
+ frc971::wpilib::AbsoluteEncoder front_left_encoder_, front_right_encoder_,
+ back_left_encoder_, back_right_encoder_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return std::make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+ void Run() override {
+ std::shared_ptr<const constants::Values> values =
+ std::make_shared<const constants::Values>(constants::MakeValues());
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+ std::vector<std::shared_ptr<Falcon>> falcons;
+
+ // TODO(max): Change the CanBus names with TalonFX software.
+ std::shared_ptr<SwerveModule> front_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{6, false},
+ frc971::wpilib::FalconParams{5, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> front_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{3, false},
+ frc971::wpilib::FalconParams{4, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> back_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{8, false},
+ frc971::wpilib::FalconParams{7, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<SwerveModule> back_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::FalconParams{2, false},
+ frc971::wpilib::FalconParams{1, false}, "Drivetrain Bus",
+ &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+
+ // Thread 1
+ aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+ falcons.push_back(front_left->rotation);
+ falcons.push_back(front_left->translation);
+
+ falcons.push_back(front_right->rotation);
+ falcons.push_back(front_right->translation);
+
+ falcons.push_back(back_left->rotation);
+ falcons.push_back(back_left->translation);
+
+ falcons.push_back(back_right->rotation);
+ falcons.push_back(back_right->translation);
+
+ aos::Sender<AbsoluteCANPosition> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<AbsoluteCANPosition>(
+ "/drivetrain");
+
+ CANSensorReader can_sensor_reader(
+ &can_sensor_reader_event_loop, std::move(signals_registry), falcons,
+ [this, falcons, front_left, front_right, back_left, back_right,
+ &can_position_sender](ctre::phoenix::StatusCode status) {
+ // TODO(max): use status properly in the flatbuffer.
+ (void)status;
+
+ auto builder = can_position_sender.MakeBuilder();
+
+ for (auto falcon : falcons) {
+ falcon->RefreshNontimesyncedSignals();
+ falcon->SerializePosition(builder.fbb(), 1.0);
+ }
+
+ auto front_left_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), front_left);
+ auto front_right_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), front_right);
+ auto back_left_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), back_left);
+ auto back_right_offset = can_module_offset(
+ builder.MakeBuilder<SwerveModuleCANPosition>(), back_right);
+
+ AbsoluteCANPosition::Builder can_position_builder =
+ builder.MakeBuilder<AbsoluteCANPosition>();
+
+ can_position_builder.add_front_left(front_left_offset);
+ can_position_builder.add_front_right(front_right_offset);
+ can_position_builder.add_back_left(back_left_offset);
+ can_position_builder.add_back_right(back_right_offset);
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ });
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 2
+ // Setup CAN
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+ constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+ ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+ constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+ aos::ShmEventLoop drivetrain_writer_event_loop(&config.message());
+ drivetrain_writer_event_loop.set_name("DrivetrainWriter");
+
+ DrivetrainWriter drivetrain_writer(
+ &drivetrain_writer_event_loop,
+ constants::Values::kDrivetrainWriterPriority, 12);
+
+ drivetrain_writer.set_falcons(front_left, front_right, back_left,
+ back_right);
+
+ AddLoop(&drivetrain_writer_event_loop);
+
+ // Thread 3
+ aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+ sensor_reader_event_loop.set_name("SensorReader");
+ SensorReader sensor_reader(&sensor_reader_event_loop, values);
+
+ sensor_reader.set_follower_wheel_one_encoder(make_encoder(4));
+ sensor_reader.set_follower_wheel_two_encoder(make_encoder(5));
+
+ sensor_reader.set_front_left_encoder(make_encoder(1));
+ sensor_reader.set_front_left_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(1));
+
+ sensor_reader.set_front_right_encoder(make_encoder(0));
+ sensor_reader.set_front_right_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(0));
+
+ sensor_reader.set_back_left_encoder(make_encoder(2));
+ sensor_reader.set_back_left_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(2));
+
+ sensor_reader.set_back_right_encoder(make_encoder(3));
+ sensor_reader.set_back_right_absolute_pwm(
+ std::make_unique<frc::DigitalInput>(3));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ RunLoops();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2023_bot4
+
+AOS_ROBOT_CLASS(::y2023_bot4::wpilib::WPILibRobot)
diff --git a/y2023_bot4/y2023_bot4.json b/y2023_bot4/y2023_bot4.json
new file mode 100644
index 0000000..49db6fc
--- /dev/null
+++ b/y2023_bot4/y2023_bot4.json
@@ -0,0 +1,19 @@
+{
+ "channel_storage_duration": 10000000000,
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "imports": [
+ "y2023_bot4_roborio.json",
+ "y2023_bot4_imu.json",
+ "y2023_bot4_logger.json"
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_imu.json b/y2023_bot4/y2023_bot4_imu.json
new file mode 100644
index 0000000..274b158
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_imu.json
@@ -0,0 +1,212 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 20
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 20,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio",
+ "logger"
+ ],
+ "max_size": 400,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1600,
+ "num_senders": 2
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "imu",
+ "executable_name": "imu_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "pi6",
+ "hostnames": [
+ "pi-971-6",
+ "pi-7971-6",
+ "pi-8971-6",
+ "pi-9971-6"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "logger"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_logger.json b/y2023_bot4/y2023_bot4_logger.json
new file mode 100644
index 0000000..5cca31f
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_logger.json
@@ -0,0 +1,190 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.timing.Report",
+ "source_node": "logger",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "logger",
+ "frequency": 400,
+ "num_senders": 20
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "logger",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.Status",
+ "source_node": "logger",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "logger",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "logger",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 400,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu",
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/roborio/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/camera"
+ }
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "nodes": [
+ "logger"
+ ]
+ }
+ ],
+ "nodes": [
+ {
+ "name": "logger",
+ "hostname": "pi5",
+ "hostnames": [
+ "pi-971-5",
+ "pi-9971-5",
+ "pi-7971-5"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2023_bot4/y2023_bot4_roborio.json b/y2023_bot4/y2023_bot4_roborio.json
new file mode 100644
index 0000000..7ff175c
--- /dev/null
+++ b/y2023_bot4/y2023_bot4_roborio.json
@@ -0,0 +1,350 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.RobotState",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.timing.Report",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 8192
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio",
+ "frequency": 500,
+ "max_size": 1000,
+ "num_senders": 20
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "frequency": 10,
+ "max_size": 400,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "roborio",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 300,
+ "source_node": "roborio"
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 512,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2023_bot4.AbsoluteDrivetrainPosition",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 1,
+ "max_size": 480
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2023_bot4.AbsoluteCANPosition",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 1,
+ "max_size": 480
+ },
+ {
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.SplineGoal",
+ "source_node": "roborio",
+ "frequency": 10
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Goal",
+ "source_node": "roborio",
+ "max_size": 224,
+ "frequency": 250
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.swerve.Position",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 112,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.swerve.Output",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 200,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Status",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 1616,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.LocalizerControl",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 96,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 0
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 400,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/autonomous",
+ "type": "aos.common.actions.Status",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.Goal",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.AutonomousMode",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.PDPValues",
+ "source_node": "roborio",
+ "frequency": 55,
+ "max_size": 368
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.wpilib.PneumaticsToLog",
+ "source_node": "roborio",
+ "frequency": 50
+ },
+ {
+ "name": "/roborio",
+ "type": "frc971.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ }
+ ],
+ "applications": [
+ {
+ "name": "wpilib_interface",
+ "executable_name": "wpilib_interface",
+ "args": [
+ "--nodie_on_malloc",
+ "--ctre_diag_server"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "swerve_publisher",
+ "executable_name": "swerve_publisher",
+ "autostart": false,
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "args": [
+ "--rt_priority=16"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "logger",
+ "executable_name": "logger_main",
+ "args": [
+ "--snappy_compress",
+ "--logging_folder=/home/admin/logs/",
+ "--rotate_every",
+ "30.0"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "roborio",
+ "hostname": "roborio",
+ "hostnames": [
+ "roboRIO-971-FRC",
+ "roboRIO-6971-FRC",
+ "roboRIO-7971-FRC",
+ "roboRIO-8971-FRC",
+ "roboRIO-9971-FRC"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ },
+ {
+ "name": "logger"
+ }
+ ]
+}