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_
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 754ba62..9a8cac8 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -17,7 +17,7 @@
if namespaces:
self._namespaces = namespaces
else:
- self._namespaces = ['frc971', 'control_loops']
+ self._namespaces = ['bot3', 'control_loops']
self._namespace_start = '\n'.join(
['namespace %s {' % name for name in self._namespaces])
@@ -26,7 +26,7 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
def _HeaderGuard(self, header_file):
- return ('FRC971_CONTROL_LOOPS_' +
+ return ('BOT3_CONTROL_LOOPS_' +
header_file.upper().replace('.', '_').replace('/', '_') +
'_')
@@ -89,7 +89,7 @@
def WriteCC(self, header_file_name, cc_file):
"""Writes the cc file to the file named cc_file."""
with open(cc_file, 'w') as fd:
- fd.write('#include \"frc971/control_loops/%s\"\n' % header_file_name)
+ fd.write('#include \"bot3/control_loops/%s\"\n' % header_file_name)
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index ff5e9d8..cc11dfc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -294,11 +294,15 @@
// update_observer is whether or not to use the values in Y.
// stop_motors is whether or not to output all 0s.
- void Update(bool update_observer, bool stop_motors) {
+ // U_add is for the bot3 shooter velocity control, where a constant
+ // must be added to get a good U.
+ void Update(bool update_observer, bool stop_motors,
+ Eigen::Matrix<double, number_of_inputs, 1> U_add =
+ Eigen::Matrix<double, number_of_inputs, 1>::Zero()) {
if (stop_motors) {
U.setZero();
} else {
- U = U_uncapped = K() * (R - X_hat);
+ U = U_uncapped = K() * (R - X_hat) + U_add;
CapU();
}