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/wrist.py

diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 79a115e..2ad1d07 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -2,8 +2,6 @@
 
 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
@@ -77,8 +75,8 @@
     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]
+    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,15 +89,15 @@
     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.
     self.U_max = numpy.matrix([[12.0]])
     self.U_min = numpy.matrix([[-12.0]])
 
+    self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+
     self.InitializeState()
 
 class IntegralWrist(Wrist):
@@ -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()
 
@@ -260,13 +261,13 @@
   else:
     namespaces = ['y2016', 'control_loops', 'superstructure']
     wrist = Wrist("Wrist")
-    loop_writer = control_loop.ControlLoopWriter('Wrist', [wrist],
-                                                 namespaces=namespaces)
+    loop_writer = control_loop.ControlLoopWriter(
+        'Wrist', [wrist], namespaces=namespaces)
     loop_writer.Write(argv[1], argv[2])
 
-    integral_wrist = IntegralWrist("IntegralWrist")
-    integral_loop_writer = control_loop.ControlLoopWriter("IntegralWrist", [integral_wrist],
-                                                          namespaces=namespaces)
+    integral_wrist = IntegralWrist('IntegralWrist')
+    integral_loop_writer = control_loop.ControlLoopWriter(
+        'IntegralWrist', [integral_wrist], namespaces=namespaces)
     integral_loop_writer.Write(argv[3], argv[4])
 
 if __name__ == '__main__':