commit c5fceb86fa7afa77119e57ba4fc824574f3a2328
author Austin Schuh <austin.linux@gmail.com> Sat Feb 25 16:24:12 2017 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 04 17:55:14 2017 -0800
tree 2f292592e5e88b5ca74f84ddebfe7cf7ec6ef1e1
parent 71b5f13de3846bf9363ba238e2e7716242481003

Put a plant inside the loop and moved A,B,etc there.

Change-Id: I9cb3a1a16bd0ccda0c9287514577b3c3861bc42f

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 fba883c..e8900af 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -158,7 +158,8 @@
         fd.write('  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
                  (index, self._ControllerType(), self._ControllerType(),
                   loop.ControllerFunction()))
-      fd.write('  return %s(&controllers);\n' % self._LoopType())
+      fd.write('  return %s(Make%sPlant(), &controllers);\n' %
+          (self._LoopType(), self._gain_schedule_name))
       fd.write('}\n\n')
 
       fd.write(self._namespace_end)
@@ -276,6 +277,7 @@
         num_states, num_inputs, num_outputs, self._name)]
 
     ans.append(self._DumpMatrix('A', self.A))
+    ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
     ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
     ans.append(self._DumpMatrix('B', self.B))
     ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
@@ -285,7 +287,7 @@
     ans.append(self._DumpMatrix('U_min', self.U_min))
 
     ans.append('  return StateFeedbackPlantCoefficients<%d, %d, %d>'
-               '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
+               '(A, A_inv, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
                    num_states, num_inputs, num_outputs))
     ans.append('}\n')
     return ''.join(ans)
@@ -328,10 +330,9 @@
       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 StateFeedbackControllerConstants<%d, %d, %d>'
-               '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
-                   num_states, num_inputs, num_outputs, self._name))
+               '(L, K, Kff);\n' % (
+                   num_states, num_inputs, num_outputs))
     ans.append('}\n')
     return ''.join(ans)