Update y2020 configs with 5th pi + timestamp channels
Also, update the drivetrain_replay_test to go with it.
Change-Id: Ic88b2f79b14b260e345cfec4f3e5ce3e16a5bd2f
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 1b98592..75a8475 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -117,16 +117,6 @@
],
)
-aos_config(
- name = "replay_config",
- src = "drivetrain_replay_config.json",
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//y2020:config",
- ],
-)
-
cc_test(
name = "localizer_test",
srcs = ["localizer_test.cc"],
@@ -150,8 +140,8 @@
name = "drivetrain_replay_test",
srcs = ["drivetrain_replay_test.cc"],
data = [
- ":replay_config",
- "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+ "//y2020:config",
+ "@drivetrain_replay",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_config.json b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
deleted file mode 100644
index 987e55b..0000000
--- a/y2020/control_loops/drivetrain/drivetrain_replay_config.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
- "channels": [
- {
- "name": "/drivetrain",
- "type": "frc971.IMUValues",
- "frequency": 2000,
- "source_node": "roborio"
- }
- ],
- "imports": [
- "../../y2020.json"
- ]
-}
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 2e6b327..e6a3f6e 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -22,9 +22,10 @@
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_string(
- logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+ logfile,
+ "external/drivetrain_replay/",
"Name of the logfile to read from.");
-DEFINE_string(config, "y2020/control_loops/drivetrain/replay_config.json",
+DEFINE_string(config, "y2020/config.json",
"Name of the config file to replay using.");
namespace y2020 {
@@ -36,7 +37,8 @@
public:
DrivetrainReplayTest()
: config_(aos::configuration::ReadConfig(FLAGS_config)),
- reader_(FLAGS_logfile, &config_.message()) {
+ reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile)),
+ &config_.message()) {
aos::network::OverrideTeamNumber(971);
// TODO(james): Actually enforce not sending on the same buses as the
@@ -55,10 +57,6 @@
frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
GetDrivetrainConfig();
- // Make the modification required to the imu transform to work with the 2016
- // logs...
- config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
- config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
localizer_ =
std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
@@ -70,25 +68,6 @@
test_event_loop_ =
reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
- // IMU readings used to be published out one at a time, but we now expect
- // batches. Batch them up to upgrade the data.
- imu_sender_ =
- test_event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain");
- test_event_loop_->MakeWatcher(
- "/drivetrain", [this](const frc971::IMUValues &values) {
- aos::Sender<frc971::IMUValuesBatch>::Builder builder =
- imu_sender_.MakeBuilder();
- flatbuffers::Offset<frc971::IMUValues> values_offsets =
- aos::CopyFlatBuffer(&values, builder.fbb());
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
- values_offset = builder.fbb()->CreateVector(&values_offsets, 1);
- frc971::IMUValuesBatch::Builder imu_values_batch_builder =
- builder.MakeBuilder<frc971::IMUValuesBatch>();
- imu_values_batch_builder.add_readings(values_offset);
- builder.Send(imu_values_batch_builder.Finish());
- });
-
status_fetcher_ = test_event_loop_->MakeFetcher<
frc971::control_loops::drivetrain::Status>("/drivetrain");
}
@@ -102,7 +81,6 @@
std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
drivetrain_;
std::unique_ptr<aos::EventLoop> test_event_loop_;
- aos::Sender<frc971::IMUValuesBatch> imu_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
};
@@ -116,7 +94,10 @@
ASSERT_TRUE(status_fetcher_->has_x());
ASSERT_TRUE(status_fetcher_->has_y());
ASSERT_TRUE(status_fetcher_->has_theta());
- EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
+ EXPECT_NEAR(status_fetcher_->estimated_left_position(),
+ status_fetcher_->estimated_right_position(), 0.1);
+ EXPECT_LT(std::abs(status_fetcher_->x()),
+ std::abs(status_fetcher_->estimated_left_position()) / 2.0);
// Because the encoders should not be affecting the y or yaw axes, expect a
// reasonably precise result (although, since this is a real worl dtest, the
// robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 3059bf1..32c8834 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -24,7 +24,7 @@
}
// Indices of the pis to use.
-const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
// Calculates the pose implied by the camera target, just based on
// distance/heading components.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 5dbad02..c3b6464 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -65,7 +65,7 @@
right_encoder, 0, 0, 0, 0, 0, 0)
.finished(),
ekf_.P());
- };
+ }
private:
// Storage for a single turret position data point.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index d195b5d..a3e26b5 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -131,13 +131,14 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
event_loop_factory()->SetTimeConverter(&time_converter_);
- CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 5);
+ CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 6);
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
time_converter_.AddMonotonic({monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch() + kPiTimeOffset,
+ monotonic_clock::epoch() + kPiTimeOffset,
monotonic_clock::epoch()});
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);