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) {
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index efc1f1b..ac7aadd 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -40,6 +40,8 @@
// For making sure it keeps spinning if we're shooting.
double last_velocity_goal_;
+ Eigen::Matrix<double, 1, 1> U_add;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index e8113a4..cb707b2 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -1,10 +1,10 @@
-#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
@@ -44,4 +44,4 @@
}
} // namespace control_loops
-} // namespace frc971
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index 18ca817..1ab9702 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#ifndef BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
@@ -15,6 +15,6 @@
StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
} // namespace control_loops
-} // namespace frc971
+} // namespace bot3
-#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#endif // BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_