commit a88c407429851c2abef91ff5bf3c5ba15691e0af
author Austin Schuh <austin.linux@gmail.com> Sat Feb 06 14:31:03 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Feb 10 21:30:23 2016 -0800
tree 375743bef7ab8dc974d3db12e7d4a51ee01716af
parent 7485dbb61ff00ee607f65228117f9de8d4aa8776

Tuned superstructure loop and added feed forwards.

Change-Id: Ia2e3a1746529a4c27395f2e9b6e875c5cddb7616

y2016/control_loops/python/shoulder.py

diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 554228e..9b70dbd 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -1,12 +1,10 @@
 #!/usr/bin/python
 
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import controls
-from frc971.control_loops.python import polytope
-from y2016.control_loops.python import polydrivetrain
 import numpy
 import sys
-import matplotlib
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
 from matplotlib import pylab
 import gflags
 import glog
@@ -77,8 +75,10 @@
     self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
     self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
 
-    print 'K', self.K
-    print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+    self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+
+    glog.debug('Poles are %s for %s',
+               repr(numpy.linalg.eig(self.A - self.B * self.K)[0]), self._name)
 
     q_pos = 0.05
     q_vel = 2.65
@@ -91,9 +91,7 @@
     self.KalmanGain, self.Q_steady = controls.kalman(
         A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
 
-    print 'Kal', self.KalmanGain
     self.L = self.A * self.KalmanGain
-    print 'KalL is', self.L
 
     # The box formed by U_min and U_max must encompass all possible values,
     # or else Austin's code gets angry.
@@ -140,6 +138,9 @@
     self.K = numpy.matrix(numpy.zeros((1, 3)))
     self.K[0, 0:2] = self.K_unaugmented
     self.K[0, 2] = 1
+    self.Kff_unaugmented = self.Kff
+    self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+    self.Kff[0, 0:2] = self.Kff_unaugmented
 
     self.InitializeState()