Updated drivetrain coefficients.
Change-Id: I509fdc459979506f6a7eee4ecc3dd2126c6f8652
diff --git a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index e24fa71..85cb721 100644
--- a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00494348612824, 0.0, 1.24048455404e-05, 0.0, 0.977484092703, 0.0, 0.00492433284048, 0.0, 1.24048455404e-05, 1.0, 0.00494348612824, 0.0, 0.00492433284048, 0.0, 0.977484092703;
+ A << 1.0, 0.0048650405221, 0.0, 4.30452834469e-05, 0.0, 0.946557122299, 0.0, 0.0169038781857, 0.0, 4.30452834469e-05, 1.0, 0.0048650405221, 0.0, 0.0169038781857, 0.0, 0.946557122299;
Eigen::Matrix<double, 4, 2> B;
- B << 2.09180756177e-05, -4.59153636043e-06, 0.00833405032795, -0.00182269526971, -4.59153636043e-06, 2.09180756177e-05, -0.00182269526971, 0.00833405032795;
+ B << 3.44936607298e-05, -1.10017423477e-05, 0.0136592147549, -0.00432038303814, -1.10017423477e-05, 3.44936607298e-05, -0.00432038303814, 0.0136592147549;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,41 +73,41 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.2774840927, 0.00492433284048, 79.1440456622, 1.05150722636, 0.00492433284048, 1.2774840927, 1.05150722636, 79.1440456622;
+ L << 1.2465571223, 0.0169038781857, 72.6644245424, 3.50262182277, 0.0169038781857, 1.2465571223, 3.50262182277, 72.6644245424;
Eigen::Matrix<double, 2, 4> K;
- K << 160.19677247, 15.5358468938, 1.60401194139, 1.33861213093, 1.60401194142, 1.33861213094, 160.19677247, 15.5358468938;
+ K << 156.707528421, 12.1845100817, 3.29243225373, 1.51015663937, 3.29243225372, 1.51015663937, 156.707528421, 12.1845100817;
Eigen::Matrix<double, 4, 4> A_inv;
- A_inv << 1.0, -0.00505742153256, 0.0, 1.27875036472e-05, 0.0, 1.02306051542, 0.0, -0.00515393603998, 0.0, 1.27875036472e-05, 1.0, -0.00505742153256, 0.0, -0.00515393603998, 0.0, 1.02306051542;
+ A_inv << 1.0, -0.00514054935697, 0.0, 4.63257162805e-05, 0.0, 1.0567973091, 0.0, -0.01887257785, 0.0, 4.63257162805e-05, 1.0, -0.00514054935697, 0.0, -0.01887257785, 0.0, 1.0567973091;
return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
}
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 95913ff..ed8e990 100644
--- a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/bot3/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.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
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.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
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.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
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.955499400541, 0.00962691403749, 0.00962691403749, 0.955499400541;
+ A << 0.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0164714763931, -0.00356331126397, -0.00356331126397, 0.0164714763931;
+ B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
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.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.935499400541, 0.00962691403749, 0.00962691403749, 0.935499400541;
+ L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
Eigen::Matrix<double, 2, 2> K;
- K << 22.7750288573, 5.51143253556, 5.51143253556, 22.7750288573;
+ K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.04667938127, -0.010545576923, -0.010545576923, 1.04667938127;
+ A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
index 43b597f..c3144be 100644
--- a/bot3/control_loops/python/drivetrain.py
+++ b/bot3/control_loops/python/drivetrain.py
@@ -63,13 +63,13 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 10
+ self.J = 8
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
self.rb = 0.9603 / 2.0
# Radius of the wheels, in meters.
- self.r = .0515938
+ self.r = 0.0508
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
@@ -78,7 +78,7 @@
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_const = 28.0 / 50.0 * 20.0 / 64.0
+ self.G_const = 18.0 / 44.0 * 18.0 / 60.0
self.G_low = self.G_const
self.G_high = self.G_const