Add closed loop option to drivetrain config.
Change-Id: Icff6a814f6a75843bbb0d441d12cc33b5387b843
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 9dad2a8..6e08b84 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -260,8 +260,10 @@
}
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);
+ if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
+ loop_->mutable_X_hat()(0, 0) = kf_.X_hat()(1, 0);
+ loop_->mutable_X_hat()(1, 0) = kf_.X_hat()(3, 0);
+ }
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
@@ -318,6 +320,11 @@
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
+
+ if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
+ loop_->mutable_X_hat() =
+ loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+ }
} else {
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /