Enabled closed loop polydrive.
Change-Id: I6587801d3b5b06006ba826617560aef0ee96069b
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.cc b/y2014/control_loops/drivetrain/polydrivetrain.cc
index 02d0658..014a1ae 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.cc
+++ b/y2014/control_loops/drivetrain/polydrivetrain.cc
@@ -233,6 +233,9 @@
}
void PolyDrivetrain::Update() {
+ loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
+ loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
+
const auto &values = constants::GetValues();
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
@@ -324,11 +327,6 @@
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
-
- // TODO(austin): Model this better.
- // TODO(austin): Feed back?
- loop_->mutable_X_hat() =
- loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
} else {
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;