commit bd947022852769cbf4343e69dc0a7d2981e5beb3
author Austin Schuh <austin@peloton-tech.com> Sun Mar 01 00:10:01 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Sun Mar 01 03:05:15 2015 -0800
tree d6426b311d8ffc452b1e46496239b18b8228ac32
parent af88a8f83bbb67ad9c0524dffd90298e1889abfd

Re-tuned claw controller and filter to handle noise better.

Change-Id: I2b417198a3149e6bca36e1c0f052c5d940c6fd6d

frc971/control_loops/claw/claw_motor_plant.cc

diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 4b47c02..213a92c 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -25,9 +25,9 @@
 
 StateFeedbackController<2, 1, 1> MakeClawController() {
   Eigen::Matrix<double, 2, 1> L;
-  L << 1.33065249533, 84.5098852645;
+  L << 0.165471565307, 0.0290418032388;
   Eigen::Matrix<double, 1, 2> K;
-  K << 115.895112435, 8.54319939792;
+  K << 74.4310031124, 4.72251126222;
   Eigen::Matrix<double, 2, 2> A_inv;
   A_inv << 1.0, -0.00518405612386, 0.0, 1.07451492907;
   return StateFeedbackController<2, 1, 1>(L, K, A_inv, MakeClawPlantCoefficients());

frc971/control_loops/python/claw.py

diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 3395b1d..fd123a3 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -66,8 +66,10 @@
 
     controlability = controls.ctrb(self.A, self.B);
 
-    q_pos = 0.09
-    q_vel = 1.2
+    print "Free speed is", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G
+
+    q_pos = 0.15
+    q_vel = 2.5
     self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
                            [0.0, (1.0 / (q_vel ** 2.0))]])
 
@@ -82,6 +84,23 @@
     self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
                              self.rpl - 1j * self.ipl])
 
+    print 'L is', self.L
+
+    q_pos = 0.018
+    q_vel = 0.05
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+                           [0.0, (q_vel ** 2.0)]])
+
+    r_volts = 0.1
+    self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+    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]])
@@ -164,11 +183,11 @@
 
 
 def main(argv):
-  loaded_mass = 5
+  loaded_mass = 0
   #loaded_mass = 0
-  claw = Claw(mass=5 + loaded_mass)
-  claw_controller = Claw(mass=5 + 5)
-  observer_claw = Claw(mass=5 + 5)
+  claw = Claw(mass=4 + loaded_mass)
+  claw_controller = Claw(mass=5 + 0)
+  observer_claw = Claw(mass=5 + 0)
   #observer_claw = None
 
   # Test moving the claw with constant separation.