Made KF data available to polydrivetrain.
Change-Id: I067d0d05b9fc32d6ad50fbd35a6e9ad07b63f20d
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index c538894..5717f36 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -33,7 +33,8 @@
::y2014::control_loops::DrivetrainQueue *my_drivetrain)
: aos::controls::ControlLoop<::y2014::control_loops::DrivetrainQueue>(
my_drivetrain),
- kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()) {
+ kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()),
+ dt_openloop_(&kf_) {
::aos::controls::HPolytope<0>::Init();
}
@@ -49,6 +50,16 @@
}
no_position_.Print();
+ kf_.set_controller_index(dt_openloop_.controller_index());
+
+ {
+ Eigen::Matrix<double, 3, 1> Y;
+ Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
+ kf_.Correct(Y);
+ integrated_kf_heading_ +=
+ kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
+ }
+
bool control_loop_driving = false;
if (goal) {
double wheel = goal->steering;
@@ -125,14 +136,6 @@
left_voltage *= scalar;
right_voltage *= scalar;
- kf_.set_controller_index(dt_openloop_.controller_index());
-
- Eigen::Matrix<double, 3, 1> Y;
- Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
- kf_.Correct(Y);
- integrated_kf_heading_ +=
- kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
-
// To validate, look at the following:
// Observed - dx/dt velocity for left, right.