Remove unused 2022 channels
IMUValuesBatch on the roborio is unused, I erroneously added a drivetrain
Position message on the imu node earlier, and the drivetrain test
library needed to be updated to support the encoders in the IMUValues
message (as well as the GyroReadings message).
Also, begin forwarding superstructure status message to the IMU pi, and
set the LocalizerControl message to be reliable.
Change-Id: I18f1b4131710e0539018002e91fce99f7f1215b4
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
index cbc9c42..93233d2 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -25,7 +25,7 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
drivetrain::MakeDrivetrainLoop,
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
index bbf0324..3332c1e 100644
--- a/y2022/control_loops/localizer/BUILD
+++ b/y2022/control_loops/localizer/BUILD
@@ -60,7 +60,6 @@
"//frc971/control_loops:c2d",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
"//frc971/control_loops/drivetrain:localizer_fbs",
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index a145ad4..d6afd56 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -507,9 +507,6 @@
model_based_(dt_config),
status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
- position_fetcher_(
- event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Position>(
- "/localizer")),
output_fetcher_(
event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
"/drivetrain")) {
@@ -526,26 +523,24 @@
event_loop_->MakeWatcher(
"/localizer", [this](const frc971::IMUValuesBatch &values) {
CHECK(values.has_readings());
- position_fetcher_.Fetch();
output_fetcher_.Fetch();
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
if (zeroer_.Zeroed()) {
- CHECK(position_fetcher_.get() != nullptr);
- CHECK(output_fetcher_.get() != nullptr);
- const bool disabled =
- output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- event_loop_->context().monotonic_event_time;
- model_based_.HandleImu(
- aos::monotonic_clock::time_point(
- std::chrono::nanoseconds(value->monotonic_timestamp_ns())),
- zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
- {position_fetcher_->left_encoder(),
- position_fetcher_->right_encoder()},
- disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()});
+ if (output_fetcher_.get() != nullptr) {
+ const bool disabled =
+ output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time;
+ model_based_.HandleImu(
+ aos::monotonic_clock::time_point(std::chrono::nanoseconds(
+ value->monotonic_timestamp_ns())),
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ {value->left_encoder(), value->right_encoder()},
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ }
}
{
auto builder = status_sender_.MakeBuilder();
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index f2d76ef..3a09b28 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -11,7 +11,6 @@
#include "y2022/control_loops/localizer/localizer_status_generated.h"
#include "y2022/control_loops/localizer/localizer_output_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
@@ -208,7 +207,6 @@
ModelBasedLocalizer model_based_;
aos::Sender<LocalizerStatus> status_sender_;
aos::Sender<LocalizerOutput> output_sender_;
- aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index db7ec8f..eb76e00 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -18,7 +18,7 @@
Imu::Imu(aos::ShmEventLoop *event_loop)
: event_loop_(event_loop),
imu_sender_(
- event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain")) {
+ event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
event_loop->SetRuntimeRealtimePriority(30);
imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index da0e4cb..793d31a 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -199,6 +199,9 @@
"frequency": 2200,
"max_size": 2000,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
"destination_nodes": [
{
"name": "logger",
@@ -227,6 +230,9 @@
"frequency": 200,
"max_size": 200,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
"destination_nodes": [
{
"name": "roborio",
@@ -271,9 +277,12 @@
"type": "frc971.IMUValuesBatch",
"source_node": "imu",
"frequency": 2200,
- "max_size": 1000,
+ "max_size": 1600,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
"destination_nodes": [
{
"name": "logger",
@@ -294,35 +303,6 @@
"frequency": 2200,
"num_senders": 2,
"max_size": 200
- },
- {
- "name": "/localizer",
- "type": "frc971.control_loops.drivetrain.Position",
- "source_node": "imu",
- "frequency": 200,
- "max_size": 200,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-control_loops-drivetrain-Position",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "imu",
- "logger": "NOT_LOGGED",
- "frequency": 200,
- "num_senders": 2,
- "max_size": 200
}
],
"applications": [
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 67f2822..592109e 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -20,22 +20,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 2,
- "time_to_live": 500000000
- }
- ]
- },
- {
- "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Position",
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 41f5baa..cb1b2d8 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -220,7 +220,18 @@
"type": "y2022.control_loops.superstructure.Status",
"source_node": "roborio",
"frequency": 200,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/superstructure",
@@ -240,14 +251,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "frequency": 250,
- "max_size": 2000,
- "num_senders": 2
- },
- {
- "name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",
"frequency": 200,
@@ -300,6 +303,9 @@
"max_size": 80,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
"destination_nodes": [
{
"name": "imu",
@@ -336,6 +342,9 @@
"frequency": 200,
"max_size": 96,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
"destination_nodes": [
{
"name": "imu",
@@ -344,7 +353,7 @@
"timestamp_logger_nodes": [
"imu"
],
- "time_to_live": 5000000
+ "time_to_live": 0
}
]
},