Added a hybrid KF for the shooter.
Change-Id: If3ba2e6978773aef2e63c9bfeb0cc6e2dec483d5
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 6186f7f..0560ec4 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -116,7 +116,8 @@
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
loop_->Update(disabled);
- stuck_indexer_detector_->UpdateObserver(loop_->U());
+ stuck_indexer_detector_->UpdateObserver(loop_->U(),
+ ::aos::controls::kLoopFrequency);
}
void IndexerController::SetStatus(IndexerStatus *status) {
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index c4d1630..8991658 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,7 +23,8 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<3, 1, 1>(
+ : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 41e24c0..7dc44bf 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -49,7 +49,9 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<
+ 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 5;