Merge "Moving calibration file naming to a centralized call, for consistency"
diff --git a/aos/BUILD b/aos/BUILD
index 67e5b29..1f2d975 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -595,6 +595,23 @@
)
cc_binary(
+ name = "aos_jitter",
+ srcs = [
+ "aos_jitter.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":aos_cli_utils",
+ ":configuration",
+ ":init",
+ ":json_to_flatbuffer",
+ ":realtime",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_binary(
name = "aos_send",
srcs = [
"aos_send.cc",
diff --git a/aos/aos_jitter.cc b/aos/aos_jitter.cc
new file mode 100644
index 0000000..3ac35e8
--- /dev/null
+++ b/aos/aos_jitter.cc
@@ -0,0 +1,147 @@
+#include <unistd.h>
+
+#include <iostream>
+
+#include "gflags/gflags.h"
+
+#include "aos/aos_cli_utils.h"
+#include "aos/configuration.h"
+#include "aos/ftrace.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/realtime.h"
+
+DEFINE_int32(priority, -1, "If set, the RT priority to run at.");
+DEFINE_double(max_jitter, 0.01,
+ "The max time in milliseconds between messages before marking it "
+ "as too late.");
+DEFINE_bool(print_jitter, true,
+ "If true, print jitter events. These will impact RT performance.");
+DECLARE_bool(enable_ftrace);
+DEFINE_bool(print_latency_stats, false,
+ "If true, print latency stats. These will impact RT performance.");
+
+namespace aos {
+
+class State {
+ public:
+ State(Ftrace *ftrace, aos::EventLoop *event_loop, const Channel *channel)
+ : ftrace_(ftrace),
+ channel_(channel),
+ channel_name_(aos::configuration::StrippedChannelToString(channel_)) {
+ LOG(INFO) << "Watching for jitter on " << channel_name_;
+
+ event_loop->MakeRawWatcher(
+ channel_, [this](const aos::Context &context, const void *message) {
+ HandleMessage(context, message);
+ });
+
+ if (FLAGS_print_latency_stats) {
+ timer_handle_ = event_loop->AddTimer([this]() { PrintLatencyStats(); });
+ timer_handle_->set_name("jitter");
+ event_loop->OnRun([this, event_loop]() {
+ timer_handle_->Schedule(event_loop->monotonic_now(),
+ std::chrono::milliseconds(1000));
+ });
+ }
+ }
+
+ void HandleMessage(const aos::Context &context, const void * /*message*/) {
+ if (last_time_ != aos::monotonic_clock::min_time) {
+ latency_.push_back(std::chrono::duration<double>(
+ context.monotonic_event_time - last_time_)
+ .count());
+ if (context.monotonic_event_time >
+ last_time_ + std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_max_jitter))) {
+ if (FLAGS_enable_ftrace) {
+ ftrace_->FormatMessage(
+ "Got high latency event on %s -> %.9f between messages",
+ channel_name_.c_str(),
+ std::chrono::duration<double>(context.monotonic_event_time -
+ last_time_)
+ .count());
+ ftrace_->TurnOffOrDie();
+ }
+
+ if (FLAGS_print_jitter) {
+ // Printing isn't realtime, but if someone wants to run as RT, they
+ // should know this. Bypass the warning.
+ ScopedNotRealtime nrt;
+
+ LOG(INFO) << "Got a high latency event on "
+ << aos::configuration::StrippedChannelToString(channel_)
+ << " -> " << std::fixed << std::setprecision(9)
+ << std::chrono::duration<double>(
+ context.monotonic_event_time - last_time_)
+ .count()
+ << " between messages.";
+ }
+ }
+ }
+
+ last_time_ = context.monotonic_event_time;
+ }
+
+ void PrintLatencyStats() {
+ std::sort(latency_.begin(), latency_.end());
+ if (latency_.size() >= 100) {
+ LOG(INFO) << "Percentiles 25th: " << latency_[latency_.size() * 0.25]
+ << " 50th: " << latency_[latency_.size() * 0.5]
+ << " 75th: " << latency_[latency_.size() * 0.75]
+ << " 90th: " << latency_[latency_.size() * 0.9]
+ << " 95th: " << latency_[latency_.size() * 0.95]
+ << " 99th: " << latency_[latency_.size() * 0.99];
+ LOG(INFO) << "Max: " << latency_.back() << " Min: " << latency_.front()
+ << " Mean: "
+ << std::accumulate(latency_.begin(), latency_.end(), 0.0) /
+ latency_.size();
+ }
+ }
+
+ private:
+ Ftrace *ftrace_;
+ const Channel *channel_;
+
+ std::string channel_name_;
+
+ aos::monotonic_clock::time_point last_time_ = aos::monotonic_clock::min_time;
+
+ std::vector<double> latency_;
+
+ aos::TimerHandler *timer_handle_;
+};
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::CliUtilInfo cli_info;
+ if (cli_info.Initialize(
+ &argc, &argv,
+ [&cli_info](const aos::Channel *channel) {
+ return aos::configuration::ChannelIsReadableOnNode(
+ channel, cli_info.event_loop->node());
+ },
+ "channel is readeable on node", true)) {
+ return 0;
+ }
+
+ aos::Ftrace ftrace;
+
+ std::vector<std::unique_ptr<aos::State>> states;
+
+ for (const aos::Channel *channel : cli_info.found_channels) {
+ states.emplace_back(std::make_unique<aos::State>(
+ &ftrace, &(cli_info.event_loop.value()), channel));
+ }
+
+ if (FLAGS_priority > 0) {
+ cli_info.event_loop->SetRuntimeRealtimePriority(FLAGS_priority);
+ }
+
+ cli_info.event_loop->Run();
+
+ return 0;
+}
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index a26b564..9d14d8f 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -21,6 +21,7 @@
DEFINE_bool(dump_lockless_queue_data, false,
"If true, print the data out when dumping the queue.");
+DECLARE_bool(skip_realtime_scheduler);
namespace aos::ipc_lib {
namespace {
@@ -813,7 +814,19 @@
// Boost if we are RT and there is a higher priority sender out there.
// Otherwise we might run into priority inversions.
if (max_priority > current_priority && current_priority > 0) {
- SetCurrentThreadRealtimePriority(max_priority);
+ // Inline the setscheduler call rather than using aos/realtime.h. This is
+ // quite performance sensitive, and halves the time needed to send a
+ // message when pi boosting is in effect.
+ if (!FLAGS_skip_realtime_scheduler) {
+ // TODO(austin): Do we need to boost the soft limit here too like we
+ // were before?
+ struct sched_param param;
+ param.sched_priority = max_priority;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0)
+ << ": changing to SCHED_FIFO with " << max_priority
+ << ", if you want to bypass this check for testing, use "
+ "--skip_realtime_scheduler";
+ }
}
// Build up the siginfo to send.
@@ -842,7 +855,14 @@
// Drop back down if we were boosted.
if (max_priority > current_priority && current_priority > 0) {
- SetCurrentThreadRealtimePriority(current_priority);
+ if (!FLAGS_skip_realtime_scheduler) {
+ struct sched_param param;
+ param.sched_priority = current_priority;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0)
+ << ": changing to SCHED_FIFO with " << max_priority
+ << ", if you want to bypass this check for testing, use "
+ "--skip_realtime_scheduler";
+ }
}
}
diff --git a/aos/starter/irq_affinity.cc b/aos/starter/irq_affinity.cc
index 8b6bb2a..a188127 100644
--- a/aos/starter/irq_affinity.cc
+++ b/aos/starter/irq_affinity.cc
@@ -109,7 +109,7 @@
}
}
- void ConfigurePid(pid_t pid) const {
+ void ConfigurePid(pid_t pid, std::string_view name) const {
struct sched_param param;
param.sched_priority = priority;
int new_scheduler;
@@ -126,7 +126,11 @@
default:
LOG(FATAL) << "Unknown scheduler";
}
- PCHECK(sched_setscheduler(pid, new_scheduler, ¶m) == 0);
+ PCHECK(sched_setscheduler(pid, new_scheduler, ¶m) == 0)
+ << ", Failed to set " << name << "(" << pid << ") to "
+ << (new_scheduler == SCHED_OTHER
+ ? "SCHED_OTHER"
+ : (new_scheduler == SCHED_RR ? "SCHED_RR" : "SCHED_FIFO"));
if (scheduler == starter::Scheduler::SCHEDULER_OTHER && nice.has_value()) {
PCHECK(setpriority(PRIO_PROCESS, pid, *nice) == 0)
@@ -172,14 +176,14 @@
if (reading.second.kthread) {
for (const ParsedKThreadConfig &match : kthreads_) {
if (match.Matches(reading.second.name)) {
- match.ConfigurePid(reading.first);
+ match.ConfigurePid(reading.first, reading.second.name);
break;
}
}
} else {
for (const ParsedKThreadConfig &match : threads_) {
if (match.Matches(reading.second.name)) {
- match.ConfigurePid(reading.first);
+ match.ConfigurePid(reading.first, reading.second.name);
break;
}
}
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index 0215156..dc71cbd 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -10,10 +10,18 @@
ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
-elif [[ "$(hostname)" == "pi-"* || "$(hostname)" == "orin-"* ]]; then
+elif [[ "$(hostname)" == "pi-"* ]]; then
# We have systemd configured to handle restarting, so just exec.
export PATH="${PATH}:/home/pi/bin"
exec starterd --user=pi --purge_shm_base
+elif [[ "$(hostname)" == "orin-"* ]]; then
+ # We have systemd configured to handle restarting, so just exec.
+ export PATH="${PATH}:/home/pi/bin"
+
+ # Turn the fans up.
+ echo 255 > /sys/devices/platform/pwm-fan/hwmon/hwmon1/pwm1
+
+ exec starterd --user=pi --purge_shm_base
else
ROBOT_CODE="${HOME}/bin"
fi
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 7bb4061..599d477 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -578,3 +578,25 @@
"//aos/testing:path",
],
)
+
+static_flatbuffer(
+ name = "filesystem_fbs",
+ srcs = ["filesystem.fbs"],
+)
+
+cc_static_flatbuffer(
+ name = "filesystem_schema",
+ function = "aos::util::FilesystemStatusSchema",
+ target = ":filesystem_fbs_reflection_out",
+)
+
+cc_binary(
+ name = "filesystem_monitor",
+ srcs = ["filesystem_monitor.cc"],
+ deps = [
+ ":filesystem_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "@com_google_absl//absl/strings",
+ ],
+)
diff --git a/aos/util/filesystem.fbs b/aos/util/filesystem.fbs
new file mode 100644
index 0000000..0571d15
--- /dev/null
+++ b/aos/util/filesystem.fbs
@@ -0,0 +1,23 @@
+namespace aos.util;
+
+table Filesystem {
+ // Mountpoint of the filesystem in question.
+ path: string (id: 0);
+ // Type (e.g., "ext4") of the filesystem.
+ type: string (id: 1);
+ // Total size of the filesystem, in bytes.
+ overall_space: uint64 (id: 2);
+ // Total free space on the filesystem, in bytes.
+ free_space: uint64 (id: 3);
+ // Total number of inodes on this filesystem.
+ overall_inodes: uint64 (id: 4);
+ // Total free inodes on this filesystem.
+ free_inodes: uint64 (id: 5);
+}
+
+// Table to track the current state of a compute platform's filesystem.
+table FilesystemStatus {
+ filesystems: [Filesystem] (id: 0);
+}
+
+root_type FilesystemStatus;
diff --git a/aos/util/filesystem_monitor.cc b/aos/util/filesystem_monitor.cc
new file mode 100644
index 0000000..4efb141
--- /dev/null
+++ b/aos/util/filesystem_monitor.cc
@@ -0,0 +1,140 @@
+#include <sys/statvfs.h>
+
+#include "absl/strings/str_split.h"
+#include "gflags/gflags.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/filesystem_generated.h"
+
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
+namespace aos::util {
+namespace {
+std::optional<std::string> ReadShortFile(std::string_view file_name) {
+ // Open as input and seek to end immediately.
+ std::ifstream file(std::string(file_name), std::ios_base::in);
+ if (!file.good()) {
+ VLOG(1) << "Can't read " << file_name;
+ return std::nullopt;
+ }
+ const size_t kMaxLineLength = 4096;
+ char buffer[kMaxLineLength];
+ file.read(buffer, kMaxLineLength);
+ if (!file.eof()) {
+ return std::nullopt;
+ }
+ return std::string(buffer, file.gcount());
+}
+} // namespace
+
+// Periodically sends out the Filesystems message with filesystem utilization
+// info.
+class FilesystemMonitor {
+ public:
+ FilesystemMonitor(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ sender_(event_loop_->MakeSender<FilesystemStatus>("/aos")) {
+ periodic_timer_ =
+ event_loop_->AddTimer([this]() { PublishFilesystemStatus(); });
+ event_loop_->OnRun([this]() {
+ periodic_timer_->Schedule(event_loop_->monotonic_now(),
+ std::chrono::seconds(5));
+ });
+ }
+
+ private:
+ void PublishFilesystemStatus() {
+ aos::Sender<FilesystemStatus>::Builder builder = sender_.MakeBuilder();
+
+ std::optional<std::string> contents = ReadShortFile("/proc/self/mountinfo");
+
+ CHECK(contents.has_value());
+
+ std::vector<flatbuffers::Offset<Filesystem>> filesystems;
+
+ // Iterate through /proc/self/mounts to find all the filesystems.
+ for (std::string_view line :
+ absl::StrSplit(std::string_view(contents->c_str(), contents->size()),
+ '\n', absl::SkipWhitespace())) {
+ // See https://www.kernel.org/doc/Documentation/filesystems/proc.txt for
+ // the format.
+ std::vector<std::string_view> elements =
+ absl::StrSplit(line, ' ', absl::SkipWhitespace());
+
+ // First thing after - is the filesystem type.
+ size_t i = 6;
+ while (elements[i] != "-") {
+ ++i;
+ CHECK_LT(i + 1, elements.size());
+ }
+
+ // Mount point is the 4th element.
+ std::string mount_point(elements[4]);
+ std::string_view type = elements[i + 1];
+
+ // Ignore filesystems without reasonable types.
+ if (type != "ext2" && type != "xfs" && type != "vfat" && type != "ext3" &&
+ type != "ext4" && type != "tmpfs" && type != "devtmpfs") {
+ continue;
+ }
+ VLOG(1) << mount_point << ", type " << type;
+
+ struct statvfs info;
+
+ PCHECK(statvfs(mount_point.c_str(), &info) == 0);
+
+ VLOG(1) << "overall size: " << info.f_frsize * info.f_blocks << ", free "
+ << info.f_bfree * info.f_bsize << ", inodes " << info.f_files
+ << ", free " << info.f_ffree;
+
+ flatbuffers::Offset<flatbuffers::String> path_offset =
+ builder.fbb()->CreateString(mount_point);
+ flatbuffers::Offset<flatbuffers::String> type_offset =
+ builder.fbb()->CreateString(type);
+ Filesystem::Builder filesystem_builder =
+ builder.MakeBuilder<Filesystem>();
+ filesystem_builder.add_path(path_offset);
+ filesystem_builder.add_type(type_offset);
+ filesystem_builder.add_overall_space(info.f_frsize * info.f_blocks);
+ filesystem_builder.add_free_space(info.f_bfree * info.f_bsize);
+ filesystem_builder.add_overall_inodes(info.f_files);
+ filesystem_builder.add_free_inodes(info.f_ffree);
+
+ filesystems.emplace_back(filesystem_builder.Finish());
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Filesystem>>>
+ filesystems_offset = builder.fbb()->CreateVector(filesystems);
+
+ FilesystemStatus::Builder filesystem_status_builder =
+ builder.MakeBuilder<FilesystemStatus>();
+
+ filesystem_status_builder.add_filesystems(filesystems_offset);
+
+ (void)builder.Send(filesystem_status_builder.Finish());
+ }
+
+ aos::EventLoop *event_loop_;
+
+ aos::Sender<FilesystemStatus> sender_;
+
+ aos::TimerHandler *periodic_timer_;
+};
+
+} // namespace aos::util
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop shm_event_loop(&config.message());
+
+ aos::util::FilesystemMonitor filesystem_monitor(&shm_event_loop);
+
+ shm_event_loop.Run();
+
+ return 0;
+}
diff --git a/aos/util/top.h b/aos/util/top.h
index e8e0d57..a71873a 100644
--- a/aos/util/top.h
+++ b/aos/util/top.h
@@ -11,7 +11,7 @@
namespace aos::util {
// ProcStat is a struct to hold all the fields available in /proc/[pid]/stat.
-// Currently we only use a small subset of the feilds. See man 5 proc for
+// Currently we only use a small subset of the fields. See man 5 proc for
// details on what the fields are--these are in the same order as they appear in
// the stat file.
//
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 9c236c8..b2d11f5 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -124,3 +124,18 @@
],
deps = ["@RangeHTTPServer"],
)
+
+cc_binary(
+ name = "pdp_values",
+ srcs = [
+ "pdp_values.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/util:simulation_logger",
+ "//frc971/wpilib:pdp_values_fbs",
+ ],
+)
diff --git a/frc971/analysis/pdp_values.cc b/frc971/analysis/pdp_values.cc
new file mode 100644
index 0000000..b314cc6
--- /dev/null
+++ b/frc971/analysis/pdp_values.cc
@@ -0,0 +1,57 @@
+#include <fstream>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "frc971/wpilib/pdp_values_generated.h"
+
+DEFINE_string(output_path, "/tmp/pdp_values.csv", "");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+
+ aos::SimulatedEventLoopFactory event_loop_factory(reader.configuration());
+
+ reader.RegisterWithoutStarting(&event_loop_factory);
+
+ const aos::Node *roborio =
+ aos::configuration::GetNode(reader.configuration(), "roborio");
+
+ std::unique_ptr<aos::EventLoop> event_loop =
+ event_loop_factory.MakeEventLoop("roborio", roborio);
+
+ std::ofstream file_stream;
+ file_stream.open(FLAGS_output_path);
+ file_stream << "timestamp,currents,voltage\n";
+
+ event_loop->SkipAosLog();
+ event_loop->MakeWatcher(
+ "/roborio/aos",
+ [&file_stream, &event_loop](const frc971::PDPValues &pdp_values) {
+ file_stream << event_loop->context().monotonic_event_time << ","
+ << "[";
+
+ for (uint i = 0; i < pdp_values.currents()->size(); i++) {
+ file_stream << pdp_values.currents()->Get(i);
+ if (i != pdp_values.currents()->size() - 1) {
+ file_stream << ", ";
+ }
+ }
+
+ file_stream << "]," << pdp_values.voltage() << "\n";
+ });
+
+ event_loop_factory.Run();
+
+ reader.Deregister();
+
+ file_stream.close();
+
+ return 0;
+}
diff --git a/frc971/analysis/trim_log_to_enabled.cc b/frc971/analysis/trim_log_to_enabled.cc
index 0853ea5..47e7703 100644
--- a/frc971/analysis/trim_log_to_enabled.cc
+++ b/frc971/analysis/trim_log_to_enabled.cc
@@ -9,12 +9,19 @@
DEFINE_string(output_folder, "/tmp/trimmed/",
"Name of the folder to write the trimmed log to.");
+DEFINE_string(node, "roborio", "");
DEFINE_double(pre_enable_time_sec, 10.0,
"Amount of time to leave in the new log before the first enable "
"signal happens.");
DEFINE_double(post_enable_time_sec, 1.0,
"Amount of time to leave in the new log after the final enable "
"signal ends.");
+DEFINE_double(force_start_monotonic, -1.0,
+ "If set, time, in seconds, at which to forcibly trim the start "
+ "of the log.");
+DEFINE_double(
+ force_end_monotonic, -1.0,
+ "If set, time, in seconds, at which to forcibly trim the end of the log.");
int main(int argc, char *argv[]) {
gflags::SetUsageMessage(
@@ -26,15 +33,16 @@
std::optional<aos::monotonic_clock::time_point> start_time;
std::optional<aos::monotonic_clock::time_point> end_time;
bool printed_match = false;
+ bool force_time_range = FLAGS_force_start_monotonic > 0;
// We need to do two passes through the logfile; one to figure out when the
// start/end times are, one to actually do the trimming.
- {
+ if (!force_time_range) {
aos::logger::LogReader reader(logfiles);
const aos::Node *roborio =
- aos::configuration::GetNode(reader.configuration(), "roborio");
+ aos::configuration::GetNode(reader.configuration(), FLAGS_node);
reader.Register();
std::unique_ptr<aos::EventLoop> event_loop =
- reader.event_loop_factory()->MakeEventLoop("roborio", roborio);
+ reader.event_loop_factory()->MakeEventLoop(FLAGS_node, roborio);
event_loop->MakeWatcher(
"/aos", [&start_time, &end_time, &printed_match,
&event_loop](const aos::JoystickState &msg) {
@@ -60,25 +68,33 @@
if (!printed_match) {
LOG(INFO) << "No match info.";
}
+ if (!start_time.has_value()) {
+ LOG(WARNING) << "Log does not ontain any JoystickState messages.";
+ return 1;
+ }
+ LOG(INFO) << "First enable at " << start_time.value();
+ LOG(INFO) << "Final enable at " << end_time.value();
+ start_time.value() -= std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_pre_enable_time_sec));
+ end_time.value() += std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_post_enable_time_sec));
+ } else {
+ CHECK_LT(FLAGS_force_start_monotonic, FLAGS_force_end_monotonic);
+ start_time = aos::monotonic_clock::time_point(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_force_start_monotonic)));
+ end_time = aos::monotonic_clock::time_point(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<double>(FLAGS_force_end_monotonic)));
}
- if (!start_time.has_value()) {
- LOG(WARNING) << "Log does not ontain any JoystickState messages.";
- return 1;
- }
- LOG(INFO) << "First enable at " << start_time.value();
- LOG(INFO) << "Final enable at " << end_time.value();
- start_time.value() -= std::chrono::duration_cast<std::chrono::nanoseconds>(
- std::chrono::duration<double>(FLAGS_pre_enable_time_sec));
- end_time.value() += std::chrono::duration_cast<std::chrono::nanoseconds>(
- std::chrono::duration<double>(FLAGS_post_enable_time_sec));
{
aos::logger::LogReader reader(logfiles);
const aos::Node *roborio =
- aos::configuration::GetNode(reader.configuration(), "roborio");
+ aos::configuration::GetNode(reader.configuration(), FLAGS_node);
reader.Register();
std::unique_ptr<aos::EventLoop> event_loop =
- reader.event_loop_factory()->MakeEventLoop("roborio", roborio);
+ reader.event_loop_factory()->MakeEventLoop(FLAGS_node, roborio);
auto exit_timer = event_loop->AddTimer(
[&reader]() { reader.event_loop_factory()->Exit(); });
exit_timer->Schedule(start_time.value());
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
index 380410f..d020f6a 100644
--- a/frc971/can_logger/can_logger.cc
+++ b/frc971/can_logger/can_logger.cc
@@ -4,6 +4,10 @@
"If true, poll the CAN bus every 100ms. If false, wake up for "
"each frame and publish it.");
+DEFINE_int32(priority, 10,
+ "If --poll is not set, set the realtime priority to this.");
+DEFINE_int32(affinity, -1, "If positive, pin to this core.");
+
namespace frc971::can_logger {
CanLogger::CanLogger(aos::ShmEventLoop *event_loop,
@@ -14,7 +18,11 @@
frames_sender_(shm_event_loop_->MakeSender<CanFrame>(channel_name)) {
// TODO(max): Figure out a proper priority
if (!FLAGS_poll) {
- shm_event_loop_->SetRuntimeRealtimePriority(10);
+ shm_event_loop_->SetRuntimeRealtimePriority(FLAGS_priority);
+ }
+ if (FLAGS_affinity >= 0) {
+ shm_event_loop_->SetRuntimeAffinity(
+ aos::MakeCpusetFromCpus({FLAGS_affinity}));
}
struct ifreq ifr;
strcpy(ifr.ifr_name, interface_name.data());
diff --git a/frc971/control_loops/can_talonfx.fbs b/frc971/control_loops/can_talonfx.fbs
index 81efbb1..eea6379 100644
--- a/frc971/control_loops/can_talonfx.fbs
+++ b/frc971/control_loops/can_talonfx.fbs
@@ -22,5 +22,10 @@
position:float (id: 5);
// Nominal range is -1 to 1, but can be -2 to +2
- duty_cycle: float (id: 6);
+ duty_cycle:float (id: 6);
+
+ // The timestamp of the measurement on the canivore clock in nanoseconds
+ // This will have less jitter than the
+ // timestamp of the message being sent out.
+ timestamp:int64 (id: 7);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 95fab0a2..81f1d8d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -28,7 +28,7 @@
namespace {
// Maximum variation to allow in the gyro when zeroing.
-constexpr double kMaxYawGyroZeroingRange = 0.08;
+constexpr double kMaxYawGyroZeroingRange = 0.15;
} // namespace
DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
@@ -355,7 +355,7 @@
event_loop->TryMakeSender<
frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
"/drivetrain")) {
- event_loop->SetRuntimeRealtimePriority(30);
+ event_loop->SetRuntimeRealtimePriority(37);
for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
trajectory_fetchers_[ii].fetcher =
event_loop->MakeFetcher<fb::Trajectory>("/drivetrain");
@@ -570,6 +570,7 @@
builder.add_down_estimator(down_estimator_state_offset);
builder.add_localizer(localizer_offset);
builder.add_zeroing(zeroer_offset);
+ builder.add_filters_ready(filters_.Ready());
builder.add_send_failures(status_failure_counter_.failures());
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index 539b18e..d598c29 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -9,7 +9,7 @@
// The timestamp of the measurement on the canivore clock in nanoseconds
// This will have less jitter than the
// timestamp of the message being sent out.
- timestamp:int64 (id: 1);
+ timestamp:int64 (id: 1, deprecated);
// The ctre::phoenix::StatusCode of the measurement
// Should be OK = 0
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 8e18915..9d02bb2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -52,6 +52,14 @@
PLANNED,
}
+table SplineState {
+ x:double (id: 0);
+ y:double (id: 1);
+ theta:double (id: 2);
+ left_velocity:double (id: 3);
+ right_velocity:double (id: 4);
+}
+
// For logging information about the state of the trajectory planning.
table TrajectoryLogging {
// state of planning the trajectory.
@@ -82,6 +90,12 @@
// Splines that we have full plans for.
available_splines:[int] (id: 12);
+
+ state_error:SplineState (id: 14);
+ left_voltage_components:SplineState (id: 15);
+ right_voltage_components:SplineState (id: 16);
+ left_ff_voltage:double (id: 17);
+ right_ff_voltage:double (id: 18);
}
enum RobotSide : ubyte {
@@ -279,6 +293,8 @@
send_failures:uint64 (id: 28);
encoder_faults:Faults (id: 29);
+
+ filters_ready:bool (id: 30);
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 2f89a0c..0a41c55 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
#include <chrono>
+#include <optional>
#include "Eigen/Dense"
@@ -306,22 +307,27 @@
// Currently, we use the drivetrain config for modelling constants
// (continuous time A and B matrices) and for the noise matrices for the
// encoders/gyro.
- HybridEkf(const DrivetrainConfig<double> &dt_config)
+ // If force_dt is set, then all predict steps will use a dt of force_dt.
+ // This can be used in situations where there is no reliable clock guiding
+ // the measurement updates, but the source is coming in at a reasonably
+ // consistent period.
+ HybridEkf(const DrivetrainConfig<double> &dt_config,
+ std::optional<std::chrono::nanoseconds> force_dt = std::nullopt)
: dt_config_(dt_config),
velocity_drivetrain_coefficients_(
dt_config.make_hybrid_drivetrain_velocity_loop()
.plant()
- .coefficients()) {
+ .coefficients()),
+ force_dt_(force_dt) {
InitializeMatrices();
}
// Set the initial guess of the state. Can only be called once, and before
- // any measurement updates have occured.
+ // any measurement updates have occurred.
void ResetInitialState(::aos::monotonic_clock::time_point t,
const State &state, const StateSquare &P) {
observations_.clear();
X_hat_ = state;
- have_zeroed_encoders_ = true;
P_ = P;
observations_.PushFromBottom({
t,
@@ -358,8 +364,9 @@
// A utility function for specifically updating with encoder and gyro
// measurements.
- void UpdateEncodersAndGyro(const Scalar left_encoder,
- const Scalar right_encoder, const Scalar gyro_rate,
+ void UpdateEncodersAndGyro(const std::optional<Scalar> left_encoder,
+ const std::optional<Scalar> right_encoder,
+ const Scalar gyro_rate,
const Eigen::Matrix<Scalar, 2, 1> &voltage,
const Eigen::Matrix<Scalar, 3, 1> &accel,
aos::monotonic_clock::time_point t) {
@@ -371,8 +378,8 @@
}
// Version of UpdateEncodersAndGyro that takes a input matrix rather than
// taking in a voltage/acceleration separately.
- void RawUpdateEncodersAndGyro(const Scalar left_encoder,
- const Scalar right_encoder,
+ void RawUpdateEncodersAndGyro(const std::optional<Scalar> left_encoder,
+ const std::optional<Scalar> right_encoder,
const Scalar gyro_rate, const Input &U,
aos::monotonic_clock::time_point t) {
// Because the check below for have_zeroed_encoders_ will add an
@@ -386,26 +393,38 @@
// wpilib_interface, then we can get some obnoxious initial corrections
// that mess up the localization.
State newstate = X_hat_;
- newstate(kLeftEncoder) = left_encoder;
- newstate(kRightEncoder) = right_encoder;
+ have_zeroed_encoders_ = true;
+ if (left_encoder.has_value()) {
+ newstate(kLeftEncoder) = left_encoder.value();
+ } else {
+ have_zeroed_encoders_ = false;
+ }
+ if (right_encoder.has_value()) {
+ newstate(kRightEncoder) = right_encoder.value();
+ } else {
+ have_zeroed_encoders_ = false;
+ }
newstate(kLeftVoltageError) = 0.0;
newstate(kRightVoltageError) = 0.0;
newstate(kAngularError) = 0.0;
newstate(kLongitudinalVelocityOffset) = 0.0;
newstate(kLateralVelocity) = 0.0;
ResetInitialState(t, newstate, P_);
- // We need to set have_zeroed_encoders_ after ResetInitialPosition because
- // the reset clears have_zeroed_encoders_...
- have_zeroed_encoders_ = true;
}
- Output z(left_encoder, right_encoder, gyro_rate);
+ Output z(left_encoder.value_or(0.0), right_encoder.value_or(0.0),
+ gyro_rate);
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
CHECK(H_encoders_and_gyro_.has_value());
- Correct(z, &U, nullptr, &H_encoders_and_gyro_.value(), R, t);
+ CHECK(H_gyro_only_.has_value());
+ LinearH *H = &H_encoders_and_gyro_.value();
+ if (!left_encoder.has_value() || !right_encoder.has_value()) {
+ H = &H_gyro_only_.value();
+ }
+ Correct(z, &U, nullptr, H, R, t);
}
// Sundry accessor:
@@ -658,6 +677,9 @@
void PredictImpl(Observation *obs, std::chrono::nanoseconds dt, State *state,
StateSquare *P) {
+ if (force_dt_.has_value()) {
+ dt = force_dt_.value();
+ }
// Only recalculate the discretization if the timestep has changed.
// Technically, this isn't quite correct, since the discretization will
// change depending on the current state. However, the slight loss of
@@ -728,10 +750,12 @@
State X_hat_;
StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
velocity_drivetrain_coefficients_;
+ std::optional<std::chrono::nanoseconds> force_dt_;
StateSquare A_continuous_;
StateSquare Q_continuous_;
StateSquare P_;
std::optional<LinearH> H_encoders_and_gyro_;
+ std::optional<LinearH> H_gyro_only_;
Scalar encoder_noise_, gyro_noise_;
Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;
@@ -912,13 +936,14 @@
{
Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro;
H_encoders_and_gyro.setZero();
+ // Gyro rate is just the difference between right/left side speeds:
+ H_encoders_and_gyro(2, kLeftVelocity) = -1.0 / diameter;
+ H_encoders_and_gyro(2, kRightVelocity) = 1.0 / diameter;
+ H_gyro_only_.emplace(H_encoders_and_gyro);
// Encoders are stored directly in the state matrix, so are a minor
// transform away.
H_encoders_and_gyro(0, kLeftEncoder) = 1.0;
H_encoders_and_gyro(1, kRightEncoder) = 1.0;
- // Gyro rate is just the difference between right/left side speeds:
- H_encoders_and_gyro(2, kLeftVelocity) = -1.0 / diameter;
- H_encoders_and_gyro(2, kRightVelocity) = 1.0 / diameter;
H_encoders_and_gyro_.emplace(H_encoders_and_gyro);
}
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 2119c7d..96868cb 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -378,6 +378,14 @@
State true_X = ekf_.X_hat();
Input U;
U << -1.0, 5.0, 0.0, 0.0;
+ // Give one good update before starting to move things so that we initialize
+ // the encoders at zero.
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_);
for (int ii = 0; ii < 100; ++ii) {
true_X = Update(true_X, U, false);
ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
@@ -423,6 +431,14 @@
// small they are negligible.
Input U;
U << 10.0, 5.0, 0.0, 0.0;
+ // Give one good update before starting to move things so that we initialize
+ // the encoders at zero.
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_);
for (int ii = 0; ii < 100; ++ii) {
true_X = Update(true_X, U, false);
ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 57abe69..b1af886 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -120,8 +120,9 @@
int num_distance) {
return new Trajectory(
DistanceSpline(*spline),
- ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), nullptr, -1,
- vmax, num_distance);
+ std::make_unique<DrivetrainConfig<double>>(
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig()),
+ nullptr, -1, vmax, num_distance);
}
void deleteTrajectory(Trajectory *t) { delete t; }
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 0986037..14bc2ef 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -41,7 +41,9 @@
template <typename T>
std::optional<Eigen::Vector2d> GetPosition(
T &fetcher, aos::monotonic_clock::time_point now) {
- fetcher.Fetch();
+ if (!fetcher.Fetch()) {
+ return std::nullopt;
+ }
const bool stale =
(fetcher.get() == nullptr) ||
(fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 4d21e3b..8c3a239 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -31,9 +31,9 @@
// [left_voltage, right_voltage]
Eigen::Vector2d VoltageOrZero(aos::monotonic_clock::time_point now);
- // Returns the latest drivetrain encoder values (in meters), or nullopt if no
- // position message is available (or if the message is stale).
- // Returns encoders as [left_position, right_position]
+ // Returns the latest drivetrain encoder values (in meters), or nullopt if
+ // there has been no new encoder reading since the last call. Returns encoders
+ // as [left_position, right_position]
std::optional<Eigen::Vector2d> Encoders(aos::monotonic_clock::time_point now);
// Returns true if either there is no JoystickState message available or if
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 508967c..dbabfb1 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -74,7 +74,7 @@
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
CHECK_LT(trajectories_.size(), trajectories_.capacity());
- trajectories_.emplace_back(dt_config_, trajectory, velocity_drivetrain_);
+ trajectories_.emplace_back(&dt_config_, trajectory, velocity_drivetrain_);
UpdateSplineHandles(commanded_spline_);
}
@@ -175,6 +175,7 @@
Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ last_U_components_ = K * Eigen::DiagonalMatrix<double, 5>(state_error);
if (backwards) {
Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
@@ -186,6 +187,8 @@
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
+ last_state_error_ = state_error;
+ last_U_ff_ = U_ff;
}
}
@@ -215,6 +218,20 @@
}
}
+namespace {
+template <typename Matrix>
+flatbuffers::Offset<SplineState> MakeSplineState(
+ const Matrix &state, flatbuffers::FlatBufferBuilder *fbb) {
+ SplineState::Builder builder(*fbb);
+ builder.add_x(state(0));
+ builder.add_y(state(1));
+ builder.add_theta(::aos::math::NormalizeAngle(state(2)));
+ builder.add_left_velocity(state(3));
+ builder.add_right_velocity(state(4));
+ return builder.Finish();
+}
+} // namespace
+
flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const {
int *spline_handles;
@@ -225,6 +242,13 @@
spline_handles[ii] = trajectories_[ii].spline_handle();
}
+ const flatbuffers::Offset<SplineState> state_error_offset =
+ MakeSplineState(last_state_error_, builder);
+ const flatbuffers::Offset<SplineState> left_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(0), builder);
+ const flatbuffers::Offset<SplineState> right_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(1), builder);
+
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
if (executing_spline_) {
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
@@ -241,6 +265,13 @@
trajectory_logging_builder.add_left_velocity(goal_state(3));
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
+ trajectory_logging_builder.add_state_error(state_error_offset);
+ trajectory_logging_builder.add_left_voltage_components(
+ left_voltage_components_offset);
+ trajectory_logging_builder.add_right_voltage_components(
+ right_voltage_components_offset);
+ trajectory_logging_builder.add_left_ff_voltage(last_U_ff_(0));
+ trajectory_logging_builder.add_right_ff_voltage(last_U_ff_(1));
}
trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index f02a5b6..24fb66c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -105,6 +105,9 @@
// Information used for status message.
::Eigen::Matrix<double, 2, 1> uncapped_U_;
+ ::Eigen::Matrix<double, 5, 1> last_state_error_;
+ ::Eigen::Matrix<double, 2, 5> last_U_components_;
+ ::Eigen::Matrix<double, 2, 1> last_U_ff_;
bool enable_ = false;
bool output_was_capped_ = false;
};
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 36d0947..a876ae3 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -33,7 +33,7 @@
} // namespace
FinishedTrajectory::FinishedTrajectory(
- const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+ const DrivetrainConfig<double> *config, const fb::Trajectory *buffer,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
@@ -80,15 +80,15 @@
BaseTrajectory::BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config,
+ const DrivetrainConfig<double> *config,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain)
: velocity_drivetrain_(std::move(velocity_drivetrain)),
config_(config),
- robot_radius_l_(config.robot_radius),
- robot_radius_r_(config.robot_radius),
+ robot_radius_l_(config->robot_radius),
+ robot_radius_r_(config->robot_radius),
lateral_acceleration_(
ConstraintValue(constraints, ConstraintType::LATERAL_ACCELERATION)),
longitudinal_acceleration_(ConstraintValue(
@@ -96,7 +96,7 @@
voltage_limit_(ConstraintValue(constraints, ConstraintType::VOLTAGE)) {}
Trajectory::Trajectory(const SplineGoal &spline_goal,
- const DrivetrainConfig<double> &config)
+ const DrivetrainConfig<double> *config)
: Trajectory(DistanceSpline{spline_goal.spline()}, config,
spline_goal.spline()->constraints(),
spline_goal.spline_idx()) {
@@ -104,7 +104,7 @@
}
Trajectory::Trajectory(
- DistanceSpline &&input_spline, const DrivetrainConfig<double> &config,
+ DistanceSpline &&input_spline, const DrivetrainConfig<double> *config,
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
int spline_idx, double vmax, int num_distance)
: BaseTrajectory(constraints, config),
@@ -127,6 +127,15 @@
}
}
+Trajectory::Trajectory(
+ DistanceSpline &&spline, std::unique_ptr<DrivetrainConfig<double>> config,
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ int spline_idx, double vmax, int num_distance)
+ : Trajectory(std::move(spline), config.get(), constraints, spline_idx, vmax,
+ num_distance) {
+ owned_config_ = std::move(config);
+}
+
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
const double distance = Distance(i);
@@ -751,7 +760,8 @@
// finite-horizon much longer (albeit with the extension just using the
// linearization for the infal point).
void Trajectory::CalculatePathGains() {
- const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan = PlanXVA(config_.dt);
+ const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan =
+ PlanXVA(config_->dt);
if (xva_plan.empty()) {
LOG(ERROR) << "Plan is empty--unable to plan trajectory.";
return;
@@ -783,7 +793,7 @@
PathRelativeContinuousSystem(distance, &A_continuous, &B_continuous);
Eigen::Matrix<double, 5, 5> A_discrete;
Eigen::Matrix<double, 5, 2> B_discrete;
- controls::C2D(A_continuous, B_continuous, config_.dt, &A_discrete,
+ controls::C2D(A_continuous, B_continuous, config_->dt, &A_discrete,
&B_discrete);
if (i == max_index) {
@@ -898,9 +908,9 @@
result(2, 0) = spline().Theta(distance);
result.block<2, 1>(3, 0) =
- config_.Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
- spline().DThetaDt(distance, velocity))
- .finished();
+ config_->Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
+ spline().DThetaDt(distance, velocity))
+ .finished();
return result;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a9f14f6..5964bf5 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -14,6 +14,11 @@
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/control_loops/state_feedback_loop.h"
+// Note for all of these classes:
+// Whenever a pointer to a DrivetrainConfig is taken in the constructor, it must
+// live for the entire lifetime of the object. The classes here do not take
+// ownership of the pointer.
+
namespace frc971::control_loops::drivetrain {
template <typename F>
@@ -43,16 +48,16 @@
public:
BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config)
+ const DrivetrainConfig<double> *config)
: BaseTrajectory(constraints, config,
std::make_shared<StateFeedbackLoop<
2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>(
- config.make_hybrid_drivetrain_velocity_loop())) {}
+ config->make_hybrid_drivetrain_velocity_loop())) {}
BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config,
+ const DrivetrainConfig<double> *config,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
@@ -192,7 +197,7 @@
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
- DrivetrainConfig<double> config_;
+ const DrivetrainConfig<double> *config_;
// Robot radiuses.
double robot_radius_l_;
@@ -209,20 +214,20 @@
// Note: The lifetime of the supplied buffer is assumed to be greater than
// that of this object.
explicit FinishedTrajectory(
- const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+ const DrivetrainConfig<double> *config, const fb::Trajectory *buffer,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain);
- explicit FinishedTrajectory(const DrivetrainConfig<double> &config,
+ explicit FinishedTrajectory(const DrivetrainConfig<double> *config,
const fb::Trajectory *buffer)
: FinishedTrajectory(
config, buffer,
std::make_shared<StateFeedbackLoop<
2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>(
- config.make_hybrid_drivetrain_velocity_loop())) {}
+ config->make_hybrid_drivetrain_velocity_loop())) {}
FinishedTrajectory(const FinishedTrajectory &) = delete;
FinishedTrajectory &operator=(const FinishedTrajectory &) = delete;
@@ -264,16 +269,23 @@
class Trajectory : public BaseTrajectory {
public:
Trajectory(const SplineGoal &spline_goal,
- const DrivetrainConfig<double> &config);
+ const DrivetrainConfig<double> *config);
Trajectory(
- DistanceSpline &&spline, const DrivetrainConfig<double> &config,
+ DistanceSpline &&spline, const DrivetrainConfig<double> *config,
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
int spline_idx = 0, double vmax = 10.0, int num_distance = 0);
+ // Version that owns its own DrivetrainConfig.
+ Trajectory(
+ DistanceSpline &&spline, std::unique_ptr<DrivetrainConfig<double>> config,
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ int spline_idx, double vmax, int num_distance);
+
virtual ~Trajectory() = default;
std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(std::chrono::nanoseconds dt);
+ const DrivetrainConfig<double> *drivetrain_config() { return config_; }
enum class VoltageLimit {
kConservative,
kAggressive,
@@ -390,7 +402,8 @@
// The spline we are planning for.
const DistanceSpline spline_;
- const DrivetrainConfig<double> config_;
+ std::unique_ptr<DrivetrainConfig<double>> owned_config_;
+ const DrivetrainConfig<double> *config_;
// Velocities in the plan (distance for each index is defined by Distance())
std::vector<double> plan_;
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index 8e3e0f8..e531167 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -14,7 +14,7 @@
}
void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
- Trajectory trajectory(goal, dt_config_);
+ Trajectory trajectory(goal, &dt_config_);
trajectory.Plan();
aos::Sender<fb::Trajectory>::Builder builder =
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 8c3ef47..9eb95b5 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -50,7 +50,7 @@
(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
-0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
.finished()))),
- config, nullptr);
+ &config, nullptr);
trajectory.set_lateral_acceleration(2.0);
trajectory.set_longitudinal_acceleration(1.0);
@@ -131,7 +131,7 @@
aos::FlatbufferDetachedBuffer<fb::Trajectory> trajectory_buffer(
fbb.Release());
- FinishedTrajectory finished_trajectory(config, &trajectory_buffer.message());
+ FinishedTrajectory finished_trajectory(&config, &trajectory_buffer.message());
::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
state(0, 0) = FLAGS_dx;
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index fbf8970..6e9089a 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -74,7 +74,7 @@
const int spline_index = 12345;
// Run lots of steps to make the feedforwards terms more accurate.
trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
- DistanceSpline(GetParam().control_points), dt_config_,
+ DistanceSpline(GetParam().control_points), &dt_config_,
/*constraints=*/nullptr, spline_index, GetParam().velocity_limit));
distance_spline_ = &trajectory_->spline();
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
@@ -111,7 +111,7 @@
EXPECT_EQ(spline_index, trajectory_buffer_->message().handle());
finished_trajectory_ = std::make_unique<FinishedTrajectory>(
- dt_config_, &trajectory_buffer_->message());
+ &dt_config_, &trajectory_buffer_->message());
}
void TearDown() {
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 260a6ad..f96e2fe 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -148,7 +148,8 @@
return "y2022/actors/splines"
elif field.year == 2023:
return "y2023/autonomous/splines"
- #TODO: Update 2024 spline jsons
+ elif field.year == 2024:
+ return "y2024/autonomous/splines"
else:
return "frc971/control_loops/python/spline_jsons"
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 8ff7748..1535432 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -6,6 +6,7 @@
import math
import sys
from matplotlib import pylab
+import matplotlib
import glog
@@ -563,6 +564,9 @@
def PlotDrivetrainSprint(drivetrain_params):
+ # Set up the gtk backend before running matplotlib.
+ matplotlib.use("GTK3Agg")
+
# Simulate the response of the system to a step input.
drivetrain = KFDrivetrain(left_low=False,
right_low=False,
@@ -810,6 +814,9 @@
def PlotDrivetrainMotions(drivetrain_params):
+ # Set up the gtk backend before running matplotlib.
+ matplotlib.use("GTK3Agg")
+
# Test out the voltage error.
drivetrain = KFDrivetrain(left_low=False,
right_low=False,
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 1659e55..0cd41cb 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -311,7 +311,8 @@
file = filename.with_suffix(f".{index}.json")
print(f" {file.relative_to(export_folder)}")
with open(file, mode='w') as points_file:
- json.dump(multispline.toJsonObject(), points_file)
+ # Indent to make the file actually readable
+ json.dump(multispline.toJsonObject(), points_file, indent=4)
def import_json(self, file_name):
# Abort place mode
diff --git a/frc971/imu_fdcan/can_translator_lib.cc b/frc971/imu_fdcan/can_translator_lib.cc
index e4a927e..bbf758d 100644
--- a/frc971/imu_fdcan/can_translator_lib.cc
+++ b/frc971/imu_fdcan/can_translator_lib.cc
@@ -3,7 +3,6 @@
using frc971::imu_fdcan::CANTranslator;
constexpr std::size_t kCanFrameSize = 64;
-constexpr int kImuCanId = 1;
CANTranslator::CANTranslator(aos::EventLoop *event_loop,
std::string_view canframe_channel)
@@ -13,21 +12,23 @@
can_translator_status_sender_(
event_loop->MakeSender<frc971::imu::CanTranslatorStatusStatic>(
"/imu")) {
+ packets_arrived_.fill(false);
// TODO(max): Update this with a proper priority
- event_loop->SetRuntimeRealtimePriority(15);
+ event_loop->SetRuntimeRealtimePriority(58);
event_loop->MakeWatcher(
canframe_channel, [this](const frc971::can_logger::CanFrame &can_frame) {
- if (can_frame.data()->size() / sizeof(uint8_t) != 64) {
+ if (can_frame.data()->size() / sizeof(uint8_t) != 8) {
invalid_packet_count_++;
+ return;
}
- if (can_frame.can_id() != kImuCanId) {
+ if (can_frame.can_id() == 0 || can_frame.can_id() > 8) {
invalid_can_id_count_++;
return;
}
- if (can_frame.data()->size() / sizeof(uint8_t) == 64) {
+ if (can_frame.data()->size() / sizeof(uint8_t) == 8) {
valid_packet_count_++;
HandleFrame(&can_frame);
}
@@ -41,6 +42,7 @@
status_builder->set_valid_packet_count(valid_packet_count_);
status_builder->set_invalid_packet_count(invalid_packet_count_);
status_builder->set_invalid_can_id_count(invalid_can_id_count_);
+ status_builder->set_out_of_order_count(out_of_order_count_);
status_builder.CheckOk(status_builder.Send());
},
@@ -61,10 +63,26 @@
// Values from the data field mapping table in
// https://docs.google.com/document/d/12AJUruW7DZ2pIrDzTyPC0qqFoia4QOSVlax6Jd7m4H0/edit?usp=sharing
void CANTranslator::HandleFrame(const frc971::can_logger::CanFrame *can_frame) {
+ const size_t frame_index = can_frame->can_id() - 1u;
+ packets_arrived_[frame_index] = true;
+ for (size_t index = 0; index <= frame_index; ++index) {
+ if (!packets_arrived_[index]) {
+ ++out_of_order_count_;
+ packets_arrived_.fill(false);
+ return;
+ }
+ }
+ // Should have already checked length.
+ CHECK_EQ(can_frame->data()->size(), 8u);
+ memcpy(current_frame_.data() + frame_index * 8, can_frame->data()->data(), 8);
+ if (frame_index < 7) {
+ return;
+ }
+
aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
dual_imu_sender_.MakeStaticBuilder();
- std::span can_data(can_frame->data()->data(), kCanFrameSize);
+ std::span<const uint8_t> can_data(current_frame_.data(), kCanFrameSize);
frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
diff --git a/frc971/imu_fdcan/can_translator_lib.h b/frc971/imu_fdcan/can_translator_lib.h
index 12acb0a..af05aba 100644
--- a/frc971/imu_fdcan/can_translator_lib.h
+++ b/frc971/imu_fdcan/can_translator_lib.h
@@ -21,9 +21,13 @@
aos::Sender<imu::DualImuStatic> dual_imu_sender_;
aos::Sender<imu::CanTranslatorStatusStatic> can_translator_status_sender_;
+ std::array<uint8_t, 64> current_frame_;
+ std::array<bool, 8> packets_arrived_;
+
uint64_t valid_packet_count_ = 0;
uint64_t invalid_packet_count_ = 0;
uint64_t invalid_can_id_count_ = 0;
+ uint64_t out_of_order_count_ = 0;
};
} // namespace frc971::imu_fdcan
diff --git a/frc971/imu_fdcan/can_translator_lib_test.cc b/frc971/imu_fdcan/can_translator_lib_test.cc
index f8e6d85..fcef6a4 100644
--- a/frc971/imu_fdcan/can_translator_lib_test.cc
+++ b/frc971/imu_fdcan/can_translator_lib_test.cc
@@ -49,80 +49,23 @@
aos::monotonic_clock::epoch() + std::chrono::seconds(0),
aos::realtime_clock::epoch() + std::chrono::seconds(100));
can_frame_event_loop_->OnRun([this] {
- aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
- can_frame_builder = can_frame_sender_.MakeStaticBuilder();
+ std::array<uint8_t, 64> full_frame{
+ 226, 100, 108, 8, 152, 40, 202, 121, 202, 121, 202, 121, 85,
+ 85, 81, 189, 0, 0, 8, 189, 85, 213, 127, 191, 12, 189,
+ 34, 187, 255, 219, 220, 59, 147, 173, 5, 61, 88, 68, 205,
+ 188, 230, 92, 24, 189, 235, 1, 127, 191, 210, 7, 34, 54,
+ 86, 103, 133, 186, 100, 205, 101, 185, 29, 26, 26, 0};
+ for (size_t i = 0; i < 8; ++i) {
+ aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
+ can_frame_builder = can_frame_sender_.MakeStaticBuilder();
- can_frame_builder->set_can_id(1);
- can_frame_builder->set_realtime_timestamp_ns(100e9 + 971);
- auto can_data = can_frame_builder->add_data();
- CHECK(can_data->reserve(sizeof(uint8_t) * 64));
+ can_frame_builder->set_can_id(i + 1);
+ can_frame_builder->set_realtime_timestamp_ns(100e9 + 971);
+ auto can_data = can_frame_builder->add_data();
+ CHECK(can_data->FromData(full_frame.data() + i * 8, 8));
- CHECK(can_data->emplace_back(226));
- CHECK(can_data->emplace_back(100));
- CHECK(can_data->emplace_back(108));
- CHECK(can_data->emplace_back(8));
- CHECK(can_data->emplace_back(152));
- CHECK(can_data->emplace_back(40));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(81));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(0));
- CHECK(can_data->emplace_back(0));
- CHECK(can_data->emplace_back(8));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(213));
- CHECK(can_data->emplace_back(127));
- CHECK(can_data->emplace_back(191));
- CHECK(can_data->emplace_back(12));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(34));
- CHECK(can_data->emplace_back(187));
- CHECK(can_data->emplace_back(255));
- CHECK(can_data->emplace_back(219));
- CHECK(can_data->emplace_back(220));
- CHECK(can_data->emplace_back(59));
- CHECK(can_data->emplace_back(147));
- CHECK(can_data->emplace_back(173));
- CHECK(can_data->emplace_back(5));
- CHECK(can_data->emplace_back(61));
- CHECK(can_data->emplace_back(88));
- CHECK(can_data->emplace_back(68));
- CHECK(can_data->emplace_back(205));
- CHECK(can_data->emplace_back(188));
- CHECK(can_data->emplace_back(230));
- CHECK(can_data->emplace_back(92));
- CHECK(can_data->emplace_back(24));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(235));
- CHECK(can_data->emplace_back(1));
- CHECK(can_data->emplace_back(127));
- CHECK(can_data->emplace_back(191));
- CHECK(can_data->emplace_back(210));
- CHECK(can_data->emplace_back(7));
- CHECK(can_data->emplace_back(34));
- CHECK(can_data->emplace_back(54));
- CHECK(can_data->emplace_back(86));
- CHECK(can_data->emplace_back(103));
- CHECK(can_data->emplace_back(133));
- CHECK(can_data->emplace_back(186));
- CHECK(can_data->emplace_back(100));
- CHECK(can_data->emplace_back(205));
- CHECK(can_data->emplace_back(101));
- CHECK(can_data->emplace_back(185));
- CHECK(can_data->emplace_back(29));
- CHECK(can_data->emplace_back(26));
- CHECK(can_data->emplace_back(26));
- CHECK(can_data->emplace_back(0));
-
- can_frame_builder.CheckOk(can_frame_builder.Send());
+ can_frame_builder.CheckOk(can_frame_builder.Send());
+ }
});
event_loop_factory_.RunFor(std::chrono::milliseconds(200));
@@ -132,7 +75,7 @@
ASSERT_FALSE(can_translator_status_fetcher_->invalid_packet_count() > 0);
ASSERT_FALSE(can_translator_status_fetcher_->invalid_can_id_count() > 0);
- EXPECT_EQ(can_translator_status_fetcher_->valid_packet_count(), 1);
+ EXPECT_EQ(can_translator_status_fetcher_->valid_packet_count(), 8);
EXPECT_EQ(dual_imu_fetcher_->board_timestamp_us(), 141321442);
EXPECT_EQ(dual_imu_fetcher_->packet_counter(), 10392);
@@ -177,12 +120,11 @@
aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
can_frame_builder = can_frame_sender_.MakeStaticBuilder();
- can_frame_builder->set_can_id(2);
+ can_frame_builder->set_can_id(10);
can_frame_builder->set_realtime_timestamp_ns(100);
auto can_data = can_frame_builder->add_data();
- CHECK(can_data->reserve(sizeof(uint8_t) * 1));
-
- CHECK(can_data->emplace_back(0));
+ CHECK(can_data->reserve(sizeof(uint8_t) * 8));
+ can_data->resize(8);
can_frame_builder.CheckOk(can_frame_builder.Send());
});
@@ -192,6 +134,6 @@
ASSERT_TRUE(can_translator_status_fetcher_.Fetch());
ASSERT_FALSE(dual_imu_fetcher_.Fetch());
- EXPECT_EQ(can_translator_status_fetcher_->invalid_packet_count(), 1);
+ EXPECT_EQ(can_translator_status_fetcher_->invalid_packet_count(), 0);
EXPECT_EQ(can_translator_status_fetcher_->invalid_can_id_count(), 1);
}
diff --git a/frc971/imu_fdcan/can_translator_status.fbs b/frc971/imu_fdcan/can_translator_status.fbs
index 232f3ba..63273bc 100644
--- a/frc971/imu_fdcan/can_translator_status.fbs
+++ b/frc971/imu_fdcan/can_translator_status.fbs
@@ -7,6 +7,8 @@
invalid_packet_count: uint64 (id: 1);
// Number of times we've gotten an invalid can id
invalid_can_id_count: uint64 (id: 2);
+ // Number of times that we have observed an out of order can id.
+ out_of_order_count: uint64 (id: 3);
}
root_type CanTranslatorStatus;
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.cc b/frc971/imu_fdcan/dual_imu_blender_lib.cc
index 09655e3..6099844 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.cc
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.cc
@@ -14,6 +14,7 @@
// Coefficient to multiply the saturation values by to give some room on where
// we switch to tdk.
static constexpr double kSaturationCoeff = 0.9;
+static constexpr int kSaturationCounterThreshold = 20;
using frc971::imu_fdcan::DualImuBlender;
@@ -45,6 +46,12 @@
imu_values->set_pico_timestamp_us(dual_imu->board_timestamp_us());
imu_values->set_monotonic_timestamp_ns(dual_imu->kernel_timestamp());
imu_values->set_data_counter(dual_imu->packet_counter());
+ // Notes on saturation strategy:
+ // We use the TDK to detect saturation because we presume that if the Murata
+ // is saturated then it may produce poor or undefined behavior (including
+ // potentially producing values that make it look like it is not saturated).
+ // In practice, the Murata does seem to behave reasonably under saturation (it
+ // just maxes out its outputs at the given value).
if (std::abs(dual_imu->tdk()->gyro_x()) >=
kSaturationCoeff * kMurataGyroSaturation) {
@@ -64,8 +71,24 @@
imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
}
+ // TODO(james): Currently we only do hysteresis for the gyro Z axis because
+ // this is the only axis that is particularly critical. We should do something
+ // like this for all axes.
if (std::abs(dual_imu->tdk()->gyro_z()) >=
kSaturationCoeff * kMurataGyroSaturation) {
+ ++saturated_counter_;
+ } else {
+ --saturated_counter_;
+ }
+ if (saturated_counter_ <= -kSaturationCounterThreshold) {
+ is_saturated_ = false;
+ saturated_counter_ = -kSaturationCounterThreshold;
+ } else if (saturated_counter_ >= kSaturationCounterThreshold) {
+ is_saturated_ = true;
+ saturated_counter_ = kSaturationCounterThreshold;
+ }
+
+ if (is_saturated_) {
dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::TDK);
imu_values->set_gyro_z(dual_imu->tdk()->gyro_z());
} else {
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.h b/frc971/imu_fdcan/dual_imu_blender_lib.h
index 04044d2..223f3ed 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.h
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.h
@@ -20,6 +20,8 @@
private:
aos::Sender<IMUValuesBatchStatic> imu_values_batch_sender_;
aos::Sender<imu::DualImuBlenderStatusStatic> dual_imu_blender_status_sender_;
+ int saturated_counter_ = 0;
+ bool is_saturated_ = false;
};
} // namespace frc971::imu_fdcan
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
index b3a3015..4237bd1 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
+++ b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
@@ -30,6 +30,7 @@
dual_imu_blender_(dual_imu_blender_event_loop_.get()) {}
void CheckImuType(frc971::imu::ImuType type) {
+ dual_imu_blender_status_fetcher_.Fetch();
EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_x(), type);
EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_y(), type);
EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_z(), type);
@@ -182,41 +183,43 @@
CheckImuType(frc971::imu::ImuType::MURATA);
// Make sure we switch to TDK on saturation
- dual_imu_blender_event_loop_->OnRun([this] {
- aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
- dual_imu_sender_.MakeStaticBuilder();
+ dual_imu_blender_event_loop_->AddPhasedLoop(
+ [this](int) {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder
+ dual_imu_builder = dual_imu_sender_.MakeStaticBuilder();
- frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
- auto *murata_chip_states = murata->add_chip_states();
- frc971::imu::ChipStateStatic *murata_uno_chip_state =
- murata_chip_states->emplace_back();
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
- frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
- dual_imu_builder->set_board_timestamp_us(1);
- dual_imu_builder->set_kernel_timestamp(1);
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
- tdk->set_gyro_x(6.0);
- tdk->set_gyro_y(6.0);
- tdk->set_gyro_z(6.0);
+ tdk->set_gyro_x(6.0);
+ tdk->set_gyro_y(6.0);
+ tdk->set_gyro_z(6.0);
- murata->set_gyro_x(5.2);
- murata->set_gyro_y(5.2);
- murata->set_gyro_z(5.2);
+ murata->set_gyro_x(5.2);
+ murata->set_gyro_y(5.2);
+ murata->set_gyro_z(5.2);
- tdk->set_accelerometer_x(6.2);
- tdk->set_accelerometer_y(6.3);
- tdk->set_accelerometer_z(6.5);
+ tdk->set_accelerometer_x(6.2);
+ tdk->set_accelerometer_y(6.3);
+ tdk->set_accelerometer_z(6.5);
- murata->set_accelerometer_x(5.5);
- murata->set_accelerometer_y(5.5);
- murata->set_accelerometer_z(5.5);
+ murata->set_accelerometer_x(5.5);
+ murata->set_accelerometer_y(5.5);
+ murata->set_accelerometer_z(5.5);
- murata_uno_chip_state->set_temperature(20);
+ murata_uno_chip_state->set_temperature(20);
- dual_imu_builder.CheckOk(dual_imu_builder.Send());
- });
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ },
+ std::chrono::milliseconds(1));
event_loop_factory_.RunFor(std::chrono::milliseconds(200));
@@ -243,41 +246,43 @@
CheckImuType(frc971::imu::ImuType::TDK);
// Check negative values as well
- dual_imu_blender_event_loop_->OnRun([this] {
- aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
- dual_imu_sender_.MakeStaticBuilder();
+ dual_imu_blender_event_loop_->AddPhasedLoop(
+ [this](int) {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder
+ dual_imu_builder = dual_imu_sender_.MakeStaticBuilder();
- frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
- auto *murata_chip_states = murata->add_chip_states();
- frc971::imu::ChipStateStatic *murata_uno_chip_state =
- murata_chip_states->emplace_back();
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
- frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
- dual_imu_builder->set_board_timestamp_us(1);
- dual_imu_builder->set_kernel_timestamp(1);
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
- tdk->set_gyro_x(-6.0);
- tdk->set_gyro_y(-6.0);
- tdk->set_gyro_z(-6.0);
+ tdk->set_gyro_x(-6.0);
+ tdk->set_gyro_y(-6.0);
+ tdk->set_gyro_z(-6.0);
- murata->set_gyro_x(-5.2);
- murata->set_gyro_y(-5.2);
- murata->set_gyro_z(-5.2);
+ murata->set_gyro_x(-5.2);
+ murata->set_gyro_y(-5.2);
+ murata->set_gyro_z(-5.2);
- tdk->set_accelerometer_x(-6.2);
- tdk->set_accelerometer_y(-6.3);
- tdk->set_accelerometer_z(-6.5);
+ tdk->set_accelerometer_x(-6.2);
+ tdk->set_accelerometer_y(-6.3);
+ tdk->set_accelerometer_z(-6.5);
- murata->set_accelerometer_x(-5.5);
- murata->set_accelerometer_y(-5.5);
- murata->set_accelerometer_z(-5.5);
+ murata->set_accelerometer_x(-5.5);
+ murata->set_accelerometer_y(-5.5);
+ murata->set_accelerometer_z(-5.5);
- murata_uno_chip_state->set_temperature(20);
+ murata_uno_chip_state->set_temperature(20);
- dual_imu_builder.CheckOk(dual_imu_builder.Send());
- });
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ },
+ std::chrono::milliseconds(1));
event_loop_factory_.RunFor(std::chrono::milliseconds(200));
diff --git a/frc971/imu_fdcan/dual_imu_test_config_source.json b/frc971/imu_fdcan/dual_imu_test_config_source.json
index eda06da..c425be2 100644
--- a/frc971/imu_fdcan/dual_imu_test_config_source.json
+++ b/frc971/imu_fdcan/dual_imu_test_config_source.json
@@ -12,7 +12,7 @@
{
"name": "/imu",
"type": "frc971.imu.DualImu",
- "frequency": 200
+ "frequency": 2000
},
{
"name": "/can",
@@ -32,7 +32,7 @@
{
"name": "/imu",
"type": "frc971.imu.DualImuBlenderStatus",
- "frequency": 100
+ "frequency": 2000
},
]
}
diff --git a/frc971/math/flatbuffers_matrix.h b/frc971/math/flatbuffers_matrix.h
index 57013c9..cfca897 100644
--- a/frc971/math/flatbuffers_matrix.h
+++ b/frc971/math/flatbuffers_matrix.h
@@ -121,6 +121,22 @@
return true;
}
+template <int Rows, int Cols,
+ fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+flatbuffers::Offset<fbs::Matrix> FromEigen(
+ const typename EigenMatrix<Rows, Cols, StorageOrder>::type &matrix,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ constexpr size_t kSize = Rows * Cols;
+ flatbuffers::Offset<flatbuffers::Vector<double>> data_offset =
+ fbb->CreateVector(matrix.data(), kSize);
+ fbs::Matrix::Builder builder(*fbb);
+ builder.add_rows(Rows);
+ builder.add_cols(Cols);
+ builder.add_storage_order(StorageOrder);
+ builder.add_data(data_offset);
+ return builder.Finish();
+}
+
template <typename T>
bool FromEigen(const T &matrix, fbs::MatrixStatic *flatbuffer) {
return FromEigen<T::RowsAtCompileTime, T::ColsAtCompileTime,
diff --git a/frc971/math/flatbuffers_matrix_test.cc b/frc971/math/flatbuffers_matrix_test.cc
index 2807309..b2587f3 100644
--- a/frc971/math/flatbuffers_matrix_test.cc
+++ b/frc971/math/flatbuffers_matrix_test.cc
@@ -24,11 +24,18 @@
const Eigen::Matrix<double, 3, 4> expected{
{0, 1, 2, 3}, {4, 5, 6, 7}, {8, 9, 10, 11}};
aos::fbs::Builder<fbs::MatrixStatic> builder;
+ flatbuffers::FlatBufferBuilder fbb;
ASSERT_TRUE(FromEigen(expected, builder.get()));
+ fbb.Finish(FromEigen<3, 4>(expected, &fbb));
EXPECT_EQ(
"{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"ColMajor\", \"data\": "
"[ 0.0, 4.0, 8.0, 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0 ] }",
aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+ EXPECT_EQ(
+ "{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"ColMajor\", \"data\": "
+ "[ 0.0, 4.0, 8.0, 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0 ] }",
+ aos::FlatbufferToJson(
+ aos::FlatbufferDetachedBuffer<fbs::Matrix>(fbb.Release())));
const Eigen::Matrix<double, 3, 4> result =
ToEigenOrDie<3, 4>(builder->AsFlatbuffer());
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 06b1d6c..3ce9040 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -1,5 +1,7 @@
load("//frc971:halide.bzl", "halide_library")
+exports_files(["orin_irq_config.json"])
+
halide_library(
name = "ycbcr",
src = "crcv_generator.cc",
diff --git a/frc971/orin/apriltag.h b/frc971/orin/apriltag.h
index 3309996..7c0a721 100644
--- a/frc971/orin/apriltag.h
+++ b/frc971/orin/apriltag.h
@@ -193,6 +193,11 @@
distortion_coefficients_ = distortion_coefficients;
}
+ // Undistort pixels based on our camera model, using iterative algorithm
+ // Returns false if we fail to converge
+ static bool UnDistort(double *u, double *v, const CameraMatrix *camera_matrix,
+ const DistCoeffs *distortion_coefficients);
+
private:
void UpdateFitQuads();
diff --git a/frc971/orin/apriltag_detect.cc b/frc971/orin/apriltag_detect.cc
index 9eeb9da..26de7fa 100644
--- a/frc971/orin/apriltag_detect.cc
+++ b/frc971/orin/apriltag_detect.cc
@@ -334,8 +334,10 @@
// We're undistorting using math found from this github page
// https://yangyushi.github.io/code/2020/03/04/opencv-undistort.html
-void UnDistort(double *x, double *y, CameraMatrix *camera_matrix,
- DistCoeffs *distortion_coefficients) {
+bool GpuDetector::UnDistort(double *u, double *v,
+ const CameraMatrix *camera_matrix,
+ const DistCoeffs *distortion_coefficients) {
+ bool converged = true;
const double k1 = distortion_coefficients->k1;
const double k2 = distortion_coefficients->k2;
const double p1 = distortion_coefficients->p1;
@@ -347,8 +349,11 @@
const double fy = camera_matrix->fy;
const double cy = camera_matrix->cy;
- double xP = (*x - cx) / fx;
- double yP = (*y - cy) / fy;
+ const double xPP = (*u - cx) / fx;
+ const double yPP = (*v - cy) / fy;
+
+ double xP = xPP;
+ double yP = yPP;
double x0 = xP;
double y0 = yP;
@@ -373,6 +378,7 @@
yP = (y0 - tangential_dy) * radial_distortion_inv;
if (iterations > kUndistortIterationThreshold) {
+ converged = false;
break;
}
@@ -391,8 +397,10 @@
<< " (" << prev_x << ", " << prev_y << ")";
}
- *x = xP * fx + cx;
- *y = yP * fy + cy;
+ *u = xP * fx + cx;
+ *v = yP * fy + cy;
+
+ return converged;
}
// Mostly stolen from aprilrobotics, but modified to implement the dewarp.
@@ -494,7 +502,8 @@
double bestx = x0 + n0 * nx;
double besty = y0 + n0 * ny;
- UnDistort(&bestx, &besty, camera_matrix, distortion_coefficients);
+ GpuDetector::UnDistort(&bestx, &besty, camera_matrix,
+ distortion_coefficients);
// update our line fit statistics
Mx += bestx;
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index a4dca06..42686a1 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -1,3 +1,5 @@
+#include <dirent.h>
+
#include <chrono>
#include <filesystem>
#include <thread>
@@ -522,6 +524,7 @@
aos::ShmEventLoop event_loop(&config.message());
event_loop.SetRuntimeRealtimePriority(55);
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({2, 3, 4}));
aos::Sender<frc971::vision::CameraImage> sender =
event_loop.MakeSender<frc971::vision::CameraImage>(FLAGS_channel);
@@ -630,6 +633,25 @@
camera.Start();
+ // Set the libargus threads which got spawned to RT priority.
+ {
+ DIR *dirp = opendir("/proc/self/task");
+ PCHECK(dirp != nullptr);
+ const int main_pid = getpid();
+ struct dirent *directory_entry;
+ while ((directory_entry = readdir(dirp)) != NULL) {
+ const int thread_id = std::atoi(directory_entry->d_name);
+
+ // ignore . and .. which are zeroes for some reason
+ if (thread_id != 0 && thread_id != main_pid) {
+ struct sched_param param;
+ param.sched_priority = 56;
+ sched_setscheduler(thread_id, SCHED_FIFO, ¶m);
+ }
+ }
+ closedir(dirp);
+ }
+
event_loop.Run();
LOG(INFO) << "Event loop shutting down";
diff --git a/frc971/orin/build_rootfs.py b/frc971/orin/build_rootfs.py
index e0559a1..9ae0a72 100755
--- a/frc971/orin/build_rootfs.py
+++ b/frc971/orin/build_rootfs.py
@@ -1253,6 +1253,8 @@
copyfile("root:root", "644", "etc/systemd/network/80-canc.network")
copyfile("root:root", "644", "etc/udev/rules.d/nvidia.rules")
copyfile("root:root", "644", "etc/udev/rules.d/can.rules")
+ copyfile("root:root", "644",
+ "lib/systemd/system/nvargus-daemon.service")
target(["/root/bin/change_hostname.sh", "orin-971-1"])
target(["systemctl", "enable", "systemd-networkd"])
diff --git a/frc971/orin/contents/lib/systemd/system/nvargus-daemon.service b/frc971/orin/contents/lib/systemd/system/nvargus-daemon.service
new file mode 100644
index 0000000..36b9e4e
--- /dev/null
+++ b/frc971/orin/contents/lib/systemd/system/nvargus-daemon.service
@@ -0,0 +1,16 @@
+[Unit]
+Description=NVIDIA Argus daemon
+After=local-fs.target network.target nvpmodel.service
+
+[Service]
+Type=simple
+ExecStart=/usr/sbin/nvargus-daemon
+StandardOutput=journal
+Restart=on-failure
+CPUSchedulingPolicy=fifo
+CPUSchedulingPriority=51
+#Environment=enableCamPclLogs=5
+#Environment=enableCamScfLogs=5
+
+[Install]
+WantedBy=multi-user.target
diff --git a/frc971/orin/gpu_apriltag.cc b/frc971/orin/gpu_apriltag.cc
index 27cab4d..87d5f49 100644
--- a/frc971/orin/gpu_apriltag.cc
+++ b/frc971/orin/gpu_apriltag.cc
@@ -26,7 +26,7 @@
"1.0.");
DEFINE_double(min_decision_margin, 50.0,
"Minimum decision margin (confidence) for an apriltag detection");
-DEFINE_int32(pixel_border, 10,
+DEFINE_int32(pixel_border, 150,
"Size of image border within which to reject detected corners");
DEFINE_uint64(pose_estimation_iterations, 50,
"Number of iterations for apriltag pose estimation.");
@@ -74,8 +74,10 @@
intrinsics_(frc971::vision::CameraIntrinsics(calibration_)),
extrinsics_(frc971::vision::CameraExtrinsics(calibration_)),
dist_coeffs_(frc971::vision::CameraDistCoeffs(calibration_)),
- gpu_detector_(width, height, tag_detector_, GetCameraMatrix(calibration_),
- GetDistCoeffs(calibration_)),
+ distortion_camera_matrix_(GetCameraMatrix(calibration_)),
+ distortion_coefficients_(GetDistCoeffs(calibration_)),
+ gpu_detector_(width, height, tag_detector_, distortion_camera_matrix_,
+ distortion_coefficients_),
image_callback_(
event_loop, channel_name,
[this](cv::Mat image_color_mat,
@@ -135,25 +137,19 @@
detection.distortion_factor, detection.pose_error_ratio);
}
-void ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
- // 4 corners
- constexpr size_t kRows = 4;
- // 2d points
- constexpr size_t kCols = 2;
-
- cv::Mat distorted_points(kRows, kCols, CV_64F, det->p);
- cv::Mat undistorted_points = cv::Mat::zeros(kRows, kCols, CV_64F);
-
- // Undistort the april tag points
- cv::undistortPoints(distorted_points, undistorted_points, intrinsics_,
- dist_coeffs_, cv::noArray(), projection_matrix_);
-
+bool ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
// Copy the undistorted points into det
- for (size_t i = 0; i < kRows; i++) {
- for (size_t j = 0; j < kCols; j++) {
- det->p[i][j] = undistorted_points.at<double>(i, j);
- }
+ bool converged = true;
+ for (size_t i = 0; i < 4; i++) {
+ double u = det->p[i][0];
+ double v = det->p[i][1];
+
+ converged &= GpuDetector::UnDistort(&u, &v, &distortion_camera_matrix_,
+ &distortion_coefficients_);
+ det->p[i][0] = u;
+ det->p[i][1] = v;
}
+ return converged;
}
double ApriltagDetector::ComputeDistortionFactor(
@@ -267,7 +263,6 @@
// First create an apriltag_detection_info_t struct using your known
// parameters.
apriltag_detection_info_t info;
- info.det = gpu_detection;
info.tagsize = 6.5 * 0.0254;
info.fx = intrinsics_.at<double>(0, 0);
@@ -282,7 +277,24 @@
builder.fbb(), eof, orig_corner_points,
std::vector<double>{0.0, 1.0, 0.0, 0.5}));
- UndistortDetection(gpu_detection);
+ bool converged = UndistortDetection(gpu_detection);
+
+ if (!converged) {
+ VLOG(1) << "Rejecting detection because Undistort failed to coverge";
+
+ // Send rejected corner points to foxglove in red
+ std::vector<cv::Point2f> rejected_corner_points =
+ MakeCornerVector(gpu_detection);
+ foxglove_corners.push_back(frc971::vision::BuildPointsAnnotation(
+ builder.fbb(), eof, rejected_corner_points,
+ std::vector<double>{1.0, 0.0, 0.0, 0.5}));
+ rejections_++;
+ continue;
+ }
+
+ // We're setting this here to use the undistorted corner points in pose
+ // estimation.
+ info.det = gpu_detection;
const aos::monotonic_clock::time_point before_pose_estimation =
aos::monotonic_clock::now();
@@ -302,8 +314,10 @@
before_pose_estimation)
.count()
<< " seconds for pose estimation";
- VLOG(1) << "Pose err 1: " << pose_error_1;
- VLOG(1) << "Pose err 2: " << pose_error_2;
+ VLOG(1) << "Pose err 1: " << std::setprecision(20) << std::fixed
+ << pose_error_1 << " " << (pose_error_1 < 1e-6 ? "Good" : "Bad");
+ VLOG(1) << "Pose err 2: " << std::setprecision(20) << std::fixed
+ << pose_error_2 << " " << (pose_error_2 < 1e-6 ? "Good" : "Bad");
// Send undistorted corner points in pink
std::vector<cv::Point2f> corner_points = MakeCornerVector(gpu_detection);
@@ -412,7 +426,7 @@
timeprofile_display(tag_detector_->tp);
}
- VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
+ VLOG(2) << "Took " << chrono::duration<double>(end_time - start_time).count()
<< " seconds to detect overall";
return;
diff --git a/frc971/orin/gpu_apriltag.h b/frc971/orin/gpu_apriltag.h
index 39d2b60..f02a5a3 100644
--- a/frc971/orin/gpu_apriltag.h
+++ b/frc971/orin/gpu_apriltag.h
@@ -53,7 +53,8 @@
// Undistorts the april tag corners using the camera calibration.
// Returns the detections in place.
- void UndistortDetection(apriltag_detection_t *det) const;
+ // Returns false if any of the corner undistortions fail to converge
+ bool UndistortDetection(apriltag_detection_t *det) const;
// Computes the distortion effect on this detection taking the scaled average
// delta between orig_corners (distorted corners) and corners (undistorted
@@ -81,6 +82,11 @@
std::optional<cv::Mat> extrinsics_;
cv::Mat dist_coeffs_;
+ // The distortion constants passed into gpu_detector_ and used later on to do
+ // undistort before passing corners into apriltags pose estimation.
+ CameraMatrix distortion_camera_matrix_;
+ DistCoeffs distortion_coefficients_;
+
frc971::apriltag::GpuDetector gpu_detector_;
cv::Size image_size_;
diff --git a/frc971/orin/orin_irq_config.json b/frc971/orin/orin_irq_config.json
index 0edb72b..c067e20 100644
--- a/frc971/orin/orin_irq_config.json
+++ b/frc971/orin/orin_irq_config.json
@@ -40,7 +40,7 @@
},
{
"name": "cana",
- "affinity": [4]
+ "affinity": [5]
},
{
"name": "uart-pl011",
@@ -276,12 +276,12 @@
{
"name": "irq/*-cana",
"scheduler": "SCHEDULER_FIFO",
- "priority": 51,
- "affinity": [4]
+ "priority": 60,
+ "affinity": [5]
},
{
"name": "irq/*-peak_pcie",
- "scheduler": "SCHEDULER_FIFO",
+ "scheduler": "SCHEDULER_OTHER",
"priority": 51,
"affinity": [5]
},
@@ -306,6 +306,11 @@
"nice": -20
},
{
+ "name": "ivc/bc00000.rtc",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 55
+ },
+ {
"name": "irq/*-aerdrv",
"scheduler": "SCHEDULER_OTHER",
"nice": -20
@@ -319,7 +324,7 @@
"name": "irq/*-host_sy",
"scheduler": "SCHEDULER_FIFO",
"affinity": [2, 3],
- "priority": 51
+ "priority": 58
},
{
"name": "irq/*-b950000",
@@ -328,11 +333,6 @@
"priority": 51
},
{
- "name": "ivc/*.rtc",
- "scheduler": "SCHEDULER_FIFO",
- "nice": 49
- },
- {
"name": "nvgpu_nvs_ga10b",
"scheduler": "SCHEDULER_OTHER",
"affinity": [2, 3],
diff --git a/frc971/vision/foxglove_image_converter_lib.cc b/frc971/vision/foxglove_image_converter_lib.cc
index 920eaf7..649f1bb 100644
--- a/frc971/vision/foxglove_image_converter_lib.cc
+++ b/frc971/vision/foxglove_image_converter_lib.cc
@@ -6,6 +6,8 @@
DEFINE_int32(jpeg_quality, 60,
"Compression quality of JPEGs, 0-100; lower numbers mean lower "
"quality and resulting image sizes.");
+DEFINE_uint32(max_period_ms, 100,
+ "Fastest period at which to convert images, to limit CPU usage.");
namespace frc971::vision {
std::string_view ExtensionForCompression(ImageCompression compression) {
@@ -49,9 +51,13 @@
event_loop_, input_channel,
[this, compression](const cv::Mat image,
const aos::monotonic_clock::time_point eof) {
- auto builder = sender_.MakeBuilder();
- builder.CheckOk(builder.Send(
- CompressImage(image, eof, builder.fbb(), compression)));
+ if (event_loop_->monotonic_now() >
+ (std::chrono::milliseconds(FLAGS_max_period_ms) +
+ sender_.monotonic_sent_time())) {
+ auto builder = sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(
+ CompressImage(image, eof, builder.fbb(), compression)));
+ }
}),
sender_(
event_loop_->MakeSender<foxglove::CompressedImage>(output_channel)) {}
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index 5b02bb5..b8c5bf9 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -26,7 +26,8 @@
PDPFetcher::~PDPFetcher() {}
void PDPFetcher::Loop(int iterations) {
- if (iterations != 1) {
+ // Only pollute the logs if we've missed many iterations.
+ if (iterations > 3) {
AOS_LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
std::array<double, 16> currents;
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 447e3b4..1c8c5d3 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -120,11 +120,18 @@
AOS_LOG(INFO, "Defaulting to open loop pwm synchronization\n");
}
- // Now that we are configured, actually fill in the defaults.
- timer_handler_->Schedule(
- event_loop_->monotonic_now() +
- (pwm_trigger_ ? chrono::milliseconds(3) : chrono::milliseconds(4)),
- period_);
+ if (pwm_trigger_) {
+ // Now that we are configured, actually fill in the defaults.
+ timer_handler_->Schedule(
+ event_loop_->monotonic_now() +
+ (pwm_trigger_ ? chrono::milliseconds(3) : chrono::milliseconds(4)),
+ period_);
+ } else {
+ // Synchronous CAN wakes up at round multiples of the clock. Use a phased
+ // loop to calculate it.
+ aos::time::PhasedLoop phased_loop(period_, monotonic_clock::now());
+ timer_handler_->Schedule(phased_loop.sleep_time(), period_);
+ }
last_monotonic_now_ = monotonic_clock::now();
}
diff --git a/frc971/wpilib/talonfx.cc b/frc971/wpilib/talonfx.cc
index 7959f2f..152339e 100644
--- a/frc971/wpilib/talonfx.cc
+++ b/frc971/wpilib/talonfx.cc
@@ -62,6 +62,7 @@
current_limits.StatorCurrentLimitEnable = true;
current_limits.SupplyCurrentLimit = supply_current_limit_;
current_limits.SupplyCurrentLimitEnable = true;
+ current_limits.SupplyTimeThreshold = 0.0;
ctre::phoenix6::configs::MotorOutputConfigs output_configs;
output_configs.NeutralMode = neutral_mode_;
@@ -123,4 +124,5 @@
can_talonfx->set_torque_current(torque_current());
can_talonfx->set_duty_cycle(duty_cycle());
can_talonfx->set_position(position() * gear_ratio);
+ can_talonfx->set_timestamp(GetTimestamp());
}
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index c06ed43..3164bed 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -25,8 +25,8 @@
}
} // namespace
-ImuZeroer::ImuZeroer(FaultBehavior fault_behavior)
- : fault_behavior_(fault_behavior) {
+ImuZeroer::ImuZeroer(FaultBehavior fault_behavior, double gyro_max_variation)
+ : fault_behavior_(fault_behavior), gyro_max_variation_(gyro_max_variation) {
gyro_average_.setZero();
last_gyro_sample_.setZero();
last_accel_sample_.setZero();
@@ -53,7 +53,7 @@
bool ImuZeroer::GyroZeroReady() const {
return gyro_averager_.full() &&
- gyro_averager_.GetRange() < kGyroMaxVariation &&
+ gyro_averager_.GetRange() < gyro_max_variation_ &&
(last_gyro_sample_.lpNorm<Eigen::Infinity>() <
kGyroMaxZeroingMagnitude);
}
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 2a5036d..52f6cc0 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -30,7 +30,13 @@
kTemporary
};
- explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch);
+ // Max variation (difference between the maximum and minimum value) in a
+ // kSamplesToAverage range before we allow using the samples for zeroing.
+ // These values are currently based on looking at results from the ADIS16448.
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+
+ explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch,
+ double gyro_max_variation = kGyroMaxVariation);
bool Zeroed() const;
bool Faulted() const;
bool InsertMeasurement(const IMUValues &values);
@@ -45,10 +51,6 @@
flatbuffers::FlatBufferBuilder *fbb) const;
private:
- // Max variation (difference between the maximum and minimum value) in a
- // kSamplesToAverage range before we allow using the samples for zeroing.
- // These values are currently based on looking at results from the ADIS16448.
- static constexpr double kGyroMaxVariation = 0.02; // rad / sec
// Maximum magnitude we allow the gyro zero to have--this is used to prevent
// us from zeroing the gyro if we just happen to be spinning at a very
// consistent non-zero rate. Currently this is only plausible in simulation.
@@ -74,6 +76,7 @@
Eigen::Vector3d last_accel_sample_;
const FaultBehavior fault_behavior_;
+ const double gyro_max_variation_;
bool reading_faulted_ = false;
bool zeroing_faulted_ = false;
diff --git a/scouting/www/app/app.ts b/scouting/www/app/app.ts
index ab15ae5..1a70b6f 100644
--- a/scouting/www/app/app.ts
+++ b/scouting/www/app/app.ts
@@ -40,8 +40,6 @@
@ViewChild('block_alerts') block_alerts: ElementRef;
constructor() {
- console.log(`Using development mode: ${isDevMode()}`);
-
window.addEventListener('beforeunload', (e) => {
if (!unguardedTabs.includes(this.tab)) {
if (!this.block_alerts.nativeElement.checked) {
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index 09b1a8b..21fd474 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -132,6 +132,8 @@
@Input() compLevel: CompLevel = 'qm';
@Input() skipTeamSelection = false;
+ @ViewChild('header') header: ElementRef;
+
matchList: Match[] = [];
actionList: ActionT[] = [];
@@ -139,6 +141,8 @@
errorMessage: string = '';
autoPhase: boolean = true;
mobilityCompleted: boolean = false;
+ selectedValue = 0;
+ nextTeamNumber = '';
preScouting: boolean = false;
matchStartTimestamp: number = 0;
@@ -162,10 +166,13 @@
// When the user navigated from the match list, we can skip the team
// selection. I.e. we trust that the user clicked the correct button.
this.section = this.skipTeamSelection ? 'Init' : 'Team Selection';
+ this.fetchMatchList();
+ }
- if (this.section == 'Team Selection') {
- this.fetchMatchList();
- }
+ goToNextTeam() {
+ this.ngOnInit();
+ this.teamNumber = this.nextTeamNumber;
+ this.nextTeamNumber = '';
}
async fetchMatchList() {
@@ -194,19 +201,18 @@
if (this.teamNumber == null) {
return false;
}
- const teamNumber = this.teamNumber;
for (const match of this.matchList) {
if (
this.matchNumber == match.matchNumber() &&
this.setNumber == match.setNumber() &&
this.compLevel == match.compLevel() &&
- (teamNumber === match.r1() ||
- teamNumber === match.r2() ||
- teamNumber === match.r3() ||
- teamNumber === match.b1() ||
- teamNumber === match.b2() ||
- teamNumber === match.b3())
+ (this.teamNumber === match.r1() ||
+ this.teamNumber === match.r2() ||
+ this.teamNumber === match.r3() ||
+ this.teamNumber === match.b1() ||
+ this.teamNumber === match.b2() ||
+ this.teamNumber === match.b3())
) {
return true;
}
@@ -311,8 +317,6 @@
this.section = target;
}
- @ViewChild('header') header: ElementRef;
-
private scrollToTop() {
this.header.nativeElement.scrollIntoView();
}
@@ -323,7 +327,6 @@
for (const action of this.actionList) {
let actionOffset: number | undefined;
- console.log(action.type);
switch (action.type) {
case 'startMatchAction':
@@ -489,6 +492,73 @@
// We successfully submitted the data. Report success.
this.section = 'Success';
this.actionList = [];
+
+ // Keep track of the position of the last robot, use to figure out what
+ // the next robot in the same position is.
+ let lastTeamPos = '0';
+ for (const match of this.matchList) {
+ if (
+ this.matchNumber === match.matchNumber() &&
+ this.setNumber === match.setNumber() &&
+ this.compLevel === match.compLevel()
+ ) {
+ this.teamNumber = this.teamNumber;
+ if (this.teamNumber == match.r1()) {
+ lastTeamPos = 'r1';
+ } else if (this.teamNumber == match.r2()) {
+ lastTeamPos = 'r2';
+ } else if (this.teamNumber == match.r3()) {
+ lastTeamPos = 'r3';
+ } else if (this.teamNumber == match.b1()) {
+ lastTeamPos = 'b1';
+ } else if (this.teamNumber == match.b2()) {
+ lastTeamPos = 'b2';
+ } else if (this.teamNumber == match.b3()) {
+ lastTeamPos = 'b3';
+ } else {
+ console.log('Position of scouted team not found.');
+ }
+ break;
+ }
+ }
+ if (lastTeamPos != '0') {
+ this.matchNumber += 1;
+ for (const match of this.matchList) {
+ if (
+ this.matchNumber == match.matchNumber() &&
+ this.setNumber == match.setNumber() &&
+ this.compLevel == match.compLevel()
+ ) {
+ if (lastTeamPos == 'r1') {
+ this.nextTeamNumber = match.r1();
+ } else if (lastTeamPos == 'r2') {
+ this.nextTeamNumber = match.r2();
+ } else if (lastTeamPos == 'r3') {
+ this.nextTeamNumber = match.r3();
+ } else if (lastTeamPos == 'b1') {
+ this.nextTeamNumber = match.b1();
+ } else if (lastTeamPos == 'b2') {
+ this.nextTeamNumber = match.b2();
+ } else if (lastTeamPos == 'b3') {
+ this.nextTeamNumber = match.b3();
+ } else {
+ console.log('Position of last team not found.');
+ }
+ break;
+ }
+ }
+ } else {
+ console.log('Last team position not found.');
+ }
+ this.matchList = [];
+ this.progressMessage = '';
+ this.errorMessage = '';
+ this.autoPhase = true;
+ this.actionList = [];
+ this.mobilityCompleted = false;
+ this.preScouting = false;
+ this.matchStartTimestamp = 0;
+ this.selectedValue = 0;
} else {
const resBuffer = await res.arrayBuffer();
const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index c0bba23..553fa1e 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -485,6 +485,11 @@
</div>
<div *ngSwitchCase="'Success'" id="Success" class="container-fluid">
<h2>Successfully submitted data.</h2>
+ <div class="d-grid gap-5" *ngIf="nextTeamNumber != ''">
+ <button class="btn btn-primary" (click)="goToNextTeam();">
+ SCOUT NEXT TEAM
+ </button>
+ </div>
</div>
<div *ngSwitchCase="'QR Code'" id="QR Code" class="container-fluid">
<span>Density:</span>
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 7f0b13d..7c33579 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -173,7 +173,7 @@
spline_goal_buffer(fbb.Release());
frc971::control_loops::drivetrain::Trajectory trajectory(
- spline_goal_buffer.message(), dt_config_);
+ spline_goal_buffer.message(), &dt_config_);
trajectory.Plan();
flatbuffers::FlatBufferBuilder traj_fbb;
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index d03bda3..492d44b 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -360,7 +360,6 @@
.MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
can_position_builder.add_talonfxs(falcons_list);
- can_position_builder.add_timestamp(right_front_->GetTimestamp());
can_position_builder.add_status(static_cast<int>(status));
builder.CheckOk(builder.Send(can_position_builder.Finish()));
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index 443fabd..1dd8efd 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -294,7 +294,6 @@
.MakeBuilder<frc971::control_loops::drivetrain::CANPosition>();
can_position_builder.add_talonfxs(falcons_list);
- can_position_builder.add_timestamp(right_front_->GetTimestamp());
can_position_builder.add_status(static_cast<int>(status));
builder.CheckOk(builder.Send(can_position_builder.Finish()));
diff --git a/y2024/BUILD b/y2024/BUILD
index 212f70e..4e13aed 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -50,11 +50,13 @@
":joystick_republish",
"//aos/events:aos_timing_report_streamer",
"//aos/events/logging:log_cat",
+ "//aos:aos_jitter",
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
"//aos/util:foxglove_websocket",
"//frc971/image_streamer:image_streamer",
"//frc971/vision:intrinsics_calibration",
+ "//aos/util:filesystem_monitor",
"//y2024/vision:viewer",
"//y2024/constants:constants_sender",
"//y2024/localizer:localizer_main",
@@ -63,7 +65,7 @@
],
data = [
":aos_config",
- "//frc971/rockpi:rockpi_config.json",
+ "//frc971/orin:orin_irq_config.json",
"//y2024/constants:constants.json",
"//y2024/vision:image_streamer_start",
"//y2024/www:www_files",
@@ -124,6 +126,7 @@
"//y2024/localizer:status_fbs",
"//y2024/localizer:visualization_fbs",
"//aos/network:timestamp_fbs",
+ "//aos/util:filesystem_fbs",
"//aos/network:remote_message_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
@@ -183,6 +186,7 @@
"//y2024/localizer:visualization_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
+ "//aos/util:filesystem_fbs",
"@com_github_foxglove_schemas//:schemas",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
index f69d9fc..a834f66 100644
--- a/y2024/autonomous/BUILD
+++ b/y2024/autonomous/BUILD
@@ -50,11 +50,13 @@
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/autonomous:user_button_localized_autonomous_actor",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:localizer_fbs",
"//y2024:constants",
+ "//y2024/constants:constants_fbs",
"//y2024/control_loops/drivetrain:drivetrain_base",
"//y2024/control_loops/superstructure:superstructure_goal_fbs",
"//y2024/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
index 5d2c10f..66b1b28 100644
--- a/y2024/autonomous/auto_splines.cc
+++ b/y2024/autonomous/auto_splines.cc
@@ -18,7 +18,7 @@
// For 2024: The field is mirrored across the center line, and is not
// rotationally symmetric. As such, we only flip the X coordinates when
// changing side of the field.
- if (alliance == aos::Alliance::kBlue) {
+ if (alliance == aos::Alliance::kRed) {
for (size_t ii = 0; ii < spline_x->size(); ++ii) {
spline_x->Mutate(ii, -spline_x->Get(ii));
}
@@ -121,4 +121,63 @@
return FixSpline(builder, multispline_builder.Finish(), alliance);
}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::MobilityAndShootSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ mobility_and_shoot_spline_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_1_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_2_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_4_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline5(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_5_, builder->fbb()),
+ alliance);
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
index 309222f..93360d3 100644
--- a/y2024/autonomous/auto_splines.h
+++ b/y2024/autonomous/auto_splines.h
@@ -19,7 +19,20 @@
public:
AutonomousSplines()
: test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/test_spline.json")) {}
+ "splines/test_spline.json")),
+ mobility_and_shoot_spline_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/mobilityandshoot.0.json")),
+ four_piece_spline_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.0.json")),
+ four_piece_spline_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.1.json")),
+ four_piece_spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.2.json")),
+ four_piece_spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.3.json")),
+ four_piece_spline_5_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.4.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -33,9 +46,39 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> MobilityAndShootSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline5(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> mobility_and_shoot_spline_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_4_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_5_;
};
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 3c7f2fa..9b4f7f8 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -12,6 +12,7 @@
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
+DEFINE_bool(do_fifth_piece, true, "");
namespace y2024::autonomous {
@@ -33,7 +34,8 @@
using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
-AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants)
: frc971::autonomous::UserButtonLocalizedAutonomousActor(
event_loop,
control_loops::drivetrain::GetDrivetrainConfig(event_loop)),
@@ -49,16 +51,60 @@
event_loop
->MakeFetcher<::y2024::control_loops::superstructure::Status>(
"/superstructure")),
+ robot_constants_(CHECK_NOTNULL(robot_constants)),
auto_splines_() {}
void AutonomousActor::Replan() {
- if (FLAGS_spline_auto) {
- test_spline_ =
- PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kForward);
+ AutonomousMode mode = robot_constants_->common()->autonomous_mode();
+ switch (mode) {
+ case AutonomousMode::NONE:
+ break;
+ case AutonomousMode::SPLINE_AUTO:
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
- starting_position_ = test_spline_->starting_position();
+ starting_position_ = test_spline_->starting_position();
+ break;
+ case AutonomousMode::MOBILITY_AND_SHOOT:
+ AOS_LOG(INFO, "Mobility & shoot replanning!");
+ mobility_and_shoot_splines_ = {PlanSpline(
+ std::bind(&AutonomousSplines::MobilityAndShootSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+
+ starting_position_ =
+ mobility_and_shoot_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ break;
+ case AutonomousMode::FOUR_PIECE:
+ AOS_LOG(INFO, "FOUR_PIECE replanning!");
+ four_piece_splines_ = {
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline4, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline5, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward)};
+
+ starting_position_ = four_piece_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ break;
}
is_planned_ = true;
@@ -73,10 +119,21 @@
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
- if (FLAGS_spline_auto) {
- SplineAuto();
- } else {
- AOS_LOG(WARNING, "No auto mode selected.");
+
+ AutonomousMode mode = robot_constants_->common()->autonomous_mode();
+ switch (mode) {
+ case AutonomousMode::NONE:
+ AOS_LOG(WARNING, "No auto mode selected.");
+ break;
+ case AutonomousMode::SPLINE_AUTO:
+ SplineAuto();
+ break;
+ case AutonomousMode::MOBILITY_AND_SHOOT:
+ MobilityAndShoot();
+ break;
+ case AutonomousMode::FOUR_PIECE:
+ FourPieceAuto();
+ break;
}
return true;
}
@@ -118,13 +175,177 @@
if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
+void AutonomousActor::MobilityAndShoot() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ uint32_t initial_shot_count = shot_count();
+
+ CHECK(mobility_and_shoot_splines_);
+
+ auto &splines = *mobility_and_shoot_splines_;
+
+ AOS_LOG(INFO, "Going to preload");
+
+ // Always be auto-aiming.
+ Aim();
+
+ if (!WaitForPreloaded()) return;
+
+ AOS_LOG(INFO, "Starting to Move");
+
+ if (!splines[0].WaitForPlan()) return;
+
+ splines[0].Start();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Got there %lf s\nNow Shooting\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count, std::chrono::seconds(5))) return;
+
+ StopFiring();
+
+ AOS_LOG(
+ INFO, "Note fired at %lf seconds\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+}
+
+void AutonomousActor::FourPieceAuto() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(four_piece_splines_);
+ auto &splines = *four_piece_splines_;
+
+ uint32_t initial_shot_count = shot_count();
+
+ // Always be aiming & firing.
+ Aim();
+ if (!WaitForPreloaded()) return;
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+ Shoot();
+
+ AOS_LOG(
+ INFO, "Shooting Preloaded Note %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForNoteFired(initial_shot_count, std::chrono::seconds(2))) return;
+
+ AOS_LOG(
+ INFO, "Shot first note %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Intake();
+ StopFiring();
+
+ AOS_LOG(
+ INFO, "Starting Spline 1 %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[0].WaitForPlan()) return;
+
+ splines[0].Start();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.01)) return;
+
+ if (!splines[1].WaitForPlan()) return;
+
+ AOS_LOG(
+ INFO, "Starting second spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ splines[1].Start();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.01)) return;
+
+ AOS_LOG(
+ INFO, "Finished second spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(250));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count + 1, std::chrono::seconds(2)))
+ return;
+
+ AOS_LOG(
+ INFO, "Shot second note, starting drive %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.01)) return;
+
+ if (!WaitForNoteFired(initial_shot_count + 3, std::chrono::seconds(5)))
+ return;
+
+ AOS_LOG(
+ INFO, "Finished 4 notes at %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!FLAGS_do_fifth_piece) {
+ AOS_LOG(INFO, "Exitting early due to --nodo_fifth_piece");
+ return;
+ }
+
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.1)) return;
+
+ AOS_LOG(
+ INFO, "Got to 5th note at %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ StopFiring();
+
+ if (!splines[4].WaitForPlan()) return;
+ splines[4].Start();
+
+ if (!splines[4].WaitForSplineDistanceRemaining(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Done with spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ AOS_LOG(
+ INFO, "Shooting last note! %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count + 4, std::chrono::seconds(5)))
+ return;
+
+ AOS_LOG(
+ INFO, "Done %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+}
+
void AutonomousActor::SendSuperstructureGoal() {
aos::Sender<control_loops::superstructure::GoalStatic>::StaticBuilder
goal_builder = superstructure_goal_sender_.MakeStaticBuilder();
goal_builder->set_intake_goal(intake_goal_);
+ if (intake_goal_ == control_loops::superstructure::IntakeGoal::INTAKE) {
+ goal_builder->set_intake_pivot(
+ control_loops::superstructure::IntakePivotGoal::DOWN);
+ } else {
+ goal_builder->set_intake_pivot(
+ control_loops::superstructure::IntakePivotGoal::UP);
+ }
goal_builder->set_note_goal(note_goal_);
goal_builder->set_fire(fire_);
+ goal_builder->set_climber_goal(
+ control_loops::superstructure::ClimberGoal::STOWED);
control_loops::superstructure::ShooterGoalStatic *shooter_goal =
goal_builder->add_shooter_goal();
@@ -151,6 +372,11 @@
SendSuperstructureGoal();
}
+void AutonomousActor::StopFiring() {
+ set_fire(false);
+ SendSuperstructureGoal();
+}
+
[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
set_preloaded(true);
SendSuperstructureGoal();
@@ -179,4 +405,54 @@
return true;
}
+uint32_t AutonomousActor::shot_count() {
+ superstructure_status_fetcher_.Fetch();
+ return superstructure_status_fetcher_->shot_count();
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForNoteFired(
+ uint32_t penultimate_target_shot_count, std::chrono::nanoseconds timeout) {
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+ aos::monotonic_clock::time_point end_time =
+ event_loop()->monotonic_now() + timeout;
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+
+ if (shot_count() > penultimate_target_shot_count ||
+ event_loop()->monotonic_now() > end_time) {
+ return true;
+ }
+ }
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForCatapultReady() {
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->state() ==
+ control_loops::superstructure::SuperstructureState::READY);
+ }
+
+ SendSuperstructureGoal();
+
+ return true;
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index 6796c94..fd5038d 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -8,6 +8,8 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2024/autonomous/auto_splines.h"
+#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/superstructure/superstructure_goal_static.h"
#include "y2024/control_loops/superstructure/superstructure_status_static.h"
@@ -16,7 +18,8 @@
class AutonomousActor
: public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
public:
- explicit AutonomousActor(::aos::EventLoop *event_loop);
+ explicit AutonomousActor(::aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants);
private:
void set_intake_goal(control_loops::superstructure::IntakeGoal intake_goal) {
@@ -35,13 +38,21 @@
void Reset() override;
void SplineAuto();
+ void MobilityAndShoot();
+ void FourPieceAuto();
void SendSuperstructureGoal();
void Intake();
void Aim();
void Shoot();
+ void StopFiring();
+
+ uint32_t shot_count();
[[nodiscard]] bool WaitForPreloaded();
+ [[nodiscard]] bool WaitForNoteFired(uint32_t penultimate_target_shot_count,
+ std::chrono::nanoseconds timeout);
+ [[nodiscard]] bool WaitForCatapultReady();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
@@ -52,15 +63,19 @@
aos::Fetcher<y2024::control_loops::superstructure::Status>
superstructure_status_fetcher_;
+ const Constants *robot_constants_;
+
AutonomousSplines auto_splines_;
std::optional<SplineHandle> test_spline_;
+ std::optional<std::array<SplineHandle, 1>> mobility_and_shoot_splines_;
+ std::optional<std::array<SplineHandle, 5>> four_piece_splines_;
control_loops::superstructure::IntakeGoal intake_goal_ =
control_loops::superstructure::IntakeGoal::NONE;
control_loops::superstructure::NoteGoal note_goal_ =
- control_loops::superstructure::NoteGoal::NONE;
+ control_loops::superstructure::NoteGoal::CATAPULT;
bool auto_aim_ = false;
bool fire_ = false;
@@ -69,4 +84,4 @@
} // namespace y2024::autonomous
-#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
\ No newline at end of file
+#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
diff --git a/y2024/autonomous/autonomous_actor_main.cc b/y2024/autonomous/autonomous_actor_main.cc
index 4a8e146..5049b0f 100644
--- a/y2024/autonomous/autonomous_actor_main.cc
+++ b/y2024/autonomous/autonomous_actor_main.cc
@@ -2,6 +2,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "y2024/autonomous/autonomous_actor.h"
int main(int argc, char *argv[]) {
@@ -10,8 +11,16 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+
+ const y2024::Constants *robot_constants = &constants_fetcher.constants();
+
::aos::ShmEventLoop event_loop(&config.message());
- ::y2024::autonomous::AutonomousActor autonomous(&event_loop);
+ ::y2024::autonomous::AutonomousActor autonomous(&event_loop, robot_constants);
event_loop.Run();
diff --git a/y2024/autonomous/splines/five_note.0.json b/y2024/autonomous/splines/five_note.0.json
new file mode 100644
index 0000000..ba8ac2c
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.0.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -6.96,
+ -6.616444808957066,
+ -6.440474125870341,
+ -6.410863453750628,
+ -6.106827763281826,
+ -5.963275576067123
+ ],
+ "spline_y": [
+ 0.474,
+ 0.474,
+ 0.3795291901917933,
+ 0.4482348806874367,
+ 0.31444590142149437,
+ 0.26227873892724274
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.1.json b/y2024/autonomous/splines/five_note.1.json
new file mode 100644
index 0000000..d65db75
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.1.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -5.963275576067123,
+ -6.043899374861988,
+ -6.14218730470054,
+ -6.299979097775238,
+ -6.333362095101602,
+ -6.385753156016853
+ ],
+ "spline_y": [
+ 0.26227873892724274,
+ 0.2915775945855339,
+ 0.27680408013743,
+ 0.19285548109740952,
+ 0.09423634731960748,
+ -0.012742650262493882
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 0.9,
+ "start_distance": 0.0,
+ "end_distance": 10.0
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.2.json b/y2024/autonomous/splines/five_note.2.json
new file mode 100644
index 0000000..e67e8f0
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.2.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -6.385753156016853,
+ -6.038148386644435,
+ -5.498099209905131,
+ -5.500010615292536,
+ -5.505673744840998,
+ -5.450725524128315
+ ],
+ "spline_y": [
+ -0.012742650262493882,
+ 0.6970427431345343,
+ 1.0584941253106013,
+ 1.0226571920273129,
+ 1.4628922679287215,
+ 2.245294280677646
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 0.4,
+ "start_distance": 1.5,
+ "end_distance": 10.0
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.3.json b/y2024/autonomous/splines/five_note.3.json
new file mode 100644
index 0000000..148c79d
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.3.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -5.450725524128315,
+ -5.4125513915961285,
+ -3.9062446310670764,
+ -2.1352203510523458,
+ -1.1691608738981074,
+ -0.19389194554058786
+ ],
+ "spline_y": [
+ 2.245294280677646,
+ 2.7888517924722676,
+ 3.9025565085593934,
+ 2.6507161302136577,
+ 2.1538810971324764,
+ 1.7882868785936012
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 4.5
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10.0
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 6.4,
+ "start_distance": 0.0,
+ "end_distance": 100.0
+ }
+ ]
+}
diff --git a/y2024/autonomous/splines/five_note.4.json b/y2024/autonomous/splines/five_note.4.json
new file mode 100644
index 0000000..93421e2
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.4.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.19389194554058786,
+ -1.169167650726454,
+ -2.1352271278806922,
+ -3.6552329072641223,
+ -4.682377123355169,
+ -5.357290416833793
+ ],
+ "spline_y": [
+ 1.7882868785936012,
+ 2.153883637528455,
+ 2.650718670609636,
+ 2.3043000750345737,
+ 1.9165787798578524,
+ 1.7991159872216307
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 4.5
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10.000000000000007
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 6.4,
+ "start_distance": 0.0,
+ "end_distance": 100.0
+ }
+ ]
+}
diff --git a/y2024/autonomous/splines/mobilityandshoot.0.json b/y2024/autonomous/splines/mobilityandshoot.0.json
new file mode 100644
index 0000000..22a44cf
--- /dev/null
+++ b/y2024/autonomous/splines/mobilityandshoot.0.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-7.7, -7.184654377431906, -6.673491731517509, -6.769334727626459, -6.098433754863813, -5.6], "spline_y": [-0.9, -0.9, -0.9, -0.9, -0.9, -0.9], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 4.000000000000021}, {"constraint_type": "VELOCITY", "value": 1.0}]}
diff --git a/y2024/autonomous/splines/test_spline.json b/y2024/autonomous/splines/test_spline.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2024/autonomous/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 5cee276..974fb60 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -8,13 +8,13 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-c-1_cam-24-05_2024-03-08_17-16-02.325066070.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_2024-03-08_17-16-02.325390283.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-07_2024-03-08_17-16-02.325267121.json' %}
},
{
"calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json' %}
@@ -23,23 +23,23 @@
"robot": {
{% set _ = intake_pivot_zero.update(
{
- "measured_absolute_position" : 3.2990161941868
+ "measured_absolute_position" : 3.49222521810232
}
) %}
"intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
- "measured_absolute_position" : 0.00260967415741875
+ "measured_absolute_position" : 0.00861798094474761
}
) %}
"zeroing_constants": {{ climber_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -0.935529777248618 + 1.83632555414775 + 0.0431080619919798 - 0.493015437796464 }}
+ "potentiometer_offset": {{ -0.935529777248618 + 1.83632555414775 + 0.0431080619919798 - 0.493015437796464 + 0.001602382648064 +0.00194716776942403 - 0.030467594535944}}
},
"catapult_constants": {
{% set _ = catapult_zero.update(
{
- "measured_absolute_position" : 0.741253220327565
+ "measured_absolute_position" : 0.72750793510745
}
) %}
"zeroing_constants": {{ catapult_zero | tojson(indent=2)}},
@@ -48,29 +48,29 @@
"altitude_constants": {
{% set _ = altitude_zero.update(
{
- "measured_absolute_position" : 0.130841088837793
+ "measured_absolute_position" : 0.1877
}
) %}
"zeroing_constants": {{ altitude_zero | tojson(indent=2)}},
- "potentiometer_offset": -0.15316323147786
+ "potentiometer_offset": -0.16416323147786
},
"turret_constants": {
{% set _ = turret_zero.update(
{
- "measured_absolute_position" : 0.138686395993591
+ "measured_absolute_position" : 0.961143535321169
}
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -6.47164779835404 }}
+ "potentiometer_offset": {{ -6.47164779835404 - 0.0711209027239817 + 1.0576004531907 }}
},
"extend_constants": {
{% set _ = extend_zero.update(
{
- "measured_absolute_position" : 0.0314256815130559
+ "measured_absolute_position" : 0.1547
}
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 }}
+ "potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 - 0.1577}}
}
},
{% include 'y2024/constants/common.json' %}
diff --git a/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json b/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json
deleted file mode 100755
index d013e2a..0000000
--- a/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json
+++ /dev/null
@@ -1,46 +0,0 @@
-{
- "node_name": "imu",
- "team_number": 971,
- "intrinsics": [
- 647.822815,
- 0.0,
- 715.37616,
- 0.0,
- 647.799316,
- 494.638641,
- 0.0,
- 0.0,
- 1.0
- ],
- "fixed_extrinsics": {
- "data": [
- 1.0,
- -0.0,
- 0.0,
- 0.111049,
- 0.0,
- 0.258819,
- 0.965926,
- 0.263806,
- -0.0,
- -0.965926,
- 0.258819,
- 0.347685,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ]
- },
- "dist_coeffs": [
- -0.2423,
- 0.057169,
- 0.000302,
- 0.000016,
- -0.005638
- ],
- "calibration_timestamp": 1708833147338466592,
- "camera_id": "24-07",
- "camera_number": 0,
- "reprojection_error": 1.362672
-}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json b/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json
index fbe79d5..c0ec0d8 100755
--- a/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json
+++ b/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json
@@ -41,6 +41,6 @@
],
"calibration_timestamp": 1708820514420797344,
"camera_id": "24-08",
- "camera_number": 1,
+ "camera_number": 0,
"reprojection_error": 1.591953
-}
\ No newline at end of file
+}
diff --git a/y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_2024-03-08_17-16-02.325390283.json
similarity index 61%
rename from y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json
rename to y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_2024-03-08_17-16-02.325390283.json
index 0eb10db..48da468 100755
--- a/y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_2024-03-08_17-16-02.325390283.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- -1.0,
- 0.0,
- 0.0,
- 0.111049,
- -0.0,
- -0.258819,
- -0.965926,
- -0.263806,
- 0.0,
- -0.965926,
- 0.258819,
- 0.347685,
+ -0.997807,
+ 0.015704,
+ -0.064302,
+ 0.134715,
+ 0.058111,
+ -0.25731,
+ -0.96458,
+ -0.273849,
+ -0.031694,
+ -0.966201,
+ 0.255833,
+ 0.295681,
0.0,
0.0,
0.0,
@@ -39,8 +39,8 @@
0.000005,
-0.006342
],
- "calibration_timestamp": 409229245444672,
+ "calibration_timestamp": 1709946962325390283,
"camera_id": "24-06",
- "camera_number": 1,
+ "camera_number": 0,
"reprojection_error": 1.344104
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-07_2024-03-08_17-16-02.325267121.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-07_2024-03-08_17-16-02.325267121.json
new file mode 100755
index 0000000..56072d4
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-07_2024-03-08_17-16-02.325267121.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 647.822815,
+ 0.0,
+ 715.37616,
+ 0.0,
+ 647.799316,
+ 494.638641,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.016995,
+ 0.002328,
+ 0.999853,
+ 0.35278,
+ 0.999839,
+ -0.005785,
+ -0.016981,
+ 0.237526,
+ 0.005745,
+ 0.999981,
+ -0.002426,
+ 0.387276,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.2423,
+ 0.057169,
+ 0.000302,
+ 0.000016,
+ -0.005638
+ ],
+ "calibration_timestamp": 1709946962325267121,
+ "camera_id": "24-07",
+ "camera_number": 1,
+ "reprojection_error": 1.362672
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-c-1_cam-24-05_2024-03-08_17-16-02.325066070.json b/y2024/constants/calib_files/calibration_orin-971-c-1_cam-24-05_2024-03-08_17-16-02.325066070.json
new file mode 100755
index 0000000..c1d4e60
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-c-1_cam-24-05_2024-03-08_17-16-02.325066070.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 971,
+ "intrinsics": [
+ 648.360168,
+ 0.0,
+ 729.818665,
+ 0.0,
+ 648.210327,
+ 641.988037,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.999556,
+ -0.026613,
+ 0.013428,
+ 0.146979,
+ -0.006922,
+ 0.230925,
+ 0.972947,
+ 0.30388,
+ -0.028994,
+ -0.972608,
+ 0.230638,
+ 0.32572,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.255473,
+ 0.068444,
+ 0.000028,
+ -0.000078,
+ -0.008004
+ ],
+ "calibration_timestamp": 1709946962325066070,
+ "camera_id": "24-05",
+ "camera_number": 1,
+ "reprojection_error": 1.058851
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json b/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json
deleted file mode 100755
index 317e453..0000000
--- a/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json
+++ /dev/null
@@ -1,46 +0,0 @@
-{
- "node_name": "orin1",
- "team_number": 971,
- "intrinsics": [
- 648.360168,
- 0.0,
- 729.818665,
- 0.0,
- 648.210327,
- 641.988037,
- 0.0,
- 0.0,
- 1.0
- ],
- "fixed_extrinsics": {
- "data": [
- 0.0,
- 0.0,
- 1.0,
- 0.284397,
- -1.0,
- 0.0,
- 0.0,
- 0.226771,
- 0.0,
- -1.0,
- 0.0,
- 0.442951,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ]
- },
- "dist_coeffs": [
- -0.255473,
- 0.068444,
- 0.000028,
- -0.000078,
- -0.008004
- ],
- "calibration_timestamp": 409227793683328,
- "camera_id": "24-05",
- "camera_number": 0,
- "reprojection_error": 1.058851
-}
\ No newline at end of file
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 1050815..25fae23 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -4,30 +4,55 @@
"target_map": {% include 'y2024/vision/maps/target_map.json' %},
"shooter_interpolation_table": [
{
- "distance_from_goal": 5.0,
+ "distance_from_goal": 0.7,
"shot_params": {
- "shot_velocity": 0.0,
- "shot_altitude_angle": 0.0,
- "shot_catapult_angle": 0.0,
- "shot_speed_over_ground": 2.0
+ "shot_altitude_angle": 0.85,
+ "shot_speed_over_ground": 8.0
}
},
{
- "distance_from_goal": 10.0,
+ "distance_from_goal": 1.24,
"shot_params": {
- "shot_velocity": 0.0,
- "shot_altitude_angle": 0.0,
- "shot_catapult_angle": 0.0,
- "shot_speed_over_ground": 4.0
+ "shot_altitude_angle": 0.85,
+ "shot_speed_over_ground": 8.0
+ }
+ },
+ {
+ "distance_from_goal": 1.904,
+ "shot_params": {
+ "shot_altitude_angle": 0.73,
+ "shot_speed_over_ground": 8.0
+ }
+ },
+ // 2.2 -> high.
+ {
+ "distance_from_goal": 2.744,
+ "shot_params": {
+ "shot_altitude_angle": 0.62,
+ "shot_speed_over_ground": 8.0
+ }
+ },
+ {
+ "distance_from_goal": 3.274,
+ "shot_params": {
+ "shot_altitude_angle": 0.58,
+ "shot_speed_over_ground": 8.0
+ }
+ },
+ {
+ "distance_from_goal": 4.00,
+ "shot_params": {
+ "shot_altitude_angle": 0.54,
+ "shot_speed_over_ground": 8.0
}
}
],
"intake_roller_voltages": {
- "spitting": -4.0,
- "intaking": 12.0
+ "spitting": -6.0,
+ "intaking": 9.0
},
"intake_pivot_set_points": {
- "extended": -0.03,
+ "extended": 0.045,
"retracted": 1.73
},
"intake_pivot": {
@@ -38,8 +63,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 4.0,
- "max_acceleration": 10.0
+ "max_velocity": 6.0,
+ "max_acceleration": 40.0
},
"range": {
"lower_hard": -0.2,
@@ -56,21 +81,21 @@
"intake_pivot_supply_current_limit": 40,
"intake_pivot_stator_current_limit": 100,
"intake_roller_supply_current_limit": 20,
- "intake_roller_stator_current_limit": 50,
+ "intake_roller_stator_current_limit": 100,
"transfer_roller_supply_current_limit": 20,
"transfer_roller_stator_current_limit": 50,
- "drivetrain_supply_current_limit": 35,
- "drivetrain_stator_current_limit": 60,
+ "drivetrain_supply_current_limit": 50,
+ "drivetrain_stator_current_limit": 200,
"climber_supply_current_limit": 30,
"climber_stator_current_limit": 100,
- "extend_supply_current_limit": 20,
- "extend_stator_current_limit": 100,
- "extend_roller_supply_current_limit": 60,
- "extend_roller_stator_current_limit": 200,
- "turret_supply_current_limit": 20,
- "turret_stator_current_limit": 40,
- "altitude_supply_current_limit": 10,
- "altitude_stator_current_limit": 60,
+ "extend_supply_current_limit": 30,
+ "extend_stator_current_limit": 180,
+ "extend_roller_supply_current_limit": 50,
+ "extend_roller_stator_current_limit": 180,
+ "turret_supply_current_limit": 30,
+ "turret_stator_current_limit": 80,
+ "altitude_supply_current_limit": 30,
+ "altitude_stator_current_limit": 150,
"catapult_supply_current_limit": 60,
"catapult_stator_current_limit": 250,
"retention_roller_stator_current_limit": 20,
@@ -79,18 +104,18 @@
"retention_roller_supply_current_limit": 10
},
"transfer_roller_voltages": {
- "transfer_in": 12.0,
+ "transfer_in": 9.0,
"transfer_out": -4.0,
"extend_moving": 4.0
},
"extend_roller_voltages": {
- "scoring": 12.0,
+ "scoring": 6.0,
"reversing": -4.0
},
"climber_set_points": {
"full_extend": -0.005,
- "stowed": -0.35,
- "retract": -0.478
+ "stowed": -0.442,
+ "retract": -0.472
},
"climber": {
"zeroing_voltage": 3.0,
@@ -104,9 +129,9 @@
"max_acceleration": 3.0
},
"range": {
- "lower_hard": -0.488,
+ "lower_hard": -0.495,
"upper_hard": 0.005,
- "lower": -0.478,
+ "lower": -0.492,
"upper": -0.005
},
"loop": {% include 'y2024/control_loops/superstructure/climber/integral_climber_plant.json' %}
@@ -139,8 +164,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 3.0,
- "max_acceleration": 5.0
+ "max_velocity": 5.0,
+ "max_acceleration": 30.0
},
"range": {
"lower_hard": -0.01,
@@ -158,8 +183,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 2.0,
- "max_acceleration": 5.0
+ "max_velocity": 12.0,
+ "max_acceleration": 30.0
},
"range": {
"lower_hard": -4.8,
@@ -177,8 +202,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 0.1,
- "max_acceleration": 0.3
+ "max_velocity": 2.0,
+ "max_acceleration": 10.0
},
"range": {
"lower_hard": -0.005,
@@ -196,7 +221,7 @@
"storage_order": "ColMajor",
// The data field contains the x, y and z
// coordinates of the speaker on the red alliance
- "data": [8.0645, 1.4435, 2.0705]
+ "data": [8.209, 1.4435, 2.0705]
},
"theta": 0.0
},
@@ -207,7 +232,7 @@
"storage_order": "ColMajor",
// The data field contains the x, y and z
// coordinates of the speaker on the blue alliance
- "data": [-8.0645, 1.4435, 2.0705]
+ "data": [-8.209, 1.4435, 2.0705]
},
"theta": 0.0
}
@@ -215,7 +240,7 @@
"altitude_loading_position": 0.02,
"turret_loading_position": 0.58,
"catapult_return_position": 0.0,
- "min_altitude_shooting_angle": 0.55,
+ "min_altitude_shooting_angle": 0.4,
"max_altitude_shooting_angle": 0.89,
"retention_roller_voltages": {
"retaining": 1.5,
@@ -223,8 +248,8 @@
},
// TODO(Filip): Update the speaker and amp shooter setpoints
"shooter_speaker_set_point": {
- "turret_position": 0.0,
- "altitude_position": 0.75,
+ "turret_position": 0.22,
+ "altitude_position": 0.85,
"shot_velocity": 0.0
},
"shooter_podium_set_point":{
@@ -233,10 +258,16 @@
"shot_velocity": 0.0
},
"extend_set_points": {
- "trap": 0.46,
- "amp": 0.2,
+ "trap": 0.40,
+ "amp": 0.35,
"catapult": 0.017,
"retracted": 0.017
},
- "turret_avoid_extend_collision_position": 0.0
+ "turret_avoid_extend_collision_position": 0.0,
+ "altitude_avoid_extend_collision_position": 0.3,
+ "autonomous_mode": "FOUR_PIECE",
+ "ignore_targets": {
+ "red": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16],
+ "blue": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16]
+ }
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 31e2057..c679128 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -115,6 +115,16 @@
retracted:double (id: 3);
}
+enum AutonomousMode : ubyte {
+ NONE = 0,
+ // Simple test S-spline auto mode
+ SPLINE_AUTO = 1,
+ // Simple drive-and-shoot to pick up at most one game piece.
+ MOBILITY_AND_SHOOT = 2,
+ // Auto to pick up four game pieces.
+ FOUR_PIECE = 3,
+}
+
table RobotConstants {
intake_constants:frc971.zeroing.AbsoluteEncoderZeroingConstants (id: 0);
climber_constants:PotAndAbsEncoderConstants (id: 1);
@@ -147,6 +157,13 @@
spitting:double (id: 1);
}
+// Set of april tag targets, by april tag ID, to ignore when on a
+// given alliance.
+table IgnoreTargets {
+ red:[uint64] (id: 0);
+ blue:[uint64] (id: 1);
+}
+
// Common table for constants unrelated to the robot
table Common {
target_map:frc971.vision.TargetMap (id: 0);
@@ -173,10 +190,13 @@
max_altitude_shooting_angle: double (id: 25);
shooter_speaker_set_point: ShooterSetPoint (id: 21);
shooter_podium_set_point: ShooterSetPoint (id: 22);
- extend_set_points:ExtendSetPoints (id: 23);
+ extend_set_points:ExtendSetPoints (id: 23);
// The position to move the turret to when avoiding collision
// with the extend when the extend is moving to amp/trap position.
turret_avoid_extend_collision_position: double (id: 24);
+ altitude_avoid_extend_collision_position: double (id: 28);
+ autonomous_mode:AutonomousMode (id: 26);
+ ignore_targets:IgnoreTargets (id: 27);
}
table Constants {
diff --git a/y2024/control_loops/python/altitude.py b/y2024/control_loops/python/altitude.py
index d302b79..a06bda7 100644
--- a/y2024/control_loops/python/altitude.py
+++ b/y2024/control_loops/python/altitude.py
@@ -24,14 +24,15 @@
motor=control_loop.KrakenFOC(),
G=(16.0 / 60.0) * (16.0 / 162.0),
# 4340 in^ lb
- J=1.27,
+ J=1.2,
q_pos=0.60,
q_vel=8.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
kalman_r_position=0.05,
- radius=10.5 * 0.0254)
+ radius=10.5 * 0.0254,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/catapult.py b/y2024/control_loops/python/catapult.py
index 49c6305..406f56e 100644
--- a/y2024/control_loops/python/catapult.py
+++ b/y2024/control_loops/python/catapult.py
@@ -40,7 +40,8 @@
kalman_q_voltage=0.7,
kalman_r_position=0.05,
radius=12 * 0.0254,
- delayed_u=1)
+ delayed_u=1,
+ dt=0.005)
kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
name='Catapult',
@@ -57,7 +58,8 @@
kalman_q_voltage=0.7,
kalman_r_position=0.05,
radius=12 * 0.0254,
- delayed_u=1)
+ delayed_u=1,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/climber.py b/y2024/control_loops/python/climber.py
index 997cf28..1b4e3d2 100644
--- a/y2024/control_loops/python/climber.py
+++ b/y2024/control_loops/python/climber.py
@@ -26,6 +26,7 @@
kalman_q_vel=2.00,
kalman_q_voltage=35.0,
kalman_r_position=0.05,
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
index 01f6716..b52f70d 100644
--- a/y2024/control_loops/python/drivetrain.py
+++ b/y2024/control_loops/python/drivetrain.py
@@ -16,8 +16,8 @@
J=6.5,
mass=68.0,
# TODO(austin): Measure radius a bit better.
- robot_radius=0.39,
- wheel_radius=2 * 0.0254,
+ robot_radius=0.32,
+ wheel_radius=2 * 0.0254 * 3.7592 / 3.825,
motor_type=control_loop.KrakenFOC(),
num_motors=2,
G=(14.0 / 52.0) * (36.0 / 56.0),
@@ -28,6 +28,7 @@
force=True,
kf_q_voltage=1.0,
controller_poles=[0.82, 0.82],
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/extend.py b/y2024/control_loops/python/extend.py
index f95c995..e8f066b 100644
--- a/y2024/control_loops/python/extend.py
+++ b/y2024/control_loops/python/extend.py
@@ -31,6 +31,7 @@
kalman_q_vel=2.0,
kalman_q_voltage=8.0,
kalman_r_position=0.05,
+ dt=0.005,
)
diff --git a/y2024/control_loops/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
index 5ea5c3f..c240d01 100644
--- a/y2024/control_loops/python/intake_pivot.py
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -29,7 +29,8 @@
kalman_q_voltage=1.0,
kalman_r_position=0.05,
radius=6.85 * 0.0254,
- enable_voltage_error=False)
+ enable_voltage_error=False,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/python/turret.py b/y2024/control_loops/python/turret.py
index 41cc48a..07c3d90 100644
--- a/y2024/control_loops/python/turret.py
+++ b/y2024/control_loops/python/turret.py
@@ -29,7 +29,8 @@
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
kalman_r_position=0.05,
- radius=24 * 0.0254)
+ radius=24 * 0.0254,
+ dt=0.005)
def main(argv):
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 5bb1d19..a21c09d 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -9,7 +9,7 @@
using y2024::control_loops::superstructure::Aimer;
// When the turret is at 0 the note will be leaving the robot at PI.
-static constexpr double kTurretZeroOffset = M_PI;
+static constexpr double kTurretZeroOffset = M_PI - 0.22;
Aimer::Aimer(aos::EventLoop *event_loop,
const y2024::Constants *robot_constants)
@@ -29,12 +29,20 @@
frc971::control_loops::aiming::ShotMode shot_mode,
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
*turret_goal) {
+ if (status == nullptr) {
+ return;
+ }
const frc971::control_loops::Pose robot_pose({status->x(), status->y(), 0},
status->theta());
- joystick_state_fetcher_.Fetch();
- CHECK_NOTNULL(joystick_state_fetcher_.get());
+ aos::Alliance alliance = aos::Alliance::kRed;
+ if (!joystick_state_fetcher_.Fetch() && !received_joystick_state_) {
+ received_joystick_state_ = false;
+ } else {
+ received_joystick_state_ = true;
- aos::Alliance alliance = joystick_state_fetcher_->alliance();
+ CHECK_NOTNULL(joystick_state_fetcher_.get());
+ alliance = joystick_state_fetcher_->alliance();
+ }
const frc971::control_loops::Pose red_alliance_goal(
frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
@@ -68,12 +76,11 @@
robot_constants_->common()->turret()->range()),
interpolation_table_.Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.0, kTurretZeroOffset},
+ /*wrap_mode=*/0.15, kTurretZeroOffset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
turret_goal->set_unsafe_goal(current_goal_.position);
- turret_goal->set_goal_velocity(current_goal_.velocity);
}
flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 21d427a..9bec187 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -45,6 +45,8 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
frc971::control_loops::aiming::TurretGoal current_goal_;
+
+ bool received_joystick_state_ = false;
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index cc00454..4421e15 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -10,7 +10,8 @@
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-constexpr double kCatapultActivationThreshold = 0.01;
+constexpr double kCatapultActivationTurretThreshold = 0.03;
+constexpr double kCatapultActivationAltitudeThreshold = 0.01;
Shooter::Shooter(aos::EventLoop *event_loop, const Constants *robot_constants)
: drivetrain_status_fetcher_(
@@ -44,7 +45,7 @@
const double extend_goal, double *max_extend_position,
double *min_extend_position, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
- flatbuffers::FlatBufferBuilder *fbb,
+ NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now) {
drivetrain_status_fetcher_.Fetch();
@@ -68,6 +69,15 @@
aos::fbs::FixedStackAllocator<aos::fbs::Builder<
frc971::control_loops::
StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
+ auto_aim_allocator;
+
+ aos::fbs::Builder<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic>
+ auto_aim_goal_builder(&auto_aim_allocator);
+
+ aos::fbs::FixedStackAllocator<aos::fbs::Builder<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
altitude_allocator;
aos::fbs::Builder<
@@ -96,8 +106,18 @@
bool aiming = false;
- if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
- (!piece_loaded && state_ == CatapultState::READY)) {
+ if (requested_note_goal == NoteGoal::AMP ||
+ requested_note_goal == NoteGoal::TRAP) {
+ // Being asked to amp, lift the altitude up.
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ turret_goal_builder.get(),
+ robot_constants_->common()->turret_loading_position());
+
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ altitude_goal_builder.get(),
+ robot_constants_->common()->altitude_avoid_extend_collision_position());
+ } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+ (!piece_loaded && state_ == CatapultState::READY)) {
// We don't have the note so we should be ready to intake it.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
turret_goal_builder.get(),
@@ -106,20 +126,23 @@
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(),
robot_constants_->common()->altitude_loading_position());
-
} else {
// We have a game piece, lets start aiming.
if (drivetrain_status_fetcher_.get() != nullptr) {
aiming = true;
- aimer_.Update(drivetrain_status_fetcher_.get(),
- frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
- turret_goal_builder.get());
}
}
+ // Auto aim builder is a dummy so we get a status when we aren't aiming.
+ aimer_.Update(
+ drivetrain_status_fetcher_.get(),
+ frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
+ aiming ? turret_goal_builder.get() : auto_aim_goal_builder.get());
+
// We have a game piece and are being asked to aim.
constants::Values::ShotParams shot_params;
- if (piece_loaded && shooter_goal != nullptr && shooter_goal->auto_aim() &&
+ if ((piece_loaded || state_ == CatapultState::FIRING) &&
+ shooter_goal != nullptr && shooter_goal->auto_aim() &&
interpolation_table_.GetInRange(distance_to_goal, &shot_params)) {
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(), shot_params.shot_altitude_angle);
@@ -130,23 +153,30 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (piece_loaded || state_ == CatapultState::FIRING) &&
shooter_goal->has_turret_position())
? shooter_goal->turret_position()
: &turret_goal_builder->AsFlatbuffer();
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (piece_loaded || state_ == CatapultState::FIRING) &&
shooter_goal->has_altitude_position())
? shooter_goal->altitude_position()
: &altitude_goal_builder->AsFlatbuffer();
- bool subsystems_in_range =
+ const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
- kCatapultActivationThreshold &&
- std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationThreshold &&
- altitude_.estimated_position() >
- robot_constants_->common()->min_altitude_shooting_angle());
+ kCatapultActivationTurretThreshold);
+ const bool altitude_in_range =
+ (std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
+ kCatapultActivationAltitudeThreshold);
+ const bool altitude_above_min_angle =
+ (altitude_.estimated_position() >
+ robot_constants_->common()->min_altitude_shooting_angle());
+
+ bool subsystems_in_range =
+ (turret_in_range && altitude_in_range && altitude_above_min_angle);
const bool disabled = turret_.Correct(turret_goal, position->turret(),
turret_output == nullptr);
@@ -157,6 +187,20 @@
.extend_position = extend_position},
turret_goal->unsafe_goal(), extend_goal);
+ if (!CatapultRetracted()) {
+ altitude_.set_min_position(
+ robot_constants_->common()->min_altitude_shooting_angle());
+ } else {
+ altitude_.clear_min_position();
+ }
+
+ if (!CatapultRetracted()) {
+ altitude_.set_min_position(
+ robot_constants_->common()->min_altitude_shooting_angle());
+ } else {
+ altitude_.clear_min_position();
+ }
+
turret_.set_min_position(collision_avoidance->min_turret_goal());
turret_.set_max_position(collision_avoidance->max_turret_goal());
@@ -213,8 +257,8 @@
state_ = CatapultState::RETRACTING;
}
- constexpr double kLoadingAcceleration = 20.0;
- constexpr double kLoadingDeceleration = 10.0;
+ constexpr double kLoadingAcceleration = 40.0;
+ constexpr double kLoadingDeceleration = 20.0;
switch (state_) {
case CatapultState::READY:
@@ -259,6 +303,7 @@
catapult_.set_unprofiled_goal(2.0, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
+ ++shot_count_;
} else {
break;
}
@@ -291,9 +336,7 @@
}
flatbuffers::Offset<AimerStatus> aimer_offset;
- if (aiming) {
- aimer_offset = aimer_.PopulateStatus(fbb);
- }
+ aimer_offset = aimer_.PopulateStatus(fbb);
y2024::control_loops::superstructure::ShooterStatus::Builder status_builder(
*fbb);
@@ -301,9 +344,11 @@
status_builder.add_altitude(altitude_status_offset);
status_builder.add_catapult(catapult_status_offset);
status_builder.add_catapult_state(state_);
- if (aiming) {
- status_builder.add_aimer(aimer_offset);
- }
+ status_builder.add_turret_in_range(turret_in_range);
+ status_builder.add_altitude_in_range(altitude_in_range);
+ status_builder.add_altitude_above_min_angle(altitude_above_min_angle);
+ status_builder.add_auto_aiming(aiming);
+ status_builder.add_aimer(aimer_offset);
return status_builder.Finish();
}
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 2571cab..5363288 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -106,12 +106,25 @@
const double extend_goal, double *max_extend_position,
double *min_extend_position, const double intake_pivot_position,
double *max_turret_intake_position, double *min_intake_pivot_position,
- flatbuffers::FlatBufferBuilder *fbb,
+ NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now);
+ bool loaded() const { return state_ == CatapultState::LOADED; }
+
+ uint32_t shot_count() const { return shot_count_; }
+
+ bool Firing() const {
+ return state_ != CatapultState::READY && state_ != CatapultState::LOADED &&
+ state_ != CatapultState::RETRACTING;
+ }
+
private:
CatapultState state_ = CatapultState::RETRACTING;
+ bool CatapultRetracted() const {
+ return catapult_.estimated_position() < 0.5;
+ }
+
bool CatapultClose() const {
return (std::abs(catapult_.estimated_position() -
catapult_.unprofiled_goal(0, 0)) < 0.05 &&
@@ -121,9 +134,6 @@
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- aos::Fetcher<y2024::control_loops::superstructure::CANPosition>
- superstructure_can_position_fetcher_;
-
const Constants *robot_constants_;
CatapultSubsystem catapult_;
@@ -138,6 +148,8 @@
interpolation_table_;
Debouncer debouncer_;
+
+ uint32_t shot_count_ = 0;
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index dc9369e..d3e98ab 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -16,17 +16,12 @@
// continue.
constexpr double kExtendThreshold = 0.01;
-constexpr double kTurretLoadingThreshold = 0.01;
-constexpr double kAltitudeLoadingThreshold = 0.01;
+constexpr double kTurretLoadingThreshold = 0.05;
+constexpr double kAltitudeLoadingThreshold = 0.02;
constexpr std::chrono::milliseconds kExtraIntakingTime =
std::chrono::milliseconds(500);
-// Exit catapult loading state after this much time if we never
-// trigger any beambreaks.
-constexpr std::chrono::milliseconds kMaxCatapultLoadingTime =
- std::chrono::milliseconds(3000);
-
namespace y2024::control_loops::superstructure {
using ::aos::monotonic_clock;
@@ -54,8 +49,10 @@
shooter_(event_loop, robot_constants_),
extend_(
robot_constants_->common()->extend(),
- robot_constants_->robot()->extend_constants()->zeroing_constants()) {
- event_loop->SetRuntimeRealtimePriority(30);
+ robot_constants_->robot()->extend_constants()->zeroing_constants()),
+ extend_debouncer_(std::chrono::milliseconds(30),
+ std::chrono::milliseconds(8)) {
+ event_loop->SetRuntimeRealtimePriority(37);
}
bool PositionNear(double position, double goal, double threshold) {
@@ -79,23 +76,45 @@
OutputT output_struct;
+ extend_debouncer_.Update(position->extend_beambreak(), timestamp);
+ const bool extend_beambreak = extend_debouncer_.state();
+
// Handle Climber Goal separately from main superstructure state machine
double climber_position =
robot_constants_->common()->climber_set_points()->retract();
+ double climber_velocity = robot_constants_->common()
+ ->climber()
+ ->default_profile_params()
+ ->max_velocity();
+ const double climber_accel = robot_constants_->common()
+ ->climber()
+ ->default_profile_params()
+ ->max_acceleration();
+
if (unsafe_goal != nullptr) {
switch (unsafe_goal->climber_goal()) {
case ClimberGoal::FULL_EXTEND:
climber_position =
robot_constants_->common()->climber_set_points()->full_extend();
+ // The climber can go reasonably fast when extending out.
+ climber_velocity = 0.5;
+
+ if (unsafe_goal->slow_climber()) {
+ climber_velocity = 0.01;
+ }
break;
case ClimberGoal::RETRACT:
climber_position =
robot_constants_->common()->climber_set_points()->retract();
+ // Keep the climber slower while retracting.
+ climber_velocity = 0.1;
break;
case ClimberGoal::STOWED:
climber_position =
robot_constants_->common()->climber_set_points()->stowed();
+ // Keep the climber slower while retracting.
+ climber_velocity = 0.1;
}
}
@@ -115,17 +134,13 @@
robot_constants_->common()->intake_pivot_set_points()->retracted();
if (unsafe_goal != nullptr) {
- switch (unsafe_goal->intake_goal()) {
- case IntakeGoal::INTAKE:
+ switch (unsafe_goal->intake_pivot()) {
+ case IntakePivotGoal::DOWN:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->extended();
intake_end_time_ = timestamp;
break;
- case IntakeGoal::SPIT:
- intake_pivot_position =
- robot_constants_->common()->intake_pivot_set_points()->retracted();
- break;
- case IntakeGoal::NONE:
+ case IntakePivotGoal::UP:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->retracted();
break;
@@ -206,12 +221,11 @@
}
extend_goal_location = ExtendStatus::RETRACTED;
- catapult_requested_ = false;
break;
case SuperstructureState::INTAKING:
// Switch to LOADED state when the extend beambreak is triggered
// meaning the note is loaded in the extend
- if (position->extend_beambreak()) {
+ if (extend_beambreak) {
state_ = SuperstructureState::LOADED;
}
intake_roller_state = IntakeRollerStatus::INTAKING;
@@ -219,11 +233,6 @@
extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
extend_goal_location = ExtendStatus::RETRACTED;
- if (!catapult_requested_ && unsafe_goal != nullptr &&
- unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
- catapult_requested_ = true;
- }
-
// If we are no longer requesting INTAKE or we are no longer requesting
// an INTAKE goal, wait 0.5 seconds then go back to IDLE.
if (!(unsafe_goal != nullptr &&
@@ -234,7 +243,7 @@
break;
case SuperstructureState::LOADED:
- if (!position->extend_beambreak() && !position->catapult_beambreak()) {
+ if (!extend_beambreak && !position->catapult_beambreak()) {
state_ = SuperstructureState::IDLE;
}
@@ -242,7 +251,7 @@
case NoteGoal::NONE:
break;
case NoteGoal::CATAPULT:
- state_ = SuperstructureState::MOVING;
+ state_ = SuperstructureState::LOADING_CATAPULT;
transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
break;
case NoteGoal::TRAP:
@@ -267,8 +276,8 @@
extend_goal_location = ExtendStatus::CATAPULT;
if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
altitude_ready_for_load) {
- loading_catapult_start_time_ = timestamp;
state_ = SuperstructureState::LOADING_CATAPULT;
+ loading_catapult_start_time_ = timestamp;
}
break;
case NoteGoal::TRAP:
@@ -296,17 +305,22 @@
case SuperstructureState::LOADING_CATAPULT:
extend_moving = false;
extend_goal_location = ExtendStatus::CATAPULT;
- extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
- // If we lost the game piece, reset state to idle.
- if (((timestamp - loading_catapult_start_time_) >
- kMaxCatapultLoadingTime) &&
- !position->catapult_beambreak() && !position->extend_beambreak()) {
+ if (extend_beambreak) {
+ loading_catapult_start_time_ = timestamp;
+ }
+
+ if (loading_catapult_start_time_ + std::chrono::seconds(10) < timestamp) {
state_ = SuperstructureState::IDLE;
}
+ if (turret_ready_for_load && altitude_ready_for_load) {
+ extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
+ transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
+ }
+
// Switch to READY state when the catapult beambreak is triggered
- if (position->catapult_beambreak()) {
+ if (shooter_.loaded()) {
state_ = SuperstructureState::READY;
}
break;
@@ -326,6 +340,10 @@
break;
case NoteGoal::CATAPULT:
extend_goal_location = ExtendStatus::CATAPULT;
+
+ if (!shooter_.loaded()) {
+ state_ = SuperstructureState::LOADING_CATAPULT;
+ }
break;
case NoteGoal::TRAP:
extend_goal_location = ExtendStatus::TRAP;
@@ -345,14 +363,14 @@
// Reset the state to IDLE when the game piece is fired from the
// catapult. We consider the game piece to be fired from the catapult
// when the catapultbeambreak is no longer triggered.
- if (!position->catapult_beambreak()) {
+ if (!shooter_.loaded() && !shooter_.Firing()) {
state_ = SuperstructureState::IDLE;
}
break;
case NoteGoal::TRAP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
extend_goal_location = ExtendStatus::TRAP;
- if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ if (!extend_beambreak && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
}
@@ -360,7 +378,7 @@
case NoteGoal::AMP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
extend_goal_location = ExtendStatus::AMP;
- if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ if (!extend_beambreak && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
}
@@ -373,6 +391,7 @@
unsafe_goal->intake_goal() == IntakeGoal::SPIT) {
intake_roller_state = IntakeRollerStatus::SPITTING;
transfer_roller_status = TransferRollerStatus::TRANSFERING_OUT;
+ extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
}
// Update Intake Roller voltage based on status from state machine.
@@ -434,9 +453,26 @@
break;
}
+ if (unsafe_goal != nullptr && unsafe_goal->spit_extend()) {
+ output_struct.extend_roller_voltage = -extend_roller_voltages->scoring();
+ }
+
double extend_goal_position = 0.0;
+ // If we request trap, override the extend goal to be trap unless we request
+ // amp.
if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ trap_override_ = true;
+ }
+
+ if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::AMP &&
+ trap_override_) {
+ trap_override_ = false;
+ requested_note_goal_ = NoteGoal::AMP;
+ state_ = SuperstructureState::READY;
+ }
+
+ if (trap_override_) {
extend_goal_location = ExtendStatus::TRAP;
}
@@ -499,9 +535,17 @@
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
climber_goal_buffer;
- climber_goal_buffer.Finish(
- frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *climber_goal_buffer.fbb(), climber_position));
+ {
+ flatbuffers::FlatBufferBuilder *climber_fbb = climber_goal_buffer.fbb();
+ flatbuffers::Offset<frc971::ProfileParameters> climber_profile =
+ frc971::CreateProfileParameters(*climber_fbb, climber_velocity,
+ climber_accel);
+
+ climber_goal_buffer.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *climber_fbb, climber_position, climber_profile));
+ }
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*climber_goal = &climber_goal_buffer.message();
@@ -563,7 +607,8 @@
extend_goal_position, extend_.estimated_position(),
&max_extend_position, &min_extend_position,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
- &min_intake_pivot_position, status->fbb(), timestamp);
+ &min_intake_pivot_position, requested_note_goal_, status->fbb(),
+ timestamp);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
@@ -613,6 +658,7 @@
status_builder.add_extend_status(extend_status);
status_builder.add_extend(extend_status_offset);
status_builder.add_state(state_);
+ status_builder.add_shot_count(shooter_.shot_count());
status_builder.add_uncompleted_note_goal(uncompleted_note_goal_status);
status_builder.add_extend_ready_for_transfer(extend_at_retracted);
status_builder.add_extend_at_retracted(extend_at_retracted);
@@ -620,7 +666,7 @@
status_builder.add_altitude_ready_for_load(altitude_ready_for_load);
status_builder.add_extend_ready_for_catapult_transfer(
extend_ready_for_catapult_transfer);
- status_builder.add_extend_beambreak(position->extend_beambreak());
+ status_builder.add_extend_beambreak(extend_beambreak);
status_builder.add_catapult_beambreak(position->catapult_beambreak());
(void)status->Send(status_builder.Finish());
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 1c2d119..ce279a6 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -67,12 +67,15 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
- bool catapult_requested_ = false;
-
SuperstructureState state_ = SuperstructureState::IDLE;
+ bool trap_override_ = false;
+
NoteGoal requested_note_goal_ = NoteGoal::NONE;
+ aos::monotonic_clock::time_point transfer_start_time_ =
+ aos::monotonic_clock::time_point::min();
+
aos::monotonic_clock::time_point intake_end_time_ =
aos::monotonic_clock::time_point::min();
@@ -85,6 +88,9 @@
Shooter shooter_;
PotAndAbsoluteEncoderSubsystem extend_;
+
+ Debouncer extend_debouncer_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_can_position.fbs b/y2024/control_loops/superstructure/superstructure_can_position.fbs
index e809adf..ab69337 100644
--- a/y2024/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_can_position.fbs
@@ -6,7 +6,7 @@
// The timestamp of the measurement on the canivore clock in nanoseconds
// This will have less jitter than the
// timestamp of the message being sent out.
- timestamp:int64 (id: 0);
+ timestamp:int64 (id: 0, deprecated);
// The ctre::phoenix::StatusCode of the measurement
// Should be OK = 0
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index 42caf9b..abfd405 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -12,6 +12,11 @@
SPIT = 2,
}
+enum IntakePivotGoal : ubyte {
+ UP = 0,
+ DOWN = 1,
+}
+
// Represents goal for climber
// FULL_EXTEND is for fully extending the climber
// RETRACT is for retracting the climber
@@ -54,9 +59,16 @@
table Goal {
intake_goal:IntakeGoal = NONE (id: 0);
+ intake_pivot:IntakePivotGoal = UP (id: 5);
climber_goal:ClimberGoal (id: 1);
shooter_goal:ShooterGoal (id: 2);
note_goal:NoteGoal (id: 3);
fire: bool (id: 4);
+
+ // Tells the climber to go absurdly slow on FULL_EXTEND
+ slow_climber: bool = false (id: 6);
+
+ // Spit on the extend motors
+ spit_extend: bool = false (id: 7);
}
root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 56452c8..9dad4ec 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -193,13 +193,9 @@
simulated_robot_constants->common()->catapult()->range())
.middle());
altitude_.InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->altitude()->range())
- .middle());
+ simulated_robot_constants->common()->altitude_loading_position());
turret_.InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->turret()->range())
- .middle());
+ simulated_robot_constants->common()->turret_loading_position());
extend_.InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->extend()->range())
@@ -332,7 +328,7 @@
SuperstructureTest()
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2024/aos_config.json"),
- std::chrono::microseconds(5050)),
+ std::chrono::microseconds(5000)),
simulated_constants_dummy_(SendSimulationConstants(
event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
@@ -392,7 +388,7 @@
double set_point =
superstructure_status_fetcher_->intake_pivot()->goal_position();
- if (superstructure_goal_fetcher_->intake_goal() == IntakeGoal::INTAKE) {
+ if (superstructure_goal_fetcher_->intake_pivot() == IntakePivotGoal::DOWN) {
set_point = simulated_robot_constants_->common()
->intake_pivot_set_points()
->extended();
@@ -421,7 +417,11 @@
if (superstructure_goal_fetcher_->has_shooter_goal()) {
if (superstructure_goal_fetcher_->shooter_goal()
->has_altitude_position() &&
- !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
+ !superstructure_goal_fetcher_->shooter_goal()->auto_aim() &&
+ (superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::AMP &&
+ superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::TRAP)) {
EXPECT_NEAR(
superstructure_goal_fetcher_->shooter_goal()
->altitude_position()
@@ -432,6 +432,18 @@
->altitude_position()
->unsafe_goal(),
superstructure_plant_.altitude()->position(), 0.001);
+ } else if (superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::AMP ||
+ superstructure_status_fetcher_->uncompleted_note_goal() ==
+ NoteStatus::TRAP) {
+ EXPECT_NEAR(
+ simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position(),
+ superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.001);
+ EXPECT_NEAR(simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position(),
+ superstructure_plant_.altitude()->position(), 0.001);
}
}
@@ -580,31 +592,18 @@
SetEnabled(true);
WaitUntilZeroed();
- superstructure_plant_.turret()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .middle());
- superstructure_plant_.altitude()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
-
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .middle());
+ simulated_robot_constants_->common()->turret_loading_position());
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
+ simulated_robot_constants_->common()->altitude_loading_position());
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
@@ -620,6 +619,7 @@
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -636,30 +636,6 @@
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
- superstructure_plant_.intake_pivot()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->intake_pivot()->range())
- .lower);
-
- superstructure_plant_.turret()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .lower);
-
- superstructure_plant_.altitude()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
-
- superstructure_plant_.climber()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->climber()->range())
- .lower);
-
- superstructure_plant_.extend()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->extend()->range())
- .lower);
WaitUntilZeroed();
{
@@ -685,12 +661,13 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_preloaded(true);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
@@ -698,7 +675,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_extend_beambreak(true);
+ superstructure_plant_.set_catapult_beambreak(true);
// Give it a lot of time to get there.
RunFor(chrono::seconds(15));
@@ -706,7 +683,7 @@
VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->state(),
- SuperstructureState::LOADED);
+ SuperstructureState::READY);
}
// Makes sure that the voltage on a motor is properly pulled back after
@@ -722,17 +699,13 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .upper);
+ *builder.fbb(), simulated_robot_constants_->common()
+ ->turret_avoid_extend_collision_position());
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .upper);
+ *builder.fbb(), simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position());
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
@@ -754,7 +727,7 @@
}
superstructure_plant_.set_extend_beambreak(true);
- RunFor(chrono::seconds(20));
+ RunFor(chrono::seconds(30));
VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->state(),
@@ -776,9 +749,8 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .lower,
+ simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position(),
CreateProfileParameters(*builder.fbb(), 20.0, 10));
ShooterGoal::Builder shooter_goal_builder =
@@ -854,6 +826,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -873,6 +846,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::SPIT);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -893,6 +867,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -916,6 +891,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -951,6 +927,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -996,6 +973,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(false);
@@ -1036,6 +1014,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(false);
@@ -1074,6 +1053,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
goal_builder.add_fire(false);
@@ -1184,6 +1164,7 @@
ExtendRollerStatus::IDLE);
// Should now be loaded.
+ superstructure_output_fetcher_.Fetch();
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
@@ -1222,6 +1203,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(true);
@@ -1231,7 +1213,7 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(chrono::milliseconds(1000));
+ RunFor(dt());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -1319,9 +1301,6 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- LOG(INFO) << EnumNameNoteStatus(
- superstructure_status_fetcher_->uncompleted_note_goal());
-
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::READY);
@@ -1336,7 +1315,14 @@
SetEnabled(true);
WaitUntilZeroed();
- constexpr double kDistanceFromSpeaker = 5.0;
+ constexpr double kDistanceFromSpeaker = 4.0;
+
+ const frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table =
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ simulated_robot_constants_->common()
+ ->shooter_interpolation_table());
const double kRedSpeakerX = simulated_robot_constants_->common()
->shooter_targets()
@@ -1390,6 +1376,11 @@
superstructure_plant_.set_catapult_beambreak(true);
+ constants::Values::ShotParams shot_params;
+
+ EXPECT_TRUE(
+ interpolation_table.GetInRange(kDistanceFromSpeaker, &shot_params));
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -1400,12 +1391,12 @@
EXPECT_NEAR(
-M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
- M_PI,
+ M_PI - 0.22,
5e-4);
- EXPECT_NEAR(
- -M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position() - M_PI,
- 5e-4);
+ EXPECT_NEAR(-M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() -
+ M_PI - 0.22,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
@@ -1446,12 +1437,12 @@
EXPECT_NEAR(
M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
- M_PI,
+ M_PI - 0.22,
5e-4);
- EXPECT_NEAR(
- M_PI_2,
- superstructure_status_fetcher_->shooter()->turret()->position() + M_PI,
- 5e-4);
+ EXPECT_NEAR(M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position() +
+ M_PI - 0.22,
+ 5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
superstructure_status_fetcher_->shooter()->aimer()->target_distance());
@@ -1469,6 +1460,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1495,6 +1487,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1502,7 +1495,7 @@
superstructure_plant_.set_extend_beambreak(true);
- RunFor(chrono::milliseconds(10));
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -1520,6 +1513,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1550,6 +1544,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
goal_builder.add_fire(true);
@@ -1576,6 +1571,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
goal_builder.add_fire(false);
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 8d6b14f..fd19e8d 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -77,6 +77,13 @@
// Status of the aimer
aimer:AimerStatus (id: 4);
+
+ // True if auto-aiming.
+ auto_aiming:bool (id: 5);
+
+ turret_in_range:bool (id: 6);
+ altitude_in_range:bool (id: 7);
+ altitude_above_min_angle:bool (id: 8);
}
// Contains status of transfer rollers
@@ -174,6 +181,9 @@
extend_beambreak:bool (id: 19);
catapult_beambreak:bool (id: 20);
+
+ // Number of shots we have taken.
+ shot_count:uint32 (id:21);
}
root_type Status;
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index caa6805..4a8d414 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -32,6 +32,9 @@
using frc971::input::driver_station::POVLocation;
using Side = frc971::control_loops::drivetrain::RobotSide;
+DEFINE_double(speaker_altitude_position_override, -1,
+ "If set, use this as the altitude angle for the fixed shot.");
+
namespace y2024::input::joysticks {
namespace superstructure = y2024::control_loops::superstructure;
@@ -39,18 +42,23 @@
// TODO(Xander): add button location from physical wiring
// Note: Due to use_redundant_joysticks, the AOS_LOG statements
// for the internal joystick code will give offset joystick numbering.
-const ButtonLocation kIntake(2, 8);
-const ButtonLocation kSpit(0, 0);
-const ButtonLocation kCatapultLoad(1, 7);
-const ButtonLocation kAmp(2, 7);
-const ButtonLocation kFire(2, 6);
-const ButtonLocation kTrap(2, 5);
-const ButtonLocation kAutoAim(0, 0);
-const ButtonLocation kAimSpeaker(1, 6);
+const ButtonLocation kIntake(2, 2);
+
+const ButtonLocation kSpitRollers(1, 13);
+const ButtonLocation kSpitExtend(1, 12);
+const ButtonLocation kIntakeRollers(2, 5);
+
+const ButtonLocation kCatapultLoad(2, 1);
+const ButtonLocation kAmp(2, 4);
+const ButtonLocation kFire(2, 8);
+const ButtonLocation kTrap(2, 6);
+const ButtonLocation kAutoAim(1, 8);
+const ButtonLocation kAimSpeaker(2, 11);
const ButtonLocation kAimPodium(0, 0);
const ButtonLocation kShoot(0, 0);
const ButtonLocation kRaiseClimber(3, 2);
-const ButtonLocation kRetractClimber(2, 4);
+const ButtonLocation kSlowClimber(3, 1);
+const ButtonLocation kRetractClimber(2, 3);
const ButtonLocation kExtraButtonOne(0, 0);
const ButtonLocation kExtraButtonTwo(0, 0);
const ButtonLocation kExtraButtonThree(0, 0);
@@ -88,12 +96,31 @@
if (data.IsPressed(kIntake)) {
// Intake is pressed
+ superstructure_goal_builder->set_intake_pivot(
+ superstructure::IntakePivotGoal::DOWN);
+ } else {
+ superstructure_goal_builder->set_intake_pivot(
+ superstructure::IntakePivotGoal::UP);
+ }
+
+ if (data.IsPressed(kIntakeRollers)) {
+ // Intake is pressed
superstructure_goal_builder->set_intake_goal(
superstructure::IntakeGoal::INTAKE);
+ } else if (data.IsPressed(kSpitRollers)) {
+ superstructure_goal_builder->set_intake_goal(
+ superstructure::IntakeGoal::SPIT);
} else {
superstructure_goal_builder->set_intake_goal(
superstructure::IntakeGoal::NONE);
}
+
+ if (data.IsPressed(kSpitExtend)) {
+ superstructure_goal_builder->set_spit_extend(true);
+ } else {
+ superstructure_goal_builder->set_spit_extend(false);
+ }
+
if (data.IsPressed(kAmp)) {
superstructure_goal_builder->set_note_goal(superstructure::NoteGoal::AMP);
} else if (data.IsPressed(kTrap)) {
@@ -107,7 +134,7 @@
superstructure::NoteGoal::NONE);
}
auto shooter_goal = superstructure_goal_builder->add_shooter_goal();
- shooter_goal->set_auto_aim(false);
+ shooter_goal->set_auto_aim(data.IsPressed(kAutoAim));
// Updating aiming for shooter goal, only one type of aim should be possible
// at a time, auto-aiming is preferred over the setpoints.
@@ -116,11 +143,17 @@
catapult_goal->set_shot_velocity(robot_constants_->common()
->shooter_speaker_set_point()
->shot_velocity());
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- shooter_goal->add_altitude_position(),
- robot_constants_->common()
- ->shooter_speaker_set_point()
- ->altitude_position());
+ if (FLAGS_speaker_altitude_position_override > 0) {
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_altitude_position(),
+ FLAGS_speaker_altitude_position_override);
+ } else {
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_altitude_position(),
+ robot_constants_->common()
+ ->shooter_speaker_set_point()
+ ->altitude_position());
+ }
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
shooter_goal->add_turret_position(), robot_constants_->common()
->shooter_speaker_set_point()
@@ -128,17 +161,23 @@
}
superstructure_goal_builder->set_fire(data.IsPressed(kFire));
- if (data.IsPressed(kRaiseClimber)) {
- superstructure_goal_builder->set_climber_goal(
- superstructure::ClimberGoal::FULL_EXTEND);
- } else if (data.IsPressed(kRetractClimber)) {
+ if (data.IsPressed(kRetractClimber)) {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::RETRACT);
+ } else if (data.IsPressed(kRaiseClimber)) {
+ superstructure_goal_builder->set_climber_goal(
+ superstructure::ClimberGoal::FULL_EXTEND);
} else {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::STOWED);
}
+ if (data.IsPressed(kSlowClimber)) {
+ superstructure_goal_builder->set_slow_climber(true);
+ } else {
+ superstructure_goal_builder->set_slow_climber(false);
+ }
+
superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
}
diff --git a/y2024/localizer/BUILD b/y2024/localizer/BUILD
index caec284..5e2581a 100644
--- a/y2024/localizer/BUILD
+++ b/y2024/localizer/BUILD
@@ -25,6 +25,7 @@
deps = [
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/imu_reader:imu_failures_fbs",
+ "//frc971/math:matrix_fbs",
],
)
@@ -35,6 +36,7 @@
deps = [
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
"//frc971/imu_reader:imu_failures_ts_fbs",
+ "//frc971/math:matrix_ts_fbs",
],
)
@@ -77,6 +79,7 @@
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/control_loops/drivetrain/localization:utils",
"//frc971/imu_reader:imu_watcher",
+ "//frc971/math:flatbuffers_matrix",
"//frc971/vision:target_map_fbs",
"//frc971/vision:target_map_utils",
"//y2024:constants",
@@ -133,6 +136,7 @@
srcs = ["localizer_replay.cc"],
data = [
"//y2024:aos_config",
+ "//y2024/constants:constants.json",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
@@ -144,6 +148,9 @@
"//aos/events/logging:log_reader",
"//aos/events/logging:log_writer",
"//aos/util:simulation_logger",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/imu_fdcan:dual_imu_blender_lib",
+ "//y2024/constants:simulated_constants_sender",
"//y2024/control_loops/drivetrain:drivetrain_base",
],
)
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index a177188..b8e982e 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -5,19 +5,21 @@
#include "aos/containers/sized_array.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
+#include "frc971/math/flatbuffers_matrix.h"
#include "frc971/vision/target_map_utils.h"
#include "y2024/constants.h"
-DEFINE_double(max_pose_error, 1e-6,
+DEFINE_double(max_pose_error, 1e-5,
"Throw out target poses with a higher pose error than this");
+DEFINE_double(max_distortion, 0.1, "");
DEFINE_double(
max_pose_error_ratio, 0.4,
"Throw out target poses with a higher pose error ratio than this");
-DEFINE_double(distortion_noise_scalar, 1.0,
+DEFINE_double(distortion_noise_scalar, 4.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
DEFINE_double(
- max_implied_yaw_error, 3.0,
+ max_implied_yaw_error, 5.0,
"Reject target poses that imply a robot yaw of more than this many degrees "
"off from our estimate.");
DEFINE_double(
@@ -30,6 +32,15 @@
DEFINE_double(max_auto_image_robot_speed, 2.0,
"Reject target poses when the robot is travelling faster than "
"this speed in auto.");
+DEFINE_bool(
+ do_xytheta_corrections, true,
+ "If set, uses the x/y/theta corrector rather than a heading/distance/skew "
+ "one. This is better conditioned currently, but is theoretically worse due "
+ "to not capturing noise effectively.");
+DEFINE_bool(
+ always_use_extra_tags, true,
+ "If set, we will use the \"deweighted\" tags even in auto mode (this "
+ "affects april tags whose field positions we do not trust as much).");
namespace y2024::localizer {
namespace {
@@ -62,6 +73,69 @@
}
return transforms;
}
+
+// Returns the "nominal" covariance of localizer---i.e., the values to which it
+// tends to converge during normal operation. By initializing the localizer's
+// covariance this way, we reduce the likelihood that the first few corrections
+// we receive will result in insane jumps in robot state.
+Eigen::Matrix<double, Localizer::HybridEkf::kNStates,
+ Localizer::HybridEkf::kNStates>
+NominalCovariance() {
+ Eigen::Matrix<double, Localizer::HybridEkf::kNStates,
+ Localizer::HybridEkf::kNStates>
+ P_transpose;
+ // Grabbed from when the robot was in a steady-state.
+ P_transpose << 0.00344391020344026, 2.78255540964953e-05,
+ -3.44257436790434e-09, 1.57165298196431e-09, 0.0207259965606711,
+ 1.57165298180587e-09, 0.0207259965606711, 0.054775354511474,
+ 0.0547753545094318, 3.6435938125014e-13, 0.0136249573295751,
+ -1.00705421392865e-05, 2.78255540964953e-05, 0.00107448929200992,
+ -7.42495169208041e-08, 1.85634700506266e-11, 0.000244343925617656,
+ 1.85634874205036e-11, 0.000244343925617656, 0.000645553479721632,
+ 0.000645553790286344, -3.98991820983687e-11, 0.000160471639203211,
+ 0.00085437373557969, -3.44257436791122e-09, -7.42495169208033e-08,
+ 8.84891122456971e-05, 5.60929454430362e-16, -3.19015358072956e-08,
+ 1.00798618104673e-15, -3.19015357689791e-08, 4.05905848804053e-07,
+ -5.37043312466153e-07, 2.59177623699213e-08, -3.54286115799832e-08,
+ -2.46295184320124e-07, 1.57165298196416e-09, 1.85634700506217e-11,
+ 5.60929459005148e-16, 4.99891338811926e-09, 3.59436612693873e-08,
+ 3.54690022095621e-18, 3.59436612693713e-08, 1.47025442116767e-07,
+ 1.47035806190949e-07, 4.66877989234937e-08, -1.08209016210542e-08,
+ -3.39984473837553e-14, 0.0207259965606711, 0.000244343925617655,
+ -3.19015358072649e-08, 3.59436612693935e-08, 0.301240404540565,
+ 3.59436612690168e-08, 0.301240404540535, 1.05741200222346,
+ 1.0574120022184, 8.29472747900822e-13, 0.138401597893958,
+ -1.43941751907531e-06, 1.57165298180564e-09, 1.85634874205096e-11,
+ 1.00798617244172e-15, 3.54690020212816e-18, 3.59436612690224e-08,
+ 4.99891338811924e-09, 3.59436612690048e-08, 1.4703580619019e-07,
+ 1.47025442115683e-07, -4.66877989234395e-08, -1.0820901621393e-08,
+ -3.40023588372784e-14, 0.0207259965606711, 0.000244343925617655,
+ -3.19015357689244e-08, 3.59436612693859e-08, 0.301240404540535,
+ 3.5943661269025e-08, 0.301240404540565, 1.05741200222289,
+ 1.05741200221897, 8.29752131358864e-13, 0.138401597893958,
+ -1.43941751907531e-06, 0.0547753545114739, 0.000645553479721628,
+ 4.05905848802199e-07, 1.4702544211661e-07, 1.05741200222346,
+ 1.47035806190016e-07, 1.05741200222289, 5.51003071369415,
+ 4.85571868991385, 3.11581831710161e-06, 0.388669918077443,
+ -2.97795819369728e-06, 0.054775354509432, 0.000645553790286345,
+ -5.37043312465425e-07, 1.47035806190839e-07, 1.0574120022184,
+ 1.47025442115792e-07, 1.05741200221897, 4.85571868991385,
+ 5.51003071367444, -3.11581462746269e-06, 0.388669918072973,
+ -2.97799067538699e-06, 3.64359250152554e-13, -3.98991820983e-11,
+ 2.5917762369921e-08, 4.66877989234987e-08, 8.2947423101614e-13,
+ -4.66877989234886e-08, 8.29754326977491e-13, 3.11581831710969e-06,
+ -3.11581462747612e-06, 0.212136173309098, 8.06835372350592e-13,
+ 8.80190080862899e-12, 0.0136249573295751, 0.000160471639203211,
+ -3.54286115799303e-08, -1.08209016210412e-08, 0.138401597893958,
+ -1.08209016213997e-08, 0.138401597893958, 0.388669918077444,
+ 0.388669918072972, 8.06834601598773e-13, 0.187427410345505,
+ -1.28632768080328e-06, -1.00705421392865e-05, 0.000854373735579689,
+ -2.46295184320122e-07, -3.39984473838037e-14, -1.4394175190755e-06,
+ -3.40023588373113e-14, -1.4394175190755e-06, -2.97795819369787e-06,
+ -2.9779906753875e-06, 8.80190080900245e-12, -1.28632768080338e-06,
+ 0.00381653175156393;
+ return P_transpose.transpose();
+}
} // namespace
std::array<Localizer::CameraState, Localizer::kNumCameras>
@@ -107,8 +181,14 @@
cameras_(MakeCameras(constants_fetcher_.constants(), event_loop)),
target_poses_(GetTargetLocations(constants_fetcher_.constants())),
down_estimator_(dt_config_),
- ekf_(dt_config_),
+ // Force the dt to 1 ms (the nominal IMU frequency) since we have observed
+ // issues with timing on the orins.
+ // TODO(james): Ostensibly, we should be able to use the timestamps from
+ // the IMU board itself for exactly this; however, I am currently worried
+ // about the impacts of clock drift in using that.
+ ekf_(dt_config_, std::chrono::milliseconds(1)),
observations_(&ekf_),
+ xyz_observations_(&ekf_),
imu_watcher_(event_loop, dt_config_,
y2024::constants::Values::DrivetrainEncoderToMeters(1),
std::bind(&Localizer::HandleImu, this, std::placeholders::_1,
@@ -124,7 +204,10 @@
"/aos")),
client_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
- "/aos")) {
+ "/aos")),
+ control_fetcher_(event_loop_->MakeFetcher<
+ frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")) {
if (dt_config_.is_simulated) {
down_estimator_.assume_perfect_gravity();
}
@@ -186,25 +269,7 @@
"/drivetrain",
[this](
const frc971::control_loops::drivetrain::LocalizerControl &control) {
- // This is triggered whenever we need to force the X/Y/(maybe theta)
- // position of the robot to a particular point---e.g., during pre-match
- // setup, or when commanded by a button on the driverstation.
-
- // For some forms of reset, we choose to keep our current yaw estimate
- // rather than overriding it from the control message.
- const double theta = control.keep_current_theta()
- ? ekf_.X_hat(StateIdx::kTheta)
- : control.theta();
- // Encoder values need to be reset based on the current values to ensure
- // that we don't get weird corrections on the next encoder update.
- const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
- const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
- ekf_.ResetInitialState(
- t_,
- (HybridEkf::State() << control.x(), control.y(), theta,
- left_encoder, 0, right_encoder, 0, 0, 0, 0, 0, 0)
- .finished(),
- ekf_.P());
+ HandleControl(control);
});
ekf_.set_ignore_accel(true);
@@ -212,20 +277,43 @@
event_loop->SetRuntimeRealtimePriority(10);
event_loop->OnRun([this, event_loop]() {
ekf_.ResetInitialState(event_loop->monotonic_now(),
- HybridEkf::State::Zero(), ekf_.P());
+ HybridEkf::State::Zero(), NominalCovariance());
+ if (control_fetcher_.Fetch()) {
+ HandleControl(*control_fetcher_.get());
+ }
});
}
+void Localizer::HandleControl(
+ const frc971::control_loops::drivetrain::LocalizerControl &control) {
+ // This is triggered whenever we need to force the X/Y/(maybe theta)
+ // position of the robot to a particular point---e.g., during pre-match
+ // setup, or when commanded by a button on the driverstation.
+
+ // For some forms of reset, we choose to keep our current yaw estimate
+ // rather than overriding it from the control message.
+ const double theta = control.keep_current_theta()
+ ? ekf_.X_hat(StateIdx::kTheta)
+ : control.theta();
+ // Encoder values need to be reset based on the current values to ensure
+ // that we don't get weird corrections on the next encoder update.
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
+ ekf_.ResetInitialState(t_,
+ (HybridEkf::State() << control.x(), control.y(), theta,
+ left_encoder, 0, right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
+ NominalCovariance());
+ VLOG(1) << "Reset state";
+}
+
void Localizer::HandleImu(aos::monotonic_clock::time_point /*sample_time_pico*/,
aos::monotonic_clock::time_point sample_time_orin,
std::optional<Eigen::Vector2d> /*encoders*/,
Eigen::Vector3d gyro, Eigen::Vector3d accel) {
std::optional<Eigen::Vector2d> encoders = utils_.Encoders(sample_time_orin);
last_encoder_readings_ = encoders;
- // Ignore invalid readings; the HybridEkf will handle it reasonably.
- if (!encoders.has_value()) {
- return;
- }
+ VLOG(1) << "Got encoders";
if (t_ == aos::monotonic_clock::min_time) {
t_ = sample_time_orin;
}
@@ -239,8 +327,12 @@
// convenient for debugging.
down_estimator_.Predict(gyro, accel, dt);
const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
- ekf_.UpdateEncodersAndGyro(encoders.value()(0), encoders.value()(1), yaw_rate,
- utils_.VoltageOrZero(sample_time_orin), accel, t_);
+ ekf_.UpdateEncodersAndGyro(
+ encoders.has_value() ? std::make_optional<double>(encoders.value()(0))
+ : std::nullopt,
+ encoders.has_value() ? std::make_optional<double>(encoders.value()(1))
+ : std::nullopt,
+ yaw_rate, utils_.VoltageOrZero(sample_time_orin), accel, t_);
SendStatus();
}
@@ -257,7 +349,29 @@
// (in the past) for ignoring april tags that tend to produce problematic
// readings.
bool Localizer::UseAprilTag(uint64_t target_id) {
- return target_poses_.count(target_id) != 0;
+ if (target_poses_.count(target_id) == 0) {
+ return false;
+ }
+ return true;
+}
+
+bool Localizer::DeweightAprilTag(uint64_t target_id) {
+ const flatbuffers::Vector<uint64_t> *ignore_tags = nullptr;
+
+ switch (utils_.Alliance()) {
+ case aos::Alliance::kRed:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().common()->ignore_targets()->red());
+ break;
+ case aos::Alliance::kBlue:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().common()->ignore_targets()->blue());
+ break;
+ case aos::Alliance::kInvalid:
+ return false;
+ }
+ return std::find(ignore_tags->begin(), ignore_tags->end(), target_id) !=
+ ignore_tags->end();
}
namespace {
@@ -298,6 +412,16 @@
RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, debug_builder);
return;
}
+ double april_tag_noise_scalar = 1.0;
+ if (DeweightAprilTag(target_id)) {
+ if (!FLAGS_always_use_extra_tags && utils_.MaybeInAutonomous()) {
+ VLOG(1) << "Rejecting target due to auto invalid ID " << target_id;
+ RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, debug_builder);
+ return;
+ } else {
+ april_tag_noise_scalar = 10.0;
+ }
+ }
const Transform &H_field_target = target_poses_.at(target_id);
const Transform &H_robot_camera = cameras_.at(camera_index).extrinsics;
@@ -335,31 +459,33 @@
// Heading, distance, skew at 1 meter.
Eigen::Matrix<double, 3, 1> noises(0.01, 0.05, 0.05);
- const double distance_noise_scalar = std::pow(distance_to_target, 2.0);
+ const double distance_noise_scalar =
+ std::min(1.0, std::pow(distance_to_target, 2.0));
noises(Corrector::kDistance) *= distance_noise_scalar;
noises(Corrector::kSkew) *= distance_noise_scalar;
// TODO(james): This is leftover from last year; figure out if we want it.
// Scale noise by the distortion factor for this detection
noises *= (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
+ noises *= april_tag_noise_scalar;
Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
R.diagonal() = noises.cwiseAbs2();
+ const Eigen::Vector3d camera_position =
+ corrector.observed_camera_pose().abs_pos();
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ const Transform H_camera_robot_stripped =
+ frc971::control_loops::Pose(ZToXCamera(H_robot_camera))
+ .AsTransformationMatrix()
+ .inverse();
+ const frc971::control_loops::Pose measured_pose(
+ corrector.observed_camera_pose().AsTransformationMatrix() *
+ H_camera_robot_stripped);
if (debug_builder != nullptr) {
- const Eigen::Vector3d camera_position =
- corrector.observed_camera_pose().abs_pos();
debug_builder->set_camera_x(camera_position.x());
debug_builder->set_camera_y(camera_position.y());
debug_builder->set_camera_theta(
corrector.observed_camera_pose().abs_theta());
- // Calculate the camera-to-robot transformation matrix ignoring the
- // pitch/roll of the camera.
- const Transform H_camera_robot_stripped =
- frc971::control_loops::Pose(ZToXCamera(H_robot_camera))
- .AsTransformationMatrix()
- .inverse();
- const frc971::control_loops::Pose measured_pose(
- corrector.observed_camera_pose().AsTransformationMatrix() *
- H_camera_robot_stripped);
debug_builder->set_implied_robot_x(measured_pose.rel_pos().x());
debug_builder->set_implied_robot_y(measured_pose.rel_pos().y());
debug_builder->set_implied_robot_theta(measured_pose.rel_theta());
@@ -385,8 +511,12 @@
: FLAGS_max_implied_teleop_yaw_error) *
kDegToRad;
- if (utils_.MaybeInAutonomous() &&
- (std::abs(robot_speed) > FLAGS_max_auto_image_robot_speed)) {
+ if (target.distortion_factor() > FLAGS_max_distortion) {
+ VLOG(1) << "Rejecting target due to high distortion.";
+ return RejectImage(camera_index, RejectionReason::HIGH_DISTORTION,
+ debug_builder);
+ } else if (utils_.MaybeInAutonomous() &&
+ (std::abs(robot_speed) > FLAGS_max_auto_image_robot_speed)) {
return RejectImage(camera_index, RejectionReason::ROBOT_TOO_FAST,
debug_builder);
} else if (std::abs(camera_yaw_error) > yaw_threshold) {
@@ -398,7 +528,7 @@
}
const Input U = ekf_.MostRecentInput();
- VLOG(1) << "previous state " << ekf_.X_hat().topRows<3>().transpose();
+ VLOG(1) << "previous state " << ekf_.X_hat().transpose();
const State prior_state = ekf_.X_hat();
// For the correction step, instead of passing in the measurement directly,
// we pass in (0, 0, 0) as the measurement and then for the expected
@@ -406,10 +536,27 @@
// the camera measurement and the current estimate of the
// pose. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
- observations_.CorrectKnownH(Eigen::Vector3d::Zero(), &U, corrector, R, t_);
+ if (FLAGS_do_xytheta_corrections) {
+ Eigen::Vector3d Z(measured_pose.rel_pos().x(), measured_pose.rel_pos().y(),
+ measured_pose.rel_theta());
+ Eigen::Matrix<double, 3, 1> xyz_noises(0.2, 0.2, 0.5);
+ xyz_noises *= distance_noise_scalar;
+ xyz_noises *= april_tag_noise_scalar;
+ // Scale noise by the distortion factor for this detection
+ xyz_noises *=
+ (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
+
+ Eigen::Matrix3d R_xyz = Eigen::Matrix3d::Zero();
+ R_xyz.diagonal() = xyz_noises.cwiseAbs2();
+ xyz_observations_.CorrectKnownH(Eigen::Vector3d::Zero(), &U,
+ XyzCorrector(state_at_capture.value(), Z),
+ R_xyz, t_);
+ } else {
+ observations_.CorrectKnownH(Eigen::Vector3d::Zero(), &U, corrector, R, t_);
+ }
++total_accepted_targets_;
++cameras_.at(camera_index).total_accepted_targets;
- VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();
+ VLOG(1) << "new state " << ekf_.X_hat().transpose();
if (debug_builder != nullptr) {
debug_builder->set_correction_x(ekf_.X_hat()(StateIdx::kX) -
prior_state(StateIdx::kX));
@@ -418,6 +565,9 @@
debug_builder->set_correction_theta(ekf_.X_hat()(StateIdx::kTheta) -
prior_state(StateIdx::kTheta));
debug_builder->set_accepted(true);
+ debug_builder->set_expected_robot_x(ekf_.X_hat()(StateIdx::kX));
+ debug_builder->set_expected_robot_y(ekf_.X_hat()(StateIdx::kY));
+ debug_builder->set_expected_robot_theta(ekf_.X_hat()(StateIdx::kTheta));
}
}
@@ -546,11 +696,17 @@
down_estimator_.PopulateStatus(builder.fbb(), t_);
auto imu_offset = PopulateImu(builder.fbb());
auto state_offset = PopulateState(ekf_.X_hat(), builder.fbb());
+ // covariance is a square; we use the number of rows in the state as the rows
+ // and cols of the covariance.
+ auto covariance_offset =
+ frc971::FromEigen<State::RowsAtCompileTime, State::RowsAtCompileTime>(
+ ekf_.P(), builder.fbb());
Status::Builder status_builder = builder.MakeBuilder<Status>();
status_builder.add_state(state_offset);
status_builder.add_down_estimator(down_estimator_offset);
status_builder.add_imu(imu_offset);
status_builder.add_statistics(stats_offset);
+ status_builder.add_ekf_covariance(covariance_offset);
builder.CheckOk(builder.Send(status_builder.Finish()));
}
@@ -598,4 +754,14 @@
return expected_ - observed_;
}
+Localizer::Output Localizer::XyzCorrector::H(const State &, const Input &) {
+ CHECK(Z_.allFinite());
+ Eigen::Vector3d Zhat = H_ * state_at_capture_ - Z_;
+ // Rewrap angle difference to put it back in range.
+ Zhat(2) = aos::math::NormalizeAngle(Zhat(2));
+ VLOG(1) << "Zhat " << Zhat.transpose() << " Z_ " << Z_.transpose()
+ << " state " << (H_ * state_at_capture_).transpose();
+ return Zhat;
+}
+
} // namespace y2024::localizer
diff --git a/y2024/localizer/localizer.h b/y2024/localizer/localizer.h
index 8235f13..45ca437 100644
--- a/y2024/localizer/localizer.h
+++ b/y2024/localizer/localizer.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/drivetrain/localization/utils.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/imu_reader/imu_watcher.h"
#include "frc971/vision/target_map_generated.h"
#include "y2024/constants/constants_generated.h"
@@ -89,6 +90,36 @@
const HMatrix H_;
};
+ // A corrector that just does x/y/theta based corrections rather than doing
+ // heading/distance/skew corrections.
+ class XyzCorrector : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ // Indices used for each of the members of the output vector for this
+ // Corrector.
+ enum OutputIdx {
+ kX = 0,
+ kY = 1,
+ kTheta = 2,
+ };
+ XyzCorrector(const State &state_at_capture, const Eigen::Vector3d &Z)
+ : state_at_capture_(state_at_capture), Z_(Z) {
+ H_.setZero();
+ H_(kX, StateIdx::kX) = 1;
+ H_(kY, StateIdx::kY) = 1;
+ H_(kTheta, StateIdx::kTheta) = 1;
+ }
+ Output H(const State &, const Input &) final;
+ Eigen::Matrix<double, HybridEkf::kNOutputs, HybridEkf::kNStates> DHDX(
+ const State &) final {
+ return H_;
+ }
+
+ private:
+ Eigen::Matrix<double, HybridEkf::kNOutputs, HybridEkf::kNStates> H_;
+ const State state_at_capture_;
+ const Eigen::Vector3d &Z_;
+ };
+
struct CameraState {
aos::Sender<VisualizationStatic> debug_sender;
Transform extrinsics = Transform::Zero();
@@ -98,6 +129,9 @@
size_t total_accepted_targets = 0;
};
+ // Returns true if we should use a lower weight for the specified april tag.
+ // This is used for tags where we do not trust the placement as much.
+ bool DeweightAprilTag(uint64_t target_id);
static std::array<CameraState, kNumCameras> MakeCameras(
const Constants &constants, aos::EventLoop *event_loop);
void HandleTarget(int camera_index,
@@ -123,6 +157,8 @@
CumulativeStatisticsStatic *builder);
bool UseAprilTag(uint64_t target_id);
+ void HandleControl(
+ const frc971::control_loops::drivetrain::LocalizerControl &msg);
aos::EventLoop *const event_loop_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
@@ -134,6 +170,7 @@
frc971::control_loops::drivetrain::DrivetrainUkf down_estimator_;
HybridEkf ekf_;
HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
+ HybridEkf::ExpectedObservationAllocator<XyzCorrector> xyz_observations_;
frc971::controls::ImuWatcher imu_watcher_;
frc971::control_loops::drivetrain::LocalizationUtils utils_;
@@ -153,6 +190,8 @@
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
client_statistics_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::LocalizerControl>
+ control_fetcher_;
};
} // namespace y2024::localizer
#endif // Y2024_LOCALIZER_LOCALIZER_H_
diff --git a/y2024/localizer/localizer_replay.cc b/y2024/localizer/localizer_replay.cc
index c2c4c16..ad0caae 100644
--- a/y2024/localizer/localizer_replay.cc
+++ b/y2024/localizer/localizer_replay.cc
@@ -8,14 +8,21 @@
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
#include "aos/util/simulation_logger.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+#include "y2024/constants/simulated_constants_sender.h"
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
#include "y2024/localizer/localizer.h"
DEFINE_string(config, "y2024/aos_config.json",
"Name of the config file to replay using.");
-DEFINE_int32(team, 9971, "Team number to use for logfile replay.");
+DEFINE_bool(override_config, false,
+ "If set, override the logged config with --config.");
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
DEFINE_string(output_folder, "/tmp/replayed",
"Name of the folder to write replayed logs to.");
+DEFINE_string(constants_path, "y2024/constants/constants.json",
+ "Path to the constant file");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -30,7 +37,8 @@
aos::logger::SortParts(aos::logger::FindLogs(argc, argv));
// open logfiles
- aos::logger::LogReader reader(logfiles, &config.message());
+ aos::logger::LogReader reader(
+ logfiles, FLAGS_override_config ? &config.message() : nullptr);
reader.RemapLoggedChannel("/localizer", "y2024.localizer.Status");
for (const auto orin : {"orin1", "imu"}) {
@@ -40,12 +48,20 @@
}
}
reader.RemapLoggedChannel("/localizer", "frc971.controls.LocalizerOutput");
+ reader.RemapLoggedChannel("/localizer", "frc971.IMUValuesBatch");
+ reader.RemapLoggedChannel("/imu", "frc971.imu.DualImuBlenderStatus");
+ reader.RemapLoggedChannel("/imu/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/roborio/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/orin1/constants", "y2024.Constants");
auto factory =
std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
reader.RegisterWithoutStarting(factory.get());
+ y2024::SendSimulationConstants(reader.event_loop_factory(), FLAGS_team,
+ FLAGS_constants_path);
+
const aos::Node *node = nullptr;
if (aos::configuration::MultiNode(reader.configuration())) {
node = aos::configuration::GetNode(reader.configuration(), "imu");
@@ -56,6 +72,8 @@
aos::NodeEventLoopFactory *node_factory =
factory->GetNodeEventLoopFactory(node);
node_factory->AlwaysStart<y2024::localizer::Localizer>("localizer");
+ node_factory->AlwaysStart<frc971::imu_fdcan::DualImuBlender>(
+ "dual_imu_blender");
loggers.push_back(std::make_unique<aos::util::LoggerState>(
factory.get(), node, FLAGS_output_folder));
});
diff --git a/y2024/localizer/localizer_test.cc b/y2024/localizer/localizer_test.cc
index a2fc200..46763b1 100644
--- a/y2024/localizer/localizer_test.cc
+++ b/y2024/localizer/localizer_test.cc
@@ -427,7 +427,7 @@
CHECK(status_fetcher_.Fetch());
// We should've just ended up driving straight forwards.
EXPECT_LT(0.1, output_fetcher_->x());
- EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-8);
EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
EXPECT_NEAR(2.0, status_fetcher_->state()->left_voltage_error(), 1.0);
EXPECT_NEAR(2.0, status_fetcher_->state()->right_voltage_error(), 1.0);
@@ -506,7 +506,7 @@
send_targets_ = true;
// Send the minimum pose error to be rejected
constexpr double kEps = 1e-9;
- pose_error_ = 1e-6 + kEps;
+ pose_error_ = 1e-4 + kEps;
event_loop_factory_.RunFor(std::chrono::seconds(4));
CHECK(status_fetcher_.Fetch());
diff --git a/y2024/localizer/status.fbs b/y2024/localizer/status.fbs
index ad2ac72..6e31be2 100644
--- a/y2024/localizer/status.fbs
+++ b/y2024/localizer/status.fbs
@@ -1,5 +1,6 @@
include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
include "frc971/imu_reader/imu_failures.fbs";
+include "frc971/math/matrix.fbs";
namespace y2024.localizer;
@@ -28,6 +29,8 @@
ROBOT_TOO_FAST = 7,
// Pose estimation error ratio was higher than any normal detection.
HIGH_POSE_ERROR_RATIO = 8,
+ // Distortion is too high to trust.
+ HIGH_DISTORTION = 9,
}
table RejectionCount {
@@ -38,7 +41,7 @@
table CumulativeStatistics {
total_accepted:int (id: 0);
total_candidates:int (id: 1);
- rejection_reasons:[RejectionCount] (id: 2, static_length: 9);
+ rejection_reasons:[RejectionCount] (id: 2, static_length: 10);
}
table ImuStatus {
@@ -68,6 +71,7 @@
imu:ImuStatus (id: 2);
// Statistics are per-camera, by camera index.
statistics:[CumulativeStatistics] (id: 3);
+ ekf_covariance:frc971.fbs.Matrix (id: 4);
}
root_type Status;
diff --git a/y2024/localizer/visualization.fbs b/y2024/localizer/visualization.fbs
index d903e3a..6e61512 100644
--- a/y2024/localizer/visualization.fbs
+++ b/y2024/localizer/visualization.fbs
@@ -37,6 +37,9 @@
expected_observation:Measurement (id: 15);
actual_observation:Measurement (id: 16);
modeled_noise:Measurement (id: 17);
+ expected_robot_x:double (id: 18);
+ expected_robot_y:double (id: 19);
+ expected_robot_theta:double (id: 20);
}
table Visualization {
diff --git a/y2024/orin/can_logger_main.cc b/y2024/orin/can_logger_main.cc
index febed5f..dcfec69 100644
--- a/y2024/orin/can_logger_main.cc
+++ b/y2024/orin/can_logger_main.cc
@@ -11,8 +11,6 @@
::aos::ShmEventLoop event_loop(&config.message());
frc971::can_logger::CanLogger cana_logger(&event_loop, "/can/cana", "cana");
- frc971::can_logger::CanLogger canb_logger(&event_loop, "/can/canb", "canb");
- frc971::can_logger::CanLogger canc_logger(&event_loop, "/can/canc", "canc");
event_loop.Run();
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index d36c956..4904554 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -60,6 +60,7 @@
"//aos/events:shm_event_loop",
"//aos/events/logging:log_writer",
"//aos/logging:log_namer",
+ "//aos/util:filesystem_fbs",
"//frc971/input:joystick_state_fbs",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
diff --git a/y2024/vision/image_logger.cc b/y2024/vision/image_logger.cc
index 55a4e12..2984824 100644
--- a/y2024/vision/image_logger.cc
+++ b/y2024/vision/image_logger.cc
@@ -9,6 +9,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/log_namer.h"
+#include "aos/util/filesystem_generated.h"
#include "frc971/input/joystick_state_generated.h"
DEFINE_string(config, "aos_config.json", "Config file to use.");
@@ -48,10 +49,13 @@
aos::ShmEventLoop event_loop(&config.message());
+ aos::Fetcher<aos::util::FilesystemStatus> filesystem_status =
+ event_loop.MakeFetcher<aos::util::FilesystemStatus>("/aos");
+
bool logging = false;
bool enabled = false;
aos::monotonic_clock::time_point last_disable_time =
- event_loop.monotonic_now();
+ aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point last_rotation_time =
event_loop.monotonic_now();
aos::logger::Logger logger(&event_loop);
@@ -76,13 +80,36 @@
event_loop.MakeWatcher(
"/imu/aos", [&](const aos::JoystickState &joystick_state) {
const auto timestamp = event_loop.context().monotonic_event_time;
+ filesystem_status.Fetch();
+
// Store the last time we got disabled
if (enabled && !joystick_state.enabled()) {
last_disable_time = timestamp;
}
enabled = joystick_state.enabled();
- if (!logging && enabled) {
+ bool enough_space = true;
+
+ if (filesystem_status.get() != nullptr) {
+ enough_space = false;
+ for (const aos::util::Filesystem *fs :
+ *filesystem_status->filesystems()) {
+ CHECK(fs->has_path());
+ if (fs->path()->string_view() == "/") {
+ if (fs->free_space() > 50ull * 1024ull * 1024ull * 1024ull) {
+ enough_space = true;
+ }
+ }
+ }
+ }
+
+ const bool should_be_logging =
+ (enabled ||
+ timestamp < last_disable_time + std::chrono::duration<double>(
+ FLAGS_disabled_time)) &&
+ enough_space;
+
+ if (!logging && should_be_logging) {
auto log_namer = MakeLogNamer(&event_loop);
if (log_namer == nullptr) {
return;
@@ -93,9 +120,7 @@
logger.StartLogging(std::move(log_namer));
logging = true;
last_rotation_time = event_loop.monotonic_now();
- } else if (logging && !enabled &&
- (timestamp - last_disable_time) >
- std::chrono::duration<double>(FLAGS_disabled_time)) {
+ } else if (logging && !should_be_logging) {
// Stop logging if we've been disabled for a non-negligible amount of
// time
LOG(INFO) << "Stopping logging";
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index d286939..b67b07e 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -419,7 +419,7 @@
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, robot_constants);
- sensor_reader.set_pwm_trigger(true);
+ sensor_reader.set_pwm_trigger(false);
sensor_reader.set_drivetrain_left_encoder(
std::make_unique<frc::Encoder>(8, 9));
sensor_reader.set_drivetrain_right_encoder(
@@ -521,10 +521,15 @@
current_limits->catapult_stator_current_limit(),
current_limits->catapult_supply_current_limit());
+ transfer_roller->set_neutral_mode(
+ ctre::phoenix6::signals::NeutralModeValue::Coast);
+ intake_roller->set_neutral_mode(
+ ctre::phoenix6::signals::NeutralModeValue::Coast);
+
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
- constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+ constants::Values::kDrivetrainWriterPriority, true, "Drivetrain Bus");
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
@@ -584,8 +589,6 @@
control_loops::drivetrain::kHighOutputRatio);
}
- drivetrain_can_builder->set_timestamp(
- drivetrain_talonfxs.front()->GetTimestamp());
drivetrain_can_builder->set_status(static_cast<int>(status));
drivetrain_can_builder.CheckOk(drivetrain_can_builder.Send());
@@ -612,8 +615,6 @@
extend->SerializePosition(superstructure_can_builder->add_extend(),
superstructure::extend::kOutputRatio);
- superstructure_can_builder->set_timestamp(
- intake_pivot->GetTimestamp());
superstructure_can_builder->set_status(static_cast<int>(status));
superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
});
@@ -648,8 +649,6 @@
retention_roller->SerializePosition(
superstructure_can_builder->add_retention_roller(), 1.0);
- superstructure_can_builder->set_timestamp(
- intake_roller->GetTimestamp());
superstructure_can_builder->set_status(static_cast<int>(status));
superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
},
@@ -721,16 +720,6 @@
AddLoop(&can_output_event_loop);
- ::aos::ShmEventLoop pwm_event_loop(&config.message());
- SuperstructurePWMWriter superstructure_pwm_writer(&pwm_event_loop);
- superstructure_pwm_writer.set_catapult_kraken_one(
- make_unique<frc::TalonFX>(0));
- superstructure_pwm_writer.set_catapult_kraken_two(
- make_unique<frc::TalonFX>(1));
-
- AddLoop(&pwm_event_loop);
- // Thread 6
-
RunLoops();
}
};
diff --git a/y2024/www/BUILD b/y2024/www/BUILD
index 5ff91d6..5cb57c4 100644
--- a/y2024/www/BUILD
+++ b/y2024/www/BUILD
@@ -39,6 +39,8 @@
"//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//frc971/vision:target_map_ts_fbs",
"//y2024/control_loops/superstructure:superstructure_status_ts_fbs",
+ "//y2024/localizer:status_ts_fbs",
+ "//y2024/localizer:visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/y2024/www/field.html b/y2024/www/field.html
index cfb8778..8fd4ebb 100644
--- a/y2024/www/field.html
+++ b/y2024/www/field.html
@@ -11,15 +11,8 @@
<div>
<div id="field"> </div>
<div id="legend"> </div>
- <div id="vision_readouts">
- </div>
- <div id="message_bridge_status">
- <div>
- <div>Node</div>
- <div>Client</div>
- <div>Server</div>
- </div>
- </div>
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
</div>
<div>
<table>
@@ -114,6 +107,18 @@
<td>Altitude at Loading Position</td>
<td id="altitude_ready_for_load">FALSE</td>
</tr>
+ <tr>
+ <td>Turret in Range of Goal</td>
+ <td id="turret_in_range">FALSE</td>
+ </tr>
+ <tr>
+ <td>Altitude in Range of Goal</td>
+ <td id="altitude_in_range">FALSE</td>
+ </tr>
+ <tr>
+ <td>Altitude Above Minimum Angle </td>
+ <td id="altitude_above_min_angle">FALSE</td>
+ </tr>
</table>
<table>
<tr>
@@ -136,9 +141,6 @@
<td id="shot_distance"> NA </td>
</tr>
</table>
-
- <h3>Zeroing Faults:</h3>
- <p id="zeroing_faults"> NA </p>
</div>
<div>
<table>
@@ -266,6 +268,14 @@
</table>
</div>
</div>
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </div>
+ </div>
+ <div id="vision_readouts"> </div>
</body>
</html>
\ No newline at end of file
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
index 949b34b..1b4ad1e 100644
--- a/y2024/www/field_handler.ts
+++ b/y2024/www/field_handler.ts
@@ -9,6 +9,8 @@
import {SuperstructureState, IntakeRollerStatus, CatapultState, TransferRollerStatus, ExtendRollerStatus, ExtendStatus, NoteStatus, Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
import {TargetMap} from '../../frc971/vision/target_map_generated'
+import {RejectionReason} from '../localizer/status_generated'
+import {TargetEstimateDebug, Visualization} from '../localizer/visualization_generated'
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
@@ -20,6 +22,9 @@
const ROBOT_WIDTH = 29 * IN_TO_M;
const ROBOT_LENGTH = 32 * IN_TO_M;
+const CAMERA_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const CAMERAS = ['/orin1/camera0', '/orin1/camera1', '/imu/camera0', '/imu/camera1'];
+
export class FieldHandler {
private canvas = document.createElement('canvas');
private localizerOutput: LocalizerOutput|null = null;
@@ -28,11 +33,26 @@
private drivetrainCANPosition: DrivetrainCANPosition|null = null;
private superstructureStatus: SuperstructureStatus|null = null;
+ // Image information indexed by timestamp (seconds since the epoch), so that
+ // we can stop displaying images after a certain amount of time.
+ private localizerImageMatches = new Map<number, Visualization>();
private x: HTMLElement = (document.getElementById('x') as HTMLElement);
private y: HTMLElement = (document.getElementById('y') as HTMLElement);
private theta: HTMLElement =
(document.getElementById('theta') as HTMLElement);
+ private imagesAcceptedCounter: HTMLElement =
+ (document.getElementById('images_accepted') as HTMLElement);
+ // HTML elements for rejection reasons for individual cameras. Indices
+ // corresponding to RejectionReason enum values will be for those reasons. The
+ // final row will account for images rejected by the aprilrobotics detector
+ // instead of the localizer.
+ private rejectionReasonCells: HTMLElement[][] = [];
+ private messageBridgeDiv: HTMLElement =
+ (document.getElementById('message_bridge_status') as HTMLElement);
+ private clientStatuses = new Map<string, HTMLElement>();
+ private serverStatuses = new Map<string, HTMLElement>();
+
private fieldImage: HTMLImageElement = new Image();
private zeroingFaults: HTMLElement =
@@ -70,6 +90,13 @@
private altitude_ready_for_load: HTMLElement =
(document.getElementById('altitude_ready_for_load') as HTMLElement);
+ private turret_in_range: HTMLElement =
+ (document.getElementById('turret_in_range') as HTMLElement);
+ private altitude_in_range: HTMLElement =
+ (document.getElementById('altitude_in_range') as HTMLElement);
+ private altitude_above_min_angle: HTMLElement =
+ (document.getElementById('altitude_above_min_angle') as HTMLElement);
+
private intakePivot: HTMLElement =
(document.getElementById('intake_pivot') as HTMLElement);
@@ -138,7 +165,82 @@
this.fieldImage.src = '2024.png';
+ // Construct a table header.
+ {
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = 'Rejection Reason';
+ row.appendChild(nameCell);
+ for (const camera of CAMERAS) {
+ const nodeCell = document.createElement('div');
+ nodeCell.innerHTML = camera;
+ row.appendChild(nodeCell);
+ }
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ for (const value in RejectionReason) {
+ // Typescript generates an iterator that produces both numbers and
+ // strings... don't do anything on the string iterations.
+ if (isNaN(Number(value))) {
+ continue;
+ }
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = RejectionReason[value];
+ row.appendChild(nameCell);
+ this.rejectionReasonCells.push([]);
+ for (const camera of CAMERAS) {
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells[this.rejectionReasonCells.length - 1].push(
+ valueCell);
+ row.appendChild(valueCell);
+ }
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ // Add rejection reason row for aprilrobotics rejections.
+ {
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = 'Rejected by aprilrobotics';
+ row.appendChild(nameCell);
+ this.rejectionReasonCells.push([]);
+ for (const camera of CAMERAS) {
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells[this.rejectionReasonCells.length - 1].push(
+ valueCell);
+ row.appendChild(valueCell);
+ }
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ for (let ii = 0; ii < CAMERA_COLORS.length; ++ii) {
+ const legendEntry = document.createElement('div');
+ legendEntry.style.color = CAMERA_COLORS[ii];
+ legendEntry.innerHTML = CAMERAS[ii];
+ document.getElementById('legend').appendChild(legendEntry);
+ }
+
this.connection.addConfigHandler(() => {
+ // Visualization message is reliable so that we can see *all* the vision
+ // matches.
+ for (const camera in CAMERAS) {
+ this.connection.addHandler(
+ CAMERAS[camera], 'y2024.localizer.Visualization',
+ (data) => {
+ this.handleLocalizerDebug(Number(camera), data);
+ });
+ }
+ for (const camera in CAMERAS) {
+ // Make unreliable to reduce network spam.
+ this.connection.addHandler(
+ CAMERAS[camera], 'frc971.vision.TargetMap', (data) => {
+ this.handleCameraTargetMap(camera, data);
+ });
+ }
this.connection.addHandler(
'/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
@@ -161,8 +263,43 @@
(data) => {
this.handleSuperstructureStatus(data)
});
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ServerStatistics',
+ (data) => {this.handleServerStatistics(data)});
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ClientStatistics',
+ (data) => {this.handleClientStatistics(data)});
});
}
+ private handleLocalizerDebug(camera: number, data: Uint8Array): void {
+ const now = Date.now() / 1000.0;
+
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerImageMatches.set(
+ now, Visualization.getRootAsVisualization(fbBuffer));
+
+ const debug = this.localizerImageMatches.get(now);
+
+ if (debug.statistics()) {
+ if ((debug.statistics().rejectionReasonsLength() + 1) ==
+ this.rejectionReasonCells.length) {
+ for (let ii = 0; ii < debug.statistics().rejectionReasonsLength();
+ ++ii) {
+ this.rejectionReasonCells[ii][camera].innerHTML =
+ debug.statistics().rejectionReasons(ii).count().toString();
+ }
+ } else {
+ console.error('Unexpected number of rejection reasons in counter.');
+ }
+ }
+ }
+
+ private handleCameraTargetMap(pi: string, data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const targetMap = TargetMap.getRootAsTargetMap(fbBuffer);
+ this.rejectionReasonCells[this.rejectionReasonCells.length - 1][pi]
+ .innerHTML = targetMap.rejections().toString();
+ }
private handleDrivetrainStatus(data: Uint8Array): void {
const fbBuffer = new ByteBuffer(data);
@@ -189,6 +326,67 @@
this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
}
+ private populateNodeConnections(nodeName: string): void {
+ const row = document.createElement('div');
+ this.messageBridgeDiv.appendChild(row);
+ const nodeDiv = document.createElement('div');
+ nodeDiv.innerHTML = nodeName;
+ row.appendChild(nodeDiv);
+ const clientDiv = document.createElement('div');
+ clientDiv.innerHTML = 'N/A';
+ row.appendChild(clientDiv);
+ const serverDiv = document.createElement('div');
+ serverDiv.innerHTML = 'N/A';
+ row.appendChild(serverDiv);
+ this.serverStatuses.set(nodeName, serverDiv);
+ this.clientStatuses.set(nodeName, clientDiv);
+ }
+
+ private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+ void {
+ if (state === ConnectionState.CONNECTED) {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('faulted');
+ element.classList.add('connected');
+ } else {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('connected');
+ element.classList.add('faulted');
+ }
+ }
+
+ private handleServerStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const serverStatistics =
+ ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+ for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+ const connection = serverStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.serverStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.serverStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ private handleClientStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const clientStatistics =
+ ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+ for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+ const connection = clientStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.clientStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.clientStatuses.get(nodeName), connection.state());
+ }
+ }
+
drawField(): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
@@ -292,6 +490,9 @@
this.reset();
this.drawField();
+ // Draw the matches with debugging information from the localizer.
+ const now = Date.now() / 1000.0;
+
if (this.superstructureStatus) {
this.superstructureState.innerHTML =
SuperstructureState[this.superstructureStatus.state()];
@@ -323,6 +524,12 @@
this.setBoolean(this.extend_ready_for_catapult_transfer, this.superstructureStatus.extendReadyForCatapultTransfer());
+ this.setBoolean(this.turret_in_range, this.superstructureStatus.shooter().turretInRange())
+
+ this.setBoolean(this.altitude_in_range, this.superstructureStatus.shooter().altitudeInRange())
+
+ this.setBoolean(this.altitude_above_min_angle, this.superstructureStatus.shooter().altitudeAboveMinAngle())
+
if (this.superstructureStatus.shooter() &&
this.superstructureStatus.shooter().aimer()) {
this.turret_position.innerHTML = this.superstructureStatus.shooter()
@@ -536,6 +743,37 @@
this.drawRobot(
this.localizerOutput.x(), this.localizerOutput.y(),
this.localizerOutput.theta());
+
+ this.imagesAcceptedCounter.innerHTML =
+ this.localizerOutput.imageAcceptedCount().toString();
+ }
+
+ for (const [time, value] of this.localizerImageMatches) {
+ const age = now - time;
+ const kRemovalAge = 1.0;
+ if (age > kRemovalAge) {
+ this.localizerImageMatches.delete(time);
+ continue;
+ }
+ const kMaxImageAlpha = 0.5;
+ const ageAlpha = kMaxImageAlpha * (kRemovalAge - age) / kRemovalAge
+ for (let i = 0; i < value.targetsLength(); i++) {
+ const imageDebug = value.targets(i);
+ const x = imageDebug.impliedRobotX();
+ const y = imageDebug.impliedRobotY();
+ const theta = imageDebug.impliedRobotTheta();
+ const cameraX = imageDebug.cameraX();
+ const cameraY = imageDebug.cameraY();
+ const cameraTheta = imageDebug.cameraTheta();
+ const accepted = imageDebug.accepted();
+ // Make camera readings fade over time.
+ const alpha = Math.round(255 * ageAlpha).toString(16).padStart(2, '0');
+ const dashed = false;
+ const cameraRgb = CAMERA_COLORS[imageDebug.camera()];
+ const cameraRgba = cameraRgb + alpha;
+ this.drawRobot(x, y, theta, cameraRgba, dashed);
+ this.drawCamera(cameraX, cameraY, cameraTheta, cameraRgba);
+ }
}
window.requestAnimationFrame(() => this.draw());
diff --git a/y2024/www/styles.css b/y2024/www/styles.css
index 39b7519..a11187b 100644
--- a/y2024/www/styles.css
+++ b/y2024/www/styles.css
@@ -65,7 +65,8 @@
#vision_readouts > div > div {
display: table-cell;
padding: 5px;
- text-align: right;
+ text-align: left;
+ font: small;
}
#message_bridge_status > div {
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 155edb4..189c96c 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -2,6 +2,12 @@
"channels": [
{
"name": "/imu/aos",
+ "type": "aos.util.FilesystemStatus",
+ "source_node": "imu",
+ "frequency": 2
+ },
+ {
+ "name": "/imu/aos",
"type": "aos.JoystickState",
"source_node": "imu",
"frequency": 100,
@@ -35,8 +41,8 @@
"type": "aos.timing.Report",
"source_node": "imu",
"frequency": 50,
- "num_senders": 20,
- "max_size": 6184
+ "num_senders": 30,
+ "max_size": 8192
},
{
"name": "/imu/aos",
@@ -274,23 +280,8 @@
"name": "/can/cana",
"type": "frc971.can_logger.CanFrame",
"source_node": "imu",
- "frequency": 6000,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/can/canb",
- "type": "frc971.can_logger.CanFrame",
- "source_node": "imu",
- "frequency": 6000,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/can/canc",
- "type": "frc971.can_logger.CanFrame",
- "source_node": "imu",
- "frequency": 6000,
+ "frequency": 9000,
+ "channel_storage_duration": 7000000000,
"num_senders": 2,
"max_size": 200
},
@@ -315,7 +306,7 @@
"type": "y2024.localizer.Status",
"source_node": "imu",
"frequency": 1600,
- "max_size": 1600,
+ "max_size": 3200,
"num_senders": 2
},
{
@@ -332,13 +323,30 @@
"name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
],
"nodes": [
"imu"
]
},
{
+ "name": "irq_affinity",
+ "executable_name": "irq_affinity",
+ "user": "root",
+ "args": ["--user=pi", "--irq_config=orin_irq_config.json"],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "filesystem_monitor",
+ "executable_name": "filesystem_monitor",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "joystick_republish",
"executable_name": "joystick_republish",
"user": "pi",
@@ -351,7 +359,8 @@
"executable_name": "message_bridge_server",
"user": "pi",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"nodes": [
"imu"
@@ -376,6 +385,10 @@
{
"name": "imu_can_logger",
"executable_name": "can_logger",
+ "args": [
+ "--priority=59",
+ "--affinity=5"
+ ],
"nodes": [
"imu"
]
@@ -385,7 +398,7 @@
"name": "can_translator",
"executable_name": "can_translator",
"args": [
- "--channel=/can/canb"
+ "--channel=/can/cana"
],
"nodes": [
"imu"
@@ -480,7 +493,6 @@
"name": "argus_camera0",
"executable_name": "argus_camera",
"args": [
- "--enable_ftrace",
"--camera=0",
"--channel=/camera0"
],
@@ -493,7 +505,6 @@
"name": "argus_camera1",
"executable_name": "argus_camera",
"args": [
- "--enable_ftrace",
"--camera=1",
"--channel=/camera1"
],
diff --git a/y2024/y2024_orin1.json b/y2024/y2024_orin1.json
index a9f75d5..f09be40 100644
--- a/y2024/y2024_orin1.json
+++ b/y2024/y2024_orin1.json
@@ -2,10 +2,16 @@
"channels": [
{
"name": "/orin1/aos",
+ "type": "aos.util.FilesystemStatus",
+ "source_node": "orin1",
+ "frequency": 2
+ },
+ {
+ "name": "/orin1/aos",
"type": "aos.timing.Report",
"source_node": "orin1",
"frequency": 50,
- "num_senders": 20,
+ "num_senders": 30,
"max_size": 8192
},
{
@@ -250,7 +256,8 @@
"executable_name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
],
"user": "pi",
"nodes": [
@@ -261,7 +268,14 @@
"name": "irq_affinity",
"executable_name": "irq_affinity",
"user": "root",
- "args": ["--user=pi"],
+ "args": ["--user=pi", "--irq_config=orin_irq_config.json"],
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "filesystem_monitor",
+ "executable_name": "filesystem_monitor",
"nodes": [
"orin1"
]
@@ -270,7 +284,8 @@
"name": "message_bridge_server",
"executable_name": "message_bridge_server",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"user": "pi",
"nodes": [
@@ -344,7 +359,6 @@
"name": "argus_camera0",
"executable_name": "argus_camera",
"args": [
- "--enable_ftrace",
"--camera=0",
"--channel=/camera0",
],
@@ -357,7 +371,6 @@
"name": "argus_camera1",
"executable_name": "argus_camera",
"args": [
- "--enable_ftrace",
"--camera=1",
"--channel=/camera1",
],
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 5ba882a..e0e1131 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -176,7 +176,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 424
+ "max_size": 1024
},
{
"name": "/drivetrain",
@@ -239,7 +239,7 @@
{
"name": "imu",
"priority": 5,
- "time_to_live": 5000000
+ "time_to_live": 20000000
}
]
},
@@ -401,7 +401,8 @@
"executable_name": "message_bridge_client",
"args": [
"--rt_priority=16",
- "--sinit_max_init_timeout=5000"
+ "--sinit_max_init_timeout=5000",
+ "--rmem=2097152"
],
"nodes": [
"roborio"
@@ -411,7 +412,8 @@
"name": "roborio_message_bridge_server",
"executable_name": "message_bridge_server",
"args": [
- "--rt_priority=16"
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
],
"nodes": [
"roborio"
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
index 6dec3c9..175f238 100644
--- a/y2024_defense/wpilib_interface.cc
+++ b/y2024_defense/wpilib_interface.cc
@@ -332,7 +332,6 @@
control_loops::drivetrain::kHighOutputRatio);
}
- builder->set_timestamp(falcons.front()->GetTimestamp());
builder->set_status(static_cast<int>(status));
builder.CheckOk(builder.Send());