Made Velocity Shooter loop such that it should run.
Currently, all it builds and passes all the tests. I need to go in and check to see if there are any more tests I should add.
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index 07ce397..165d99f 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -13,6 +13,7 @@
loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
last_velocity_goal_(0) {
loop_->Reset();
+ U_add = loop_->B().inverse() * (loop_->A().Identity() - loop_->A());
}
/*static*/ const double ShooterMotor::dt = 0.01;
@@ -44,7 +45,7 @@
loop_->Y << average_velocity_;
loop_->R << velocity_goal;
- loop_->Update(position, output == NULL);
+ loop_->Update(position, output == NULL, U_add * loop_->R);
// Kill power at low velocity goals.
if (velocity_goal < 1.0) {