Make drivetrain use generic localizer
This will lead into switching y2019 over to using the Localizer with
camera images.
Change-Id: I2790c09ed9e1cec8d6141c25a4b5a173c2ddc24c
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 6725987..663d36b 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -66,6 +66,15 @@
)
cc_library(
+ name = "localizer",
+ hdrs = ["localizer.h"],
+ deps = [
+ ":drivetrain_config",
+ ":hybrid_ekf",
+ ],
+)
+
+cc_library(
name = "gear",
hdrs = [
"gear.h",
@@ -102,6 +111,7 @@
":drivetrain_config",
":drivetrain_queue",
":gear",
+ ":localizer",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
@@ -210,6 +220,7 @@
":down_estimator",
":drivetrain_queue",
":gear",
+ ":localizer",
":polydrivetrain",
":splinedrivetrain",
":ssdrivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index d097ec8..9cf71f7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -31,13 +31,15 @@
DrivetrainLoop::DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
const ::std::string &name)
: aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
event_loop, name),
dt_config_(dt_config),
+ localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
- dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -80,51 +82,6 @@
}
}
-::Eigen::Matrix<double, 3, 1> DrivetrainLoop::PredictState(
- const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
- const ::Eigen::Matrix<double, 7, 1> &state,
- const ::Eigen::Matrix<double, 7, 1> &previous_state) const {
- const double dt =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count();
-
- const double distance_traveled =
- (state(0) + state(2)) / 2.0 -
- (previous_state(0) + previous_state(2)) / 2.0;
-
- const double omega0 =
- (previous_state(3) - previous_state(1)) / (dt_config_.robot_radius * 2.0);
- const double omega1 = (state(3) - state(1)) / (dt_config_.robot_radius * 2.0);
- const double alpha = (omega1 - omega0) / dt;
-
- const double velocity_start = (previous_state(3) + previous_state(1)) / 2.0;
- const double velocity_end = (state(3) + state(1)) / 2.0;
-
- const double acceleration = (velocity_end - velocity_start) / dt;
- const double velocity_offset =
- distance_traveled / dt - 0.5 * acceleration * dt - velocity_start;
- const double velocity0 = velocity_start + velocity_offset;
-
- // TODO(austin): Substep 10x here. This is super important! ?
- return RungeKutta(
- [&dt, &velocity0, &acceleration, &omega0, &alpha](
- double t, const ::Eigen::Matrix<double, 3, 1> &X) {
- const double velocity1 = velocity0 + acceleration * t;
- const double omega1 = omega0 + alpha * t;
- const double theta = X(2);
-
- return (::Eigen::Matrix<double, 3, 1>()
- << ::std::cos(theta) * velocity1,
- ::std::sin(theta) * velocity1, omega1)
- .finished();
- },
- xytheta_state, 0.0,
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count());
-}
-
void DrivetrainLoop::RunIteration(
const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
const ::frc971::control_loops::DrivetrainQueue::Position *position,
@@ -274,52 +231,9 @@
Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
-
- // We are going to choose to integrate velocity to get position by assuming
- // that velocity is a linear function of time. For drivetrains with large
- // amounts of mass, we won't get large changes in acceleration over a 5 ms
- // timestep. Do note, the only place that this matters is when we are
- // talking about the curvature errors introduced by integration. The
- // velocities are scaled such that the distance traveled is correct.
- //
- // We want to do this after the kalman filter runs so we take into account
- // the encoder and gyro corrections.
- //
- // Start by computing the beginning and ending linear and angular
- // velocities.
- // To handle 0 velocity well, compute the offset required to be added to
- // both velocities to make the robot travel the correct distance.
-
- xytheta_state_.block<3, 1>(0, 0) = PredictState(
- xytheta_state_.block<3, 1>(0, 0), kf_.X_hat(), last_state_);
-
- // Use trapezoidal integration for the gyro heading since it's more
- // accurate.
- const double average_angular_velocity =
- ((kf_.X_hat(3) - kf_.X_hat(1)) + (last_state_(3) - last_state_(1))) /
- 2.0 / (dt_config_.robot_radius * 2.0);
-
- integrated_kf_heading_ +=
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count() *
- average_angular_velocity;
-
- // Copy over the gyro heading.
- xytheta_state_(2) = integrated_kf_heading_;
- // Copy over the velocities heading.
- xytheta_state_(3) = kf_.X_hat(1);
- xytheta_state_(4) = kf_.X_hat(3);
- // Copy over the voltage errors.
- xytheta_state_.block<2, 1>(5, 0) = kf_.X_hat().block<2, 1>(4, 0);
-
- // gyro_heading = (real_right - real_left) / width
- // wheel_heading = (wheel_right - wheel_left) / width
- // gyro_heading + offset = wheel_heading
- // gyro_goal + offset = wheel_goal
- // offset = wheel_heading - gyro_heading
-
- // gyro_goal + wheel_heading - gyro_heading = wheel_goal
+ localizer_->Update({last_left_voltage_, last_right_voltage_}, monotonic_now,
+ position->left_encoder, position->right_encoder,
+ last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -338,7 +252,10 @@
dt_closedloop_.Update(output != NULL && controller_type == 1);
dt_spline_.Update(output != NULL && controller_type == 2,
- xytheta_state_.block<5, 1>(0, 0));
+ (Eigen::Matrix<double, 5, 1>() << localizer_->x(),
+ localizer_->y(), localizer_->theta(),
+ localizer_->left_velocity(), localizer_->right_velocity())
+ .finished());
switch (controller_type) {
case 0:
@@ -363,7 +280,7 @@
Eigen::Matrix<double, 2, 1> angular =
dt_config_.LeftRightToAngular(kf_.X_hat());
- angular(0, 0) = integrated_kf_heading_;
+ angular(0, 0) = localizer_->theta();
Eigen::Matrix<double, 4, 1> gyro_left_right =
dt_config_.AngularLinearToLeftRight(linear, angular);
@@ -380,11 +297,11 @@
status->left_voltage_error = kf_.X_hat(4);
status->right_voltage_error = kf_.X_hat(5);
status->estimated_angular_velocity_error = kf_.X_hat(6);
- status->estimated_heading = integrated_kf_heading_;
+ status->estimated_heading = localizer_->theta();
- status->x = xytheta_state_(0);
- status->y = xytheta_state_(1);
- status->theta = xytheta_state_(2);
+ status->x = localizer_->x();
+ status->y = localizer_->y();
+ status->theta = localizer_->theta();
status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 05bf711..1aa3e13 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
@@ -24,6 +25,7 @@
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
int ControllerIndexFromGears();
@@ -38,16 +40,12 @@
void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
- // Computes the xy state change given the change in the lr state.
- ::Eigen::Matrix<double, 3, 1> PredictState(
- const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
- const ::Eigen::Matrix<double, 7, 1> &state,
- const ::Eigen::Matrix<double, 7, 1> &previous_state) const;
-
double last_gyro_rate_ = 0.0;
const DrivetrainConfig<double> dt_config_;
+ LocalizerInterface *localizer_;
+
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
@@ -65,8 +63,6 @@
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
- double integrated_kf_heading_ = 0;
-
bool left_high_requested_;
bool right_high_requested_;
@@ -74,12 +70,6 @@
double last_accel_ = 0.0;
- // Current xytheta state of the robot. This is essentially the kalman filter
- // integrated up in a direction.
- // [x, y, theta, vl, vr, left_error, right_error]
- ::Eigen::Matrix<double, 7, 1> xytheta_state_ =
- ::Eigen::Matrix<double, 7, 1>::Zero();
-
// Last kalman filter state.
::Eigen::Matrix<double, 7, 1> last_state_ =
::Eigen::Matrix<double, 7, 1>::Zero();
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 16bd78b..363204c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -32,6 +32,7 @@
::aos::ShmEventLoop event_loop_;
const DrivetrainConfig<double> dt_config_;
+ DeadReckonEkf localizer_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
@@ -43,7 +44,8 @@
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
dt_config_(GetTestDrivetrainConfig()),
- drivetrain_motor_(dt_config_, &event_loop_),
+ localizer_(dt_config_),
+ drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
drivetrain_motor_plant_(dt_config_) {
::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
new file mode 100644
index 0000000..4da8b57
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -0,0 +1,83 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// Defines an interface for classes that provide field-global localization.
+class LocalizerInterface {
+ public:
+ // Perform a single step of the filter, using the information that is
+ // available on every drivetrain iteration.
+ // The user should pass in the U that the real system experienced from the
+ // previous timestep until now; internally, any filters will first perform a
+ // prediction step to get the estimate at time now, and then will apply
+ // corrections based on the encoder/gyro/accelerometer values from time now.
+ // TODO(james): Consider letting implementations subscribe to the sensor
+ // values themselves, and then only passing in U. This requires more
+ // coordination on timing, however.
+ virtual void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate, double longitudinal_accelerometer) = 0;
+ // There are several subtly different norms floating around for state
+ // matrices. In order to avoid that mess, we jus tprovide direct accessors for
+ // the values that most people care about.
+ virtual double x() const = 0;
+ virtual double y() const = 0;
+ virtual double theta() const = 0;
+ virtual double left_velocity() const = 0;
+ virtual double right_velocity() const = 0;
+ virtual double left_voltage_error() const = 0;
+ virtual double right_voltage_error() const = 0;
+};
+
+// Uses the generic HybridEkf implementation to provide a basic field estimator.
+// This provides no method for using cameras or the such to get global
+// measurements and just assumes that you can dead-reckon perfectly.
+class DeadReckonEkf : public LocalizerInterface {
+ typedef HybridEkf<double> Ekf;
+ typedef typename Ekf::StateIdx StateIdx;
+ public:
+ DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
+ ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
+ ekf_.P());
+ }
+
+ void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate,
+ double /*longitudinal_accelerometer*/) override {
+ ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
+ }
+
+ double x() const override { return ekf_.X_hat(StateIdx::kX); }
+ double y() const override { return ekf_.X_hat(StateIdx::kY); }
+ double theta() const override { return ekf_.X_hat(StateIdx::kTheta); }
+ double left_velocity() const override {
+ return ekf_.X_hat(StateIdx::kLeftVelocity);
+ }
+ double right_velocity() const override {
+ return ekf_.X_hat(StateIdx::kRightVelocity);
+ }
+ double left_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kLeftVoltageError);
+ }
+ double right_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kRightVoltageError);
+ }
+
+ private:
+ Ekf ekf_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 292f291..5330414 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -15,7 +15,7 @@
DrivetrainMotorsSS::DrivetrainMotorsSS(
const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *kf,
- double *integrated_kf_heading)
+ LocalizerInterface *localizer)
: dt_config_(dt_config),
kf_(kf),
U_poly_(
@@ -34,7 +34,7 @@
.finished()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
- integrated_kf_heading_(integrated_kf_heading) {
+ localizer_(localizer) {
::aos::controls::HPolytope<0>::Init();
T_ << 1, 1, 1, -1;
T_inverse_ = T_.inverse();
@@ -168,8 +168,7 @@
Eigen::Matrix<double, 2, 1> wheel_heading =
dt_config_.LeftRightToAngular(kf_->X_hat());
- const double gyro_to_wheel_offset =
- wheel_heading(0, 0) - *integrated_kf_heading_;
+ const double gyro_to_wheel_offset = wheel_heading(0, 0) - localizer_->theta();
if (enable_control_loop) {
// Update profiles.
@@ -240,7 +239,7 @@
Eigen::Matrix<double, 2, 1> wheel_linear =
dt_config_.LeftRightToLinear(kf_->X_hat());
Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
- next_angular(0, 0) = *integrated_kf_heading_;
+ next_angular(0, 0) = localizer_->theta();
unprofiled_goal_.block<4, 1>(0, 0) =
dt_config_.AngularLinearToLeftRight(wheel_linear, next_angular);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index ee8d145..762cbc2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
namespace frc971 {
namespace control_loops {
@@ -20,7 +21,7 @@
public:
DrivetrainMotorsSS(const DrivetrainConfig<double> &dt_config,
StateFeedbackLoop<7, 2, 4> *kf,
- double *integrated_kf_heading);
+ LocalizerInterface *localizer);
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
@@ -65,7 +66,7 @@
bool use_profile_ = false;
- double *integrated_kf_heading_;
+ LocalizerInterface *localizer_;
};
} // namespace drivetrain
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
index ef348a4..6e5b845 100644
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 19c2659..21dfcd2 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,10 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index a76b5a3..f13a8ab 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -9,9 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index e74ddea..12d8642 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
index ffe479a..808f215 100644
--- a/y2017/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index f0c7f0e..038497a 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
index e481f6c..c768dc9 100644
--- a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -9,9 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index 4e23987..50fd030 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;