Fixed bug in c2d by using scipy's version.
Change-Id: I5b399e331d8ee589201a0805057e0db6030d606d
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 6dbbae5..a925a89 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -75,7 +75,7 @@
Eigen::Matrix<double, 4, 2> L;
L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489275, 0.77037863276, 2.23580489275, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
Eigen::Matrix<double, 4, 4> A_inv;
A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
@@ -85,7 +85,7 @@
Eigen::Matrix<double, 4, 2> L;
L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489275, 0.77037863276, 2.23580489275, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
Eigen::Matrix<double, 4, 4> A_inv;
A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
@@ -95,7 +95,7 @@
Eigen::Matrix<double, 4, 2> L;
L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489275, 0.77037863276, 2.23580489275, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
Eigen::Matrix<double, 4, 4> A_inv;
A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
@@ -105,7 +105,7 @@
Eigen::Matrix<double, 4, 2> L;
L << 1.25738796225, 0.00756271754847, 74.8971101634, 1.58403340092, 0.00756271754847, 1.25738796225, 1.58403340092, 74.8971101634;
Eigen::Matrix<double, 2, 4> K;
- K << 154.070413936, 12.2263299292, 2.23580489275, 0.77037863276, 2.23580489275, 0.77037863276, 154.070413936, 12.2263299292;
+ K << 154.070413936, 12.2263299292, 2.23580489274, 0.77037863276, 2.23580489276, 0.77037863276, 154.070413936, 12.2263299292;
Eigen::Matrix<double, 4, 4> A_inv;
A_inv << 1.0, -0.00511059808244, 0.0, 2.03320493463e-05, 0.0, 1.04457382236, 0.0, -0.00825142689119, 0.0, 2.03320493463e-05, 1.0, -0.00511059808244, 0.0, -0.00825142689119, 0.0, 1.04457382236;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
index b463dde..ec2b669 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
Eigen::Matrix<double, 1, 1> A;
- A << 0.614537580221;
+ A << 0.783924473544;
Eigen::Matrix<double, 1, 1> B;
- B << 15.9657598852;
+ B << 8.94979586973;
Eigen::Matrix<double, 1, 1> C;
C << 1;
Eigen::Matrix<double, 1, 1> D;
@@ -25,11 +25,11 @@
StateFeedbackController<1, 1, 1> MakeCIMController() {
Eigen::Matrix<double, 1, 1> L;
- L << 0.604537580221;
+ L << 0.773924473544;
Eigen::Matrix<double, 1, 1> K;
- K << 0.0378646293422;
+ K << 0.086473980503;
Eigen::Matrix<double, 1, 1> A_inv;
- A_inv << 1.62723978514;
+ A_inv << 1.2756330919;
return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 1c908c6..3d08774 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.735841675858, 0.0410810558113, 0.0410810558113, 0.735841675858;
+ A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0517213538779, -0.00804353916233, -0.00804353916233, 0.0517213538779;
+ B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.735048848179, 0.0131811893199, 0.045929121897, 0.915703853642;
+ A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0518765869984, -0.00481755802263, -0.00899277497558, 0.0308091755839;
+ B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.915703853642, 0.045929121897, 0.0131811893199, 0.735048848179;
+ A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0308091755839, -0.00899277497558, -0.00481755802263, 0.0518765869984;
+ B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.915439806567, 0.0146814193986, 0.0146814193986, 0.915439806567;
+ A << 0.916648904963, 0.0144809094856, 0.0144809094856, 0.916648904963;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0309056814511, -0.00536587314624, -0.00536587314624, 0.0309056814511;
+ B << 0.0306942437366, -0.00533263018418, -0.00533263018418, 0.0306942437366;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.715841675858, 0.0410810558113, 0.0410810558113, 0.715841675858;
+ L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
Eigen::Matrix<double, 2, 2> K;
- K << 2.81809403994, 1.23253744933, 1.23253744933, 2.81809403994;
+ K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.36323698074, -0.0761076958907, -0.0761076958907, 1.36323698074;
+ A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.715885457343, 0.0459077351335, 0.0459077351335, 0.894867244478;
+ L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
Eigen::Matrix<double, 2, 2> K;
- K << 2.81810038978, 1.23928174475, 2.31332592354, 10.6088017388;
+ K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.36167854796, -0.0196008159867, -0.0682979543713, 1.09303924439;
+ A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.902328849033, 0.014581304798, 0.014581304798, 0.708423852788;
+ L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
Eigen::Matrix<double, 2, 2> K;
- K << 10.6088017388, 2.31332592354, 1.23928174475, 2.81810038978;
+ K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.09303924439, -0.0682979543713, -0.0196008159867, 1.36167854796;
+ A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.895439806567, 0.0146814193986, 0.0146814193986, 0.895439806567;
+ L << 0.896648904963, 0.0144809094856, 0.0144809094856, 0.896648904963;
Eigen::Matrix<double, 2, 2> K;
- K << 10.6088022944, 2.31694961514, 2.31694961514, 10.6088022944;
+ K << 10.7218164758, 2.3345221426, 2.3345221426, 10.7218164758;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.0926521463, -0.0175234726538, -0.0175234726538, 1.0926521463;
+ A_inv << 1.0912025564, -0.0172384490553, -0.0172384490553, 1.0912025564;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/fridge/elevator_motor_plant.cc b/frc971/control_loops/fridge/elevator_motor_plant.cc
index 0ae9828..e75e668 100644
--- a/frc971/control_loops/fridge/elevator_motor_plant.cc
+++ b/frc971/control_loops/fridge/elevator_motor_plant.cc
@@ -27,7 +27,7 @@
Eigen::Matrix<double, 4, 2> L;
L << 0.677106393027, 0.677106393027, 35.5375738607, 35.5375738607, 0.674426730777, -0.674426730777, 34.7138874344, -34.7138874344;
Eigen::Matrix<double, 2, 4> K;
- K << 321.310606763, 11.7674534233, 601.047935717, 12.6977148843, 321.310606763, 11.7674534233, -601.047935716, -12.6977148843;
+ K << 321.310606763, 11.7674534233, 601.047935717, 12.6977148843, 321.310606764, 11.7674534233, -601.047935716, -12.6977148843;
Eigen::Matrix<double, 4, 4> A_inv;
A_inv << 1.0, -0.00577646258091, 0.0, 0.0, 0.0, 1.32588576923, 0.0, 0.0, 0.0, 0.0, 0.996613922337, -0.00577054766522, 0.0, 0.0, 1.42044250221, 1.32216599481;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeElevatorPlantCoefficients());
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 57268a9..212df42 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -11,6 +11,7 @@
import numpy
import slycot
+import scipy.signal.cont2discrete
class Error (Exception):
"""Base class for all control loop exceptions."""
@@ -85,26 +86,10 @@
def c2d(A, B, dt):
"""Converts from continuous time state space representation to discrete time.
- Evaluates e^(A dt) - I for the discrete time version of A, and
- integral(e^(A t) * B, 0, dt).
Returns (A, B). C and D are unchanged."""
- e, P = numpy.linalg.eig(A)
- diag = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
- diage = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
- for eig, count in zip(e, range(0, A.shape[0])):
- diag[count, count] = numpy.exp(eig * dt)
- if abs(eig) < 1.0e-16:
- diage[count, count] = dt
- else:
- diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
- ans_a = P * diag * numpy.linalg.inv(P)
- ans_b = P * diage * numpy.linalg.inv(P) * B
- if numpy.abs(ans_a.imag).sum() / numpy.abs(ans_a).sum() < 1e-6:
- ans_a = ans_a.real
- if numpy.abs(ans_b.imag).sum() / numpy.abs(ans_b).sum() < 1e-6:
- ans_b = ans_b.real
- return (ans_a, ans_b)
+ ans_a, ans_b, _, _, _ = scipy.signal.cont2discrete((A, B, None, None), dt)
+ return numpy.matrix(ans_a), numpy.matrix(ans_b)
def ctrb(A, B):
"""Returns the controlability matrix.