commit 86093addac39401db887919027d5bab223383e7e
author Austin Schuh <austin.linux@gmail.com> Sat Feb 06 14:29:34 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Feb 08 00:23:09 2016 -0800
tree cb63b7ae4e5506db4a179c9e275e421f42d9643d
parent 289ee4f06f4bfee66b2eca86e235c7edd16295f4

Added the feed forwards constant to StateFeedbackLoop.

Change-Id: Ie222de1f302b0edcf5c8b1fdd7f2958e0de608c3

frc971/control_loops/python/control_loop.py

diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 586a375..951a114 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -38,7 +38,7 @@
 
     self._namespace_end = '\n'.join(
         ['}  // namespace %s' % name for name in reversed(self._namespaces)])
-    
+
     self._constant_list = []
 
   def AddConstant(self, constant):
@@ -330,10 +330,14 @@
 
     ans.append(self._DumpMatrix('L', self.L))
     ans.append(self._DumpMatrix('K', self.K))
+    if not hasattr(self, 'Kff'):
+      self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
+
+    ans.append(self._DumpMatrix('Kff', self.Kff))
     ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
 
     ans.append('  return StateFeedbackController<%d, %d, %d>'
-               '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
+               '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
                    num_states, num_inputs, num_outputs, self._name))
     ans.append('}\n')
     return ''.join(ans)