commit a25a041e19dacdd679a64a4333c61a26a0fbf494
author Austin Schuh <austin@peloton-tech.com> Sun Mar 09 00:50:04 2014 -0800
committer Austin Schuh <austin@peloton-tech.com> Sun Mar 09 00:51:16 2014 -0800
tree c6c0276881fae65312887190f9e2d67886b5fd8d
parent 05a816617527b40b546df10e15a9d48e0bc052d5

Tuned drivetrain control loops and polydrive.

frc971/control_loops/python/drivetrain.py

diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 8b1d9ad..3d6e441 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python
 
 import control_loop
+import controls
 import numpy
 import sys
 from matplotlib import pylab
@@ -90,8 +91,6 @@
     # Control loop time step
     self.dt = 0.01
     
-    print "THE NUMBER I WANT" + str((self.free_speed / 60.0 * 2.0 * numpy.pi) * self.G_high * self.r)
-
     # These describe the way that a given side of a robot will be influenced
     # by the other side. Units of 1 / kg.
     self.msp = 1.0 / self.m + self.rb * self.rb / self.J
@@ -127,7 +126,22 @@
     # Poles from last year.
     self.hp = 0.65
     self.lp = 0.83
-    self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+    self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
+    print self.K
+    q_pos = 0.07
+    q_vel = 1.0
+    self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+                           [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+                           [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+                           [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+    self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+                           [0.0, (1.0 / (12.0 ** 2.0))]])
+    self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+    print self.A
+    print self.B
+    print self.K
+    print numpy.linalg.eig(self.A - self.B * self.K)[0]
 
     self.hlp = 0.07
     self.llp = 0.09
@@ -203,6 +217,7 @@
   #pylab.show()
 
   # Write the generated constants out to a file.
+  print "Output one"
   drivetrain = Drivetrain()
 
   if len(argv) != 5:

frc971/control_loops/python/polydrivetrain.py

diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 280db16..f2dfdbe 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -396,10 +396,10 @@
     print "Expected .h file name and .cc file name"
   else:
     dog_loop_writer = control_loop.ControlLoopWriter(
-        "VDogDrivetrain", [vdrivetrain.drivetrain_low_low,
-                           vdrivetrain.drivetrain_low_high,
-                           vdrivetrain.drivetrain_high_low,
-                           vdrivetrain.drivetrain_high_high])
+        "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+                       vdrivetrain.drivetrain_low_high,
+                       vdrivetrain.drivetrain_high_low,
+                       vdrivetrain.drivetrain_high_high])
 
     if argv[1][-3:] == '.cc':
       dog_loop_writer.Write(argv[2], argv[1])