Merge "scouting: Clean up the code a tiny bit"
diff --git a/aos/aos_jitter.cc b/aos/aos_jitter.cc
index 259d59c..3ac35e8 100644
--- a/aos/aos_jitter.cc
+++ b/aos/aos_jitter.cc
@@ -18,6 +18,8 @@
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 {
@@ -28,14 +30,27 @@
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))) {
@@ -68,6 +83,22 @@
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_;
@@ -75,6 +106,10 @@
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
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/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/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/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index b931e52..0a41c55 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -364,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) {
@@ -377,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
@@ -392,24 +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;
- have_zeroed_encoders_ = true;
ResetInitialState(t, newstate, P_);
}
- 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:
@@ -740,6 +755,7 @@
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_;
@@ -920,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/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/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/imu_fdcan/can_translator_lib.cc b/frc971/imu_fdcan/can_translator_lib.cc
index 4336c82..bbf758d 100644
--- a/frc971/imu_fdcan/can_translator_lib.cc
+++ b/frc971/imu_fdcan/can_translator_lib.cc
@@ -14,7 +14,7 @@
"/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) {
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.cc b/frc971/imu_fdcan/dual_imu_blender_lib.cc
index 2627739..6099844 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.cc
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.cc
@@ -14,7 +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 size_t kSaturationCounterThreshold = 20;
+static constexpr int kSaturationCounterThreshold = 20;
using frc971::imu_fdcan::DualImuBlender;
@@ -46,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) {
@@ -65,14 +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_ = 0;
+ --saturated_counter_;
+ }
+ if (saturated_counter_ <= -kSaturationCounterThreshold) {
+ is_saturated_ = false;
+ saturated_counter_ = -kSaturationCounterThreshold;
+ } else if (saturated_counter_ >= kSaturationCounterThreshold) {
+ is_saturated_ = true;
+ saturated_counter_ = kSaturationCounterThreshold;
}
- if (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 b3aa5c0..223f3ed 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib.h
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.h
@@ -20,7 +20,8 @@
private:
aos::Sender<IMUValuesBatchStatic> imu_values_batch_sender_;
aos::Sender<imu::DualImuBlenderStatusStatic> dual_imu_blender_status_sender_;
- size_t saturated_counter_ = 0;
+ int saturated_counter_ = 0;
+ bool is_saturated_ = false;
};
} // namespace frc971::imu_fdcan
diff --git a/frc971/orin/apriltag.h b/frc971/orin/apriltag.h
index 8b5f0b2..7c0a721 100644
--- a/frc971/orin/apriltag.h
+++ b/frc971/orin/apriltag.h
@@ -193,7 +193,9 @@
distortion_coefficients_ = distortion_coefficients;
}
- static void UnDistort(double *u, double *v, const CameraMatrix *camera_matrix,
+ // 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:
diff --git a/frc971/orin/apriltag_detect.cc b/frc971/orin/apriltag_detect.cc
index 235a6d4..26de7fa 100644
--- a/frc971/orin/apriltag_detect.cc
+++ b/frc971/orin/apriltag_detect.cc
@@ -334,9 +334,10 @@
// We're undistorting using math found from this github page
// https://yangyushi.github.io/code/2020/03/04/opencv-undistort.html
-void GpuDetector::UnDistort(double *u, double *v,
+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;
@@ -377,6 +378,7 @@
yP = (y0 - tangential_dy) * radial_distortion_inv;
if (iterations > kUndistortIterationThreshold) {
+ converged = false;
break;
}
@@ -397,6 +399,8 @@
*u = xP * fx + cx;
*v = yP * fy + cy;
+
+ return converged;
}
// Mostly stolen from aprilrobotics, but modified to implement the dewarp.
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index 62ae36e..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>
@@ -631,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 19bee47..87d5f49 100644
--- a/frc971/orin/gpu_apriltag.cc
+++ b/frc971/orin/gpu_apriltag.cc
@@ -137,17 +137,19 @@
detection.distortion_factor, detection.pose_error_ratio);
}
-void ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
+bool ApriltagDetector::UndistortDetection(apriltag_detection_t *det) const {
// Copy the undistorted points into det
+ bool converged = true;
for (size_t i = 0; i < 4; i++) {
double u = det->p[i][0];
double v = det->p[i][1];
- GpuDetector::UnDistort(&u, &v, &distortion_camera_matrix_,
- &distortion_coefficients_);
+ 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(
@@ -275,7 +277,20 @@
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.
diff --git a/frc971/orin/gpu_apriltag.h b/frc971/orin/gpu_apriltag.h
index 5569e5a..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
diff --git a/frc971/orin/orin_irq_config.json b/frc971/orin/orin_irq_config.json
index 8ead6ba..c067e20 100644
--- a/frc971/orin/orin_irq_config.json
+++ b/frc971/orin/orin_irq_config.json
@@ -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",
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/y2024/constants/971.json b/y2024/constants/971.json
index 3a7cfad..908467d 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -48,7 +48,7 @@
"altitude_constants": {
{% set _ = altitude_zero.update(
{
- "measured_absolute_position" : 0.1964
+ "measured_absolute_position" : 0.1725
}
) %}
"zeroing_constants": {{ altitude_zero | tojson(indent=2)}},
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index a36844b..25fae23 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -4,46 +4,46 @@
"target_map": {% include 'y2024/vision/maps/target_map.json' %},
"shooter_interpolation_table": [
{
- "distance_from_goal": 0.8,
+ "distance_from_goal": 0.7,
"shot_params": {
"shot_altitude_angle": 0.85,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
},
{
- "distance_from_goal": 1.34,
+ "distance_from_goal": 1.24,
"shot_params": {
"shot_altitude_angle": 0.85,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
},
{
- "distance_from_goal": 2.004,
+ "distance_from_goal": 1.904,
"shot_params": {
"shot_altitude_angle": 0.73,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
},
// 2.2 -> high.
{
- "distance_from_goal": 2.844,
+ "distance_from_goal": 2.744,
"shot_params": {
"shot_altitude_angle": 0.62,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
},
{
- "distance_from_goal": 3.374,
+ "distance_from_goal": 3.274,
"shot_params": {
"shot_altitude_angle": 0.58,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
},
{
- "distance_from_goal": 4.10,
+ "distance_from_goal": 4.00,
"shot_params": {
"shot_altitude_angle": 0.54,
- "shot_speed_over_ground": 4.0
+ "shot_speed_over_ground": 8.0
}
}
],
@@ -221,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.309, 1.4435, 2.0705]
+ "data": [8.209, 1.4435, 2.0705]
},
"theta": 0.0
},
@@ -232,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.309, 1.4435, 2.0705]
+ "data": [-8.209, 1.4435, 2.0705]
},
"theta": 0.0
}
@@ -264,6 +264,7 @@
"retracted": 0.017
},
"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],
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index d33a161..c679128 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -194,6 +194,7 @@
// 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);
}
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
index 0e75589..282b146 100644
--- a/y2024/control_loops/python/drivetrain.py
+++ b/y2024/control_loops/python/drivetrain.py
@@ -16,7 +16,7 @@
J=6.5,
mass=68.0,
# TODO(austin): Measure radius a bit better.
- robot_radius=0.39,
+ robot_radius=0.32,
wheel_radius=2 * 0.0254 * 3.7592 / 3.825,
motor_type=control_loop.KrakenFOC(),
num_motors=2,
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 7fdf748..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_(
@@ -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,14 +106,16 @@
bool aiming = false;
- if (requested_note_goal == NoteGoal::AMP) {
+ 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(), 0.3);
+ 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.
@@ -118,15 +130,19 @@
// 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);
@@ -137,22 +153,24 @@
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();
const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
- kCatapultActivationThreshold);
+ kCatapultActivationTurretThreshold);
const bool altitude_in_range =
(std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationThreshold);
+ kCatapultActivationAltitudeThreshold);
const bool altitude_above_min_angle =
(altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());
@@ -169,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());
@@ -225,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:
@@ -271,6 +303,7 @@
catapult_.set_unprofiled_goal(2.0, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
+ ++shot_count_;
} else {
break;
}
@@ -303,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);
@@ -316,9 +347,8 @@
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);
- if (aiming) {
- status_builder.add_aimer(aimer_offset);
- }
+ 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 12078ff..a551993 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -109,9 +109,22 @@
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 &&
@@ -138,6 +151,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 016110a..b30d530 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,7 +49,9 @@
shooter_(event_loop, robot_constants_),
extend_(
robot_constants_->common()->extend(),
- robot_constants_->robot()->extend_constants()->zeroing_constants()) {
+ robot_constants_->robot()->extend_constants()->zeroing_constants()),
+ extend_debouncer_(std::chrono::milliseconds(30),
+ std::chrono::milliseconds(8)) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -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,18 +305,22 @@
case SuperstructureState::LOADING_CATAPULT:
extend_moving = false;
extend_goal_location = ExtendStatus::CATAPULT;
- extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
- transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
- // 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;
@@ -327,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;
@@ -346,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;
}
@@ -361,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;
}
@@ -374,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.
@@ -441,7 +459,20 @@
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;
}
@@ -504,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();
@@ -619,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);
@@ -626,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_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index b96bb65..e2c79d5 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())
@@ -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);
@@ -1223,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);
@@ -1232,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());
@@ -1320,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);
@@ -1337,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()
@@ -1391,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();
@@ -1470,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);
@@ -1496,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);
@@ -1503,7 +1495,7 @@
superstructure_plant_.set_extend_beambreak(true);
- RunFor(chrono::milliseconds(10));
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -1521,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);
@@ -1551,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);
@@ -1577,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/localizer/localizer.cc b/y2024/localizer/localizer.cc
index f8c1079..b8e982e 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -313,10 +313,6 @@
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;
@@ -331,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();
}
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 32dadbf..5baae1c 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -400,7 +400,7 @@
"name": "imu_can_logger",
"executable_name": "can_logger",
"args": [
- "--priority=55",
+ "--priority=59",
"--affinity=5"
],
"nodes": [
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index c7024f4..628e9d1 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -239,7 +239,7 @@
{
"name": "imu",
"priority": 5,
- "time_to_live": 5000000
+ "time_to_live": 20000000
}
]
},