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;
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index 19fdb5e..93f3884 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -128,7 +128,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.8, 0.8])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high