Re-tuned claw controller and filter to handle noise better.
Change-Id: I2b417198a3149e6bca36e1c0f052c5d940c6fd6d
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.