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/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();
}