Initial tuning/cleanup of 2022 localizer
This is based on some data collection off of the 2020 robot with the IMU
somewhat loosely mounted. Should behave reasonably--when the wheels are
not slipping, this currently tends to be less precise than traditional
methods, but otherwise behaves reasonably, and does handle substantial
wheel slip reasonably.
General changes:
* General tuning.
* Update some 2020 references to refer to 2022.
* Unwrap the encoder readings from the pico board.
* Make use of the pico timestamps.
Next steps are:
* Adding actual connectors for image corrections.
* Making the turret able to aim.
* Tuning this against driver-practice based IMU readings--use TODOs
in the code as a starting point.
Change-Id: Ie3effe2cbb822317f6cd4a201cce939517a4044f
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
index 8e7ec95..f1bc2d1 100644
--- a/y2022/control_loops/localizer/BUILD
+++ b/y2022/control_loops/localizer/BUILD
@@ -66,7 +66,9 @@
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//frc971/zeroing:imu_zeroer",
+ "//frc971/zeroing:wrap",
"//y2020/vision/sift:sift_fbs",
+ "//y2022:constants",
"@org_tuxfamily_eigen//:eigen",
],
)
@@ -102,7 +104,7 @@
name = "localizer_replay",
srcs = ["localizer_replay.cc"],
data = [
- "//y2020:aos_config",
+ "//y2022:aos_config",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
@@ -114,6 +116,6 @@
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_reader",
"//aos/events/logging:log_writer",
- "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
],
)
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index 3c68e59..5cbee51 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -2,11 +2,7 @@
#include "frc971/control_loops/c2d.h"
#include "frc971/wpilib/imu_batch_generated.h"
-// TODO(james): Things to do:
-// -Approach still seems fundamentally sound.
-// -Really need to work on cost/residual function.
-// -Particularly for handling stopping.
-// -Extra hysteresis
+#include "y2022/constants.h"
namespace frc971::controls {
@@ -154,8 +150,11 @@
const ModelBasedLocalizer::ModelState &state) const {
const double robot_speed =
(state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- const double velocity_x = std::cos(state(kTheta)) * robot_speed;
- const double velocity_y = std::sin(state(kTheta)) * robot_speed;
+ const double lat_speed = (AModel(state) * state)(kTheta) * long_offset_;
+ const double velocity_x = std::cos(state(kTheta)) * robot_speed -
+ std::sin(state(kTheta)) * lat_speed;
+ const double velocity_y = std::sin(state(kTheta)) * robot_speed +
+ std::cos(state(kTheta)) * lat_speed;
return (AccelState() << state(0), state(1), state(2), velocity_x, velocity_y)
.finished();
}
@@ -181,6 +180,10 @@
// Convert the model state into the acceleration-based state-space and check
// the distance between the two (should really be a weighted norm, but all the
// numbers are on ~the same scale).
+ // TODO(james): Maybe weight lateral velocity divergence different than
+ // longitudinal? Seems like we tend to get false-positives currently when in
+ // sharp turns.
+ // TODO(james): For off-center gyros, maybe reduce noise when turning?
VLOG(2) << "divergence: "
<< (state.accel_state - AccelStateForModelState(state.model_state))
.transpose();
@@ -190,9 +193,10 @@
(state.model_state(kLeftVelocity) + state.model_state(kRightVelocity)) /
2.0;
const double model_lng_accel =
- (diff_model(kLeftVelocity) + diff_model(kRightVelocity)) / 2.0;
- const Eigen::Vector2d robot_frame_accel(
- model_lng_accel, diff_model(kTheta) * model_lng_velocity);
+ (diff_model(kLeftVelocity) + diff_model(kRightVelocity)) / 2.0 -
+ diff_model(kTheta) * diff_model(kTheta) * long_offset_;
+ const double model_lat_accel = diff_model(kTheta) * model_lng_velocity;
+ const Eigen::Vector2d robot_frame_accel(model_lng_accel, model_lat_accel);
const Eigen::Vector2d model_accel =
Eigen::AngleAxisd(state.model_state(kTheta), Eigen::Vector3d::UnitZ())
.toRotationMatrix()
@@ -203,7 +207,7 @@
std::abs(diff_accel(kTheta) - diff_model(kTheta));
const Eigen::Vector2d accel_vel = state.accel_state.bottomRows<2>();
- const Eigen::Vector2d model_vel =
+ Eigen::Vector2d model_vel =
AccelStateForModelState(state.model_state).bottomRows<2>();
velocity_residual_ = (accel_vel - model_vel).norm() /
(1.0 + accel_vel.norm() + model_vel.norm());
@@ -212,6 +216,22 @@
return velocity_residual_ + theta_rate_residual_ + accel_residual_;
}
+void ModelBasedLocalizer::UpdateState(
+ CombinedState *state,
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
+ const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
+ const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
+ const AccelInput &accel_input, const ModelInput &model_input,
+ aos::monotonic_clock::duration dt) {
+ state->accel_state = UpdateAccel(state->accel_state, accel_input, dt);
+ if (down_estimator_.consecutive_still() > 500.0) {
+ state->accel_state(kVelocityX) *= 0.9;
+ state->accel_state(kVelocityY) *= 0.9;
+ }
+ state->model_state = UpdateModel(state->model_state, model_input, dt);
+ state->model_state += K * (Z - H * state->model_state);
+}
+
void ModelBasedLocalizer::HandleImu(aos::monotonic_clock::time_point t,
const Eigen::Vector3d &gyro,
const Eigen::Vector3d &accel,
@@ -229,7 +249,8 @@
t_ = t;
down_estimator_.Predict(gyro, accel, dt);
// TODO(james): Should we prefer this or use the down-estimator corrected
- // version?
+ // version? Using the down estimator is more principled, but does create more
+ // opportunities for subtle biases.
const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
const double diameter = 2.0 * dt_config_.robot_radius;
@@ -240,12 +261,8 @@
const Eigen::Vector3d absolute_accel =
orientation * dt_config_.imu_transform * kG * accel;
abs_accel_ = absolute_accel;
- const Eigen::Vector3d absolute_gyro =
- orientation * dt_config_.imu_transform * gyro;
- (void)absolute_gyro;
VLOG(2) << "abs accel " << absolute_accel.transpose();
- VLOG(2) << "abs gyro " << absolute_gyro.transpose();
VLOG(2) << "dt " << aos::time::DurationInSeconds(dt);
// Update all the branched states.
@@ -299,14 +316,9 @@
VLOG(2) << "Z\n" << Z.transpose();
for (CombinedState &state : branches_) {
- state.accel_state = UpdateAccel(state.accel_state, accel_input, dt);
- if (down_estimator_.consecutive_still() > 100.0) {
- state.accel_state(kVelocityX) *= 0.9;
- state.accel_state(kVelocityY) *= 0.9;
- }
- state.model_state = UpdateModel(state.model_state, model_input, dt);
- state.model_state += K * (Z - H * state.model_state);
+ UpdateState(&state, K, Z, H, accel_input, model_input, dt);
}
+ UpdateState(¤t_state_, K, Z, H, accel_input, model_input, dt);
VLOG(2) << "oildest accel " << branches_[0].accel_state.transpose();
VLOG(2) << "oildest accel diff "
@@ -326,8 +338,13 @@
filtered_residual_ +=
(1.0 - std::exp(-aos::time::DurationInSeconds(kNominalDt) / 0.0095)) *
(model_divergence - filtered_residual_);
- constexpr double kUseAccelThreshold = 0.4;
- constexpr double kUseModelThreshold = 0.2;
+ // TODO(james): Tune this more. Currently set to generally trust the model,
+ // perhaps a bit too much.
+ // When the residual exceeds the accel threshold, we start using the inertials
+ // alone; when it drops back below the model threshold, we go back to being
+ // model-based.
+ constexpr double kUseAccelThreshold = 2.0;
+ constexpr double kUseModelThreshold = 0.5;
constexpr size_t kShareStates = kNModelStates;
static_assert(kUseModelThreshold < kUseAccelThreshold);
if (using_model_) {
@@ -347,7 +364,6 @@
current_state_.accel_state, encoders, yaw_rate);
} else {
VLOG(2) << "Normal branching";
- current_state_.model_state = branches_[branches_.size() - 1].model_state;
current_state_.accel_state =
AccelStateForModelState(current_state_.model_state);
current_state_.branch_time = t;
@@ -368,9 +384,7 @@
current_state_.accel_state =
AccelStateForModelState(current_state_.model_state);
} else {
- current_state_.accel_state = branches_[branches_.size() - 1].accel_state;
// TODO(james): Why was I leaving the encoders/wheel velocities in place?
- current_state_.model_state = branches_[branches_.size() - 1].model_state;
current_state_.model_state = ModelStateForAccelState(
current_state_.accel_state, encoders, yaw_rate);
current_state_.branch_time = t;
@@ -379,11 +393,11 @@
// Generate a new branch, with the accel state reset based on the model-based
// state (really, just getting rid of the lateral velocity).
+ // By resetting the accel state in the new branch, this tries to minimize the
+ // odds of runaway lateral velocities. This doesn't help with runaway
+ // longitudinal velocities, however.
CombinedState new_branch = current_state_;
- //if (!keep_accel_state) {
- if (using_model_) {
- new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
- }
+ new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
new_branch.accumulated_divergence = 0.0;
++branch_counter_;
@@ -496,6 +510,13 @@
return builder.Finish();
}
+namespace {
+// Period at which the encoder readings from the IMU board wrap.
+static double DrivetrainWrapPeriod() {
+ return y2022::constants::Values::DrivetrainEncoderToMeters(1 << 16);
+}
+}
+
EventLoopLocalizer::EventLoopLocalizer(
aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
@@ -505,7 +526,9 @@
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
output_fetcher_(
event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
- "/drivetrain")) {
+ "/drivetrain")),
+ left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
+ right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
event_loop_->MakeWatcher(
"/drivetrain",
[this](
@@ -522,31 +545,58 @@
output_fetcher_.Fetch();
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
+ const Eigen::Vector2d encoders{
+ left_encoder_.Unwrap(value->left_encoder()),
+ right_encoder_.Unwrap(value->right_encoder())};
if (zeroer_.Zeroed()) {
- 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()});
+ const aos::monotonic_clock::time_point pico_timestamp{
+ std::chrono::microseconds(value->pico_timestamp_us())};
+ // TODO(james): If we get large enough drift off of the pico,
+ // actually do something about it.
+ if (!pico_offset_.has_value()) {
+ pico_offset_ =
+ event_loop_->context().monotonic_event_time - pico_timestamp;
+ last_pico_timestamp_ = pico_timestamp;
}
+ if (pico_timestamp < last_pico_timestamp_) {
+ pico_offset_.value() += std::chrono::microseconds(1ULL << 32);
+ }
+ const aos::monotonic_clock::time_point sample_timestamp =
+ pico_offset_.value() + pico_timestamp;
+ pico_offset_error_ =
+ event_loop_->context().monotonic_event_time - sample_timestamp;
+ const bool disabled =
+ (output_fetcher_.get() == nullptr) ||
+ (output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time);
+ model_based_.HandleImu(
+ sample_timestamp,
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(), encoders,
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ last_pico_timestamp_ = pico_timestamp;
}
{
auto builder = status_sender_.MakeBuilder();
const flatbuffers::Offset<ModelBasedStatus> model_based_status =
model_based_.PopulateStatus(builder.fbb());
+ const flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState>
+ zeroer_status = zeroer_.PopulateStatus(builder.fbb());
LocalizerStatus::Builder status_builder =
builder.MakeBuilder<LocalizerStatus>();
status_builder.add_model_based(model_based_status);
status_builder.add_zeroed(zeroer_.Zeroed());
status_builder.add_faulted_zero(zeroer_.Faulted());
+ status_builder.add_zeroing(zeroer_status);
+ status_builder.add_left_encoder(encoders(0));
+ status_builder.add_right_encoder(encoders(1));
+ if (pico_offset_.has_value()) {
+ status_builder.add_pico_offset_ns(pico_offset_.value().count());
+ status_builder.add_pico_offset_error_ns(
+ pico_offset_error_.count());
+ }
builder.CheckOk(builder.Send(status_builder.Finish()));
}
if (last_output_send_ + std::chrono::milliseconds(5) <
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index bb52a40..2c4d1a4 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -14,6 +14,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
+#include "frc971/zeroing/wrap.h"
namespace frc971::controls {
@@ -57,7 +58,6 @@
// TODO:
// * Implement paying attention to camera readings.
// * Tune for ADIS16505/real robot.
-// * Tune down CPU usage to run on a pi.
class ModelBasedLocalizer {
public:
static constexpr size_t kX = 0;
@@ -90,7 +90,9 @@
static constexpr size_t kNModelInputs = 2;
// Branching period, in cycles.
- static constexpr int kBranchPeriod = 1;
+ // Needs 10 to even stay alive, and still at ~96% CPU.
+ // ~20 gives ~55-60% CPU.
+ static constexpr int kBranchPeriod = 20;
typedef Eigen::Matrix<double, kNModelStates, 1> ModelState;
typedef Eigen::Matrix<double, kNAccelStates, 1> AccelState;
@@ -122,6 +124,8 @@
AccelState accel_state() const { return current_state_.accel_state; };
+ void set_longitudinal_offset(double offset) { long_offset_ = offset; }
+
private:
struct CombinedState {
AccelState accel_state;
@@ -155,6 +159,13 @@
const AccelInput &accel_inputs,
const Eigen::Vector2d &filtered_accel,
const ModelInput &model_inputs);
+ void UpdateState(
+ CombinedState *state,
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
+ const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
+ const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
+ const AccelInput &accel_input, const ModelInput &model_input,
+ aos::monotonic_clock::duration dt);
const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
const StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
@@ -186,6 +197,10 @@
aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
bool using_model_;
+ // X position of the IMU, in meters. 0 = center of robot, positive = ahead of
+ // center, negative = behind center.
+ double long_offset_ = -0.15;
+
double last_residual_ = 0.0;
double filtered_residual_ = 0.0;
Eigen::Vector2d filtered_residual_accel_ = Eigen::Vector2d::Zero();
@@ -206,6 +221,8 @@
aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+ ModelBasedLocalizer *localizer() { return &model_based_; }
+
private:
aos::EventLoop *event_loop_;
ModelBasedLocalizer model_based_;
@@ -215,6 +232,14 @@
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
aos::monotonic_clock::min_time;
+ std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
+ aos::monotonic_clock::duration pico_offset_error_;
+ // t = pico_offset_ + pico_timestamp.
+ // Note that this can drift over sufficiently long time periods!
+ std::optional<std::chrono::nanoseconds> pico_offset_;
+
+ zeroing::UnwrapSensor left_encoder_;
+ zeroing::UnwrapSensor right_encoder_;
};
} // namespace frc971::controls
#endif // Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
diff --git a/y2022/control_loops/localizer/localizer_plotter.ts b/y2022/control_loops/localizer/localizer_plotter.ts
index ce348e7..239d2cb 100644
--- a/y2022/control_loops/localizer/localizer_plotter.ts
+++ b/y2022/control_loops/localizer/localizer_plotter.ts
@@ -22,7 +22,7 @@
const localizer = aosPlotter.addMessageSource(
'/localizer', 'frc971.controls.LocalizerStatus');
const imu = aosPlotter.addRawMessageSource(
- '/drivetrain', 'frc971.IMUValuesBatch',
+ '/localizer', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
// Drivetrain Status estimated relative position
@@ -48,9 +48,21 @@
positionPlot.addMessageLine(position, ['left_encoder'])
.setColor(BROWN)
.setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['left_encoder'])
+ .setColor(RED)
+ .setDrawLine(false);
positionPlot.addMessageLine(position, ['right_encoder'])
.setColor(CYAN)
.setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['right_encoder'])
+ .setColor(GREEN)
+ .setDrawLine(false);
// Drivetrain Velocities
@@ -167,13 +179,6 @@
accelPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_x'])
- .setColor(PINK);
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_y'])
- .setColor(GREEN);
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_z'])
- .setColor(BLUE);
-
accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_x'])
.setColor(RED)
.setDrawLine(false);
@@ -197,11 +202,8 @@
xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
xPositionPlot.addMessageLine(status, ['down_estimator', 'position_x'])
.setColor(BLUE);
- xPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'x']).setColor(GREEN);
xPositionPlot.addMessageLine(localizer, ['model_based', 'x']).setColor(CYAN);
- xPositionPlot.plot.setDefaultYRange([0.0, 0.5]);
-
// Absolute Y Position
const yPositionPlot = aosPlotter.addPlot(element);
yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
@@ -212,7 +214,6 @@
localizerY.setColor(RED);
yPositionPlot.addMessageLine(status, ['down_estimator', 'position_y'])
.setColor(BLUE);
- yPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'y']).setColor(GREEN);
yPositionPlot.addMessageLine(localizer, ['model_based', 'y']).setColor(CYAN);
// Gyro
@@ -264,4 +265,13 @@
costPlot.addMessageLine(localizer, ['model_based', 'accel_residual'])
.setColor(CYAN)
.setPointSize(0);
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Timing');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot.addMessageLine(localizer, ['model_based', 'clock_resets'])
+ .setColor(GREEN)
+ .setDrawLine(false);
}
diff --git a/y2022/control_loops/localizer/localizer_replay.cc b/y2022/control_loops/localizer/localizer_replay.cc
index c438e53..199882c 100644
--- a/y2022/control_loops/localizer/localizer_replay.cc
+++ b/y2022/control_loops/localizer/localizer_replay.cc
@@ -9,9 +9,9 @@
#include "y2022/control_loops/localizer/localizer.h"
#include "y2022/control_loops/localizer/localizer_schema.h"
#include "gflags/gflags.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-DEFINE_string(config, "y2020/aos_config.json",
+DEFINE_string(config, "y2022/aos_config.json",
"Name of the config file to replay using.");
DEFINE_int32(team, 7971, "Team number to use for logfile replay.");
DEFINE_string(output_folder, "/tmp/replayed",
@@ -58,33 +58,10 @@
aos::logger::SortParts(unsorted_logfiles);
// open logfiles
- aos::logger::LogReader reader(logfiles);
- // Patch in any new channels.
- // TODO(james): With some of the extra changes I've made recently, this is no
- // longer adequate for replaying old logfiles. Just stop trying to support old
- // logs.
- aos::FlatbufferDetachedBuffer<aos::Configuration> updated_config =
- aos::configuration::MergeWithConfig(
- reader.configuration(),
- aos::configuration::AddSchema(
- R"channel({
- "channels": [
- {
- "name": "/localizer",
- "type": "frc971.controls.LocalizerStatus",
- "source_node": "roborio",
- "frequency": 2000,
- "max_size": 2000,
- "num_senders": 2
- }
- ]
-})channel",
- {aos::FlatbufferVector<reflection::Schema>(
- aos::FlatbufferSpan<reflection::Schema>(
- frc971::controls::LocalizerStatusSchema()))}));
+ aos::logger::LogReader reader(logfiles, &config.message());
- auto factory = std::make_unique<aos::SimulatedEventLoopFactory>(
- &updated_config.message());
+ auto factory =
+ std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
reader.Register(factory.get());
@@ -92,7 +69,7 @@
// List of nodes to create loggers for (note: currently just roborio; this
// code was refactored to allow easily adding new loggers to accommodate
// debugging and potential future changes).
- const std::vector<std::string> nodes_to_log = {"roborio"};
+ const std::vector<std::string> nodes_to_log = {"imu"};
for (const std::string &node : nodes_to_log) {
loggers.emplace_back(std::make_unique<LoggerState>(
&reader, aos::configuration::GetNode(reader.configuration(), node)));
@@ -100,7 +77,7 @@
const aos::Node *node = nullptr;
if (aos::configuration::MultiNode(reader.configuration())) {
- node = aos::configuration::GetNode(reader.configuration(), "roborio");
+ node = aos::configuration::GetNode(reader.configuration(), "imu");
}
std::unique_ptr<aos::EventLoop> localizer_event_loop =
@@ -109,7 +86,7 @@
frc971::controls::EventLoopLocalizer localizer(
localizer_event_loop.get(),
- y2020::control_loops::drivetrain::GetDrivetrainConfig());
+ y2022::control_loops::drivetrain::GetDrivetrainConfig());
reader.event_loop_factory()->Run();
diff --git a/y2022/control_loops/localizer/localizer_status.fbs b/y2022/control_loops/localizer/localizer_status.fbs
index 6771c5f..80cee0b 100644
--- a/y2022/control_loops/localizer/localizer_status.fbs
+++ b/y2022/control_loops/localizer/localizer_status.fbs
@@ -82,6 +82,18 @@
zeroed:bool (id: 1);
// Whether the IMU zeroing is faulted or not.
faulted_zero:bool (id: 2);
+ zeroing:control_loops.drivetrain.ImuZeroerState (id: 3);
+ // Offset between the pico clock and the pi clock, such that
+ // pico_timestamp + pico_offset_ns = pi_timestamp
+ pico_offset_ns:int64 (id: 4);
+ // Error in the offset, if we assume that the pi/pico clocks are identical and
+ // that there is a perfectly consistent latency between the two. Will be zero
+ // for the very first cycle, and then referenced off of the initial offset
+ // thereafter. If greater than zero, implies that the pico is "behind",
+ // whether due to unusually large latency or due to clock drift.
+ pico_offset_error_ns:int64 (id: 5);
+ left_encoder:double (id: 6);
+ right_encoder:double (id: 7);
}
root_type LocalizerStatus;
diff --git a/y2022/control_loops/localizer/localizer_test.cc b/y2022/control_loops/localizer/localizer_test.cc
index 79a69b5..c58f664 100644
--- a/y2022/control_loops/localizer/localizer_test.cc
+++ b/y2022/control_loops/localizer/localizer_test.cc
@@ -33,7 +33,9 @@
LocalizerTest()
: dt_config_(
control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
- localizer_(dt_config_) {}
+ localizer_(dt_config_) {
+ localizer_.set_longitudinal_offset(0.0);
+ }
ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
return localizer_.DiffModel(state, U);
}
@@ -174,7 +176,7 @@
Eigen::Vector3d gyro{0.0, 0.0, 0.0};
const Eigen::Vector2d encoders{0.0, 0.0};
const Eigen::Vector2d voltages{0.0, 0.0};
- Eigen::Vector3d accel{1.0, 0.2, 9.80665};
+ Eigen::Vector3d accel{5.0, 2.0, 9.80665};
Eigen::Vector3d accel_gs = accel / 9.80665;
while (t < start) {
// Spin to fill up the buffer.
@@ -205,8 +207,8 @@
EXPECT_EQ(0.0, xytheta(2));
}
- ASSERT_NEAR(1.0, localizer_.accel_state()(kVelocityX), 1e-10);
- ASSERT_NEAR(0.2, localizer_.accel_state()(kVelocityY), 1e-10);
+ ASSERT_NEAR(accel(0), localizer_.accel_state()(kVelocityX), 1e-10);
+ ASSERT_NEAR(accel(1), localizer_.accel_state()(kVelocityY), 1e-10);
// Start going in a cirlce, and confirm that we
// handle things correctly. We rotate the accelerometer readings by 90 degrees
@@ -315,6 +317,7 @@
"/localizer")),
status_fetcher_(
imu_test_event_loop_->MakeFetcher<LocalizerStatus>("/localizer")) {
+ localizer_.localizer()->set_longitudinal_offset(0.0);
aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
auto builder = output_sender_.MakeBuilder();
auto output_builder = builder.MakeBuilder<Output>();
@@ -480,7 +483,7 @@
CHECK(status_fetcher_.Fetch());
// Should still be using the model, but have a non-trivial residual.
ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
+ ASSERT_LT(0.02, status_fetcher_->model_based()->residual())
<< aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
// Afer running for a while, voltage error terms should converge and result in
@@ -497,7 +500,7 @@
2.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
0.1)
<< aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_GT(0.01, status_fetcher_->model_based()->residual())
+ ASSERT_GT(0.02, status_fetcher_->model_based()->residual())
<< aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
}