Made test mode and the claw work on the robot.
Change-Id: I29a5677f616f70f9e8a8595f05cacbd3a85dcba6
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 1bb4c10..4b47c02 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.53065249533, 111.171516288;
+ L << 1.33065249533, 84.5098852645;
Eigen::Matrix<double, 1, 2> K;
- K << 284.338418915, 17.4107932965;
+ K << 115.895112435, 8.54319939792;
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());
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 1a25f95..3395b1d 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -64,24 +64,21 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- print self.A
-
controlability = controls.ctrb(self.A, self.B);
- print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
- q_pos = 0.03
- q_vel = 0.5
+ q_pos = 0.09
+ q_vel = 1.2
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- print self.K
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ print 'K', self.K
+ print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
- self.rpl = 0.20
- self.ipl = 0.05
+ self.rpl = 0.30
+ self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])