Automatically generate the drivetrain constants.
Change-Id: Ib96f7d55734d44d8ac9d918ec90edf3b1fb46cd3
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
new file mode 100644
index 0000000..df7d48b
--- /dev/null
+++ b/frc971/control_loops/python/BUILD
@@ -0,0 +1,9 @@
+package(default_visibility = ['//visibility:public'])
+
+py_library(
+ name = 'controls',
+ srcs = [
+ 'control_loop.py',
+ 'controls.py',
+ ],
+)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 881880f..d3968a6 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -1,5 +1,6 @@
import controls
import numpy
+import os
class Constant(object):
def __init__ (self, name, formatt, value):
@@ -52,14 +53,14 @@
return self._namespaces[0]
def _HeaderGuard(self, header_file):
- return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
- header_file.upper().replace('.', '_').replace('/', '_') +
- '_')
+ return ('_'.join([namespace.upper() for namespace in self._namespaces]) +
+ os.path.basename(header_file).upper()
+ .replace('.', '_').replace('/', '_') + '_')
def Write(self, header_file, cc_file):
"""Writes the loops to the specified files."""
self.WriteHeader(header_file)
- self.WriteCC(header_file, cc_file)
+ self.WriteCC(os.path.basename(header_file), cc_file)
def _GenericType(self, typename):
"""Returns a loop template using typename for the type."""
@@ -121,8 +122,8 @@
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 \"%s/control_loops/%s\"\n' %
- (self._TopDirectory(), header_file_name))
+ fd.write('#include \"%s/%s\"\n' %
+ (os.path.join(*self._namespaces), header_file_name))
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index b66bd56..6494ce1 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -131,23 +131,13 @@
Returns:
KalmanGain, Covariance.
"""
- P = Q
+ I = numpy.matrix(numpy.eye(Q.shape[0]))
+ Z = numpy.matrix(numpy.zeros(Q.shape[0]))
- I = numpy.matrix(numpy.eye(P.shape[0]))
- At = A.T
- Ct = C.T
- i = 0
-
- while True:
- last_P = P
- P_prior = A * P * At + Q
- S = C * P_prior * Ct + R
- K = P_prior * Ct * numpy.linalg.inv(S)
- P = (I - K * C) * P_prior
-
- diff = P - last_P
- i += 1
- if numpy.linalg.norm(diff) / numpy.linalg.norm(P) < 1e-9:
- break
+ # Compute the steady state covariance matrix.
+ P_prior = scipy.linalg.solve_discrete_are(A.T, C.T, Q, R)
+ S = C * P_prior * C.T + R
+ K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
+ P = (I - K * C) * P_prior
return K, P