Fix some of the constants in drivetrain.
This includes the shifter hall effect constants, for which
I guessed whether we were using the drivebase from the old comp
or practice bot, and guessed wrong. They also include the
drivetrain python code, for which some of the constants weren't
correct.
(This was formerly a part of I3d012fb8a9652c0b85ed27f5d23fe7d63bb977ce.)
Change-Id: I5869b7e57a039131018150c690287f81670a3483
diff --git a/bot3/control_loops/drivetrain/drivetrain_constants.h b/bot3/control_loops/drivetrain/drivetrain_constants.h
index 4716928..8563cbef 100644
--- a/bot3/control_loops/drivetrain/drivetrain_constants.h
+++ b/bot3/control_loops/drivetrain/drivetrain_constants.h
@@ -6,17 +6,16 @@
namespace bot3 {
namespace control_loops {
-// TODO(danielp): Figure out the real values for these constants.
constexpr constants::ShifterHallEffect kBot3LeftDriveShifter =
- {170, 475, 1.2, 1.0};
+ {426, 171, 0.6, 0.47};
constexpr constants::ShifterHallEffect kBot3RightDriveShifter =
- {177, 486, 1.2, 1.0};
+ {424, 172, 0.62, 0.55};
constexpr double kBot3TurnWidth = 0.5;
constexpr double kBot3DrivetrainDoneDistance = 0.02;
-constexpr double kBot3HighGearRatio = 18.0 / 60.0 * 18.0 / 50.0;
-constexpr double kBot3LowGearRatio = 28.0 / 50.0 * 18.0 / 50.0;
+constexpr double kBot3LowGearRatio = 14.0 / 60.0 * 17.0 / 50.0;
+constexpr double kBot3HighGearRatio = 30.0 / 44.0 * 17.0 / 50.0;
} // control_loops
} // bot3
diff --git a/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 80d69bf..da59426 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.00751678417107, 0.0, 0.000244815974033, 0.0, 0.548849954683, 0.0, 0.0396601987617, 0.0, 0.000244815974033, 1.0, 0.00751678417107, 0.0, 0.0396601987617, 0.0, 0.548849954683;
+ A << 1.0, 0.00718627304278, 0.0, 0.000575327102319, 0.0, 0.498791664199, 0.0, 0.0897184892451, 0.0, 0.000575327102319, 1.0, 0.00718627304278, 0.0, 0.0897184892451, 0.0, 0.498791664199;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000357151105465, -3.52109126974e-05, 0.0648871256127, -0.0057041694345, -3.52109126974e-05, 0.000357151105465, -0.0057041694345, 0.0648871256127;
+ B << 0.000404687213064, -8.27470202964e-05, 0.0720868114296, -0.0129038552514, -8.27470202964e-05, 0.000404687213064, -0.0129038552514, 0.0720868114296;
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.00751193529517, 0.0, 3.41705796399e-05, 0.0, 0.547617329816, 0.0, 0.00612721792429, 0.0, 0.000291766839985, 1.0, 0.00965616939349, 0.0, 0.0523174915529, 0.0, 0.932105674456;
+ A << 1.0, 0.00715806931946, 0.0, 8.27195198739e-05, 0.0, 0.491841728751, 0.0, 0.0145551608494, 0.0, 0.000706303878161, 1.0, 0.00959963582133, 0.0, 0.124279814134, 0.0, 0.921454874639;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000357848500096, -1.43609003799e-05, 0.0650644091693, -0.00257509141326, -4.19636699414e-05, 0.000144501999664, -0.00752461776602, 0.0285340095421;
+ B << 0.000408743642973, -3.47646073582e-05, 0.0730863931411, -0.00611711060145, -0.000101584891631, 0.000168261415116, -0.0178746738354, 0.0330102308046;
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.00965616939349, 0.0, 0.000291766839985, 0.0, 0.932105674456, 0.0, 0.0523174915529, 0.0, 3.41705796399e-05, 1.0, 0.00751193529517, 0.0, 0.00612721792429, 0.0, 0.547617329816;
+ A << 1.0, 0.00959963582133, 0.0, 0.000706303878161, 0.0, 0.921454874639, 0.0, 0.124279814134, 0.0, 8.27195198739e-05, 1.0, 0.00715806931946, 0.0, 0.0145551608494, 0.0, 0.491841728751;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000144501999664, -4.19636699414e-05, 0.0285340095421, -0.00752461776602, -1.43609003799e-05, 0.000357848500096, -0.00257509141326, 0.0650644091693;
+ B << 0.000168261415116, -0.000101584891631, 0.0330102308046, -0.0178746738354, -3.47646073582e-05, 0.000408743642973, -0.00611711060145, 0.0730863931411;
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.00965542888285, 0.0, 4.04460947997e-05, 0.0, 0.931897839258, 0.0, 0.00790011087396, 0.0, 4.04460947997e-05, 1.0, 0.00965542888285, 0.0, 0.00790011087396, 0.0, 0.931897839258;
+ A << 1.0, 0.00959512220091, 0.0, 0.000100752776744, 0.0, 0.920202836632, 0.0, 0.0195951135, 0.0, 0.000100752776744, 1.0, 0.00959512220091, 0.0, 0.0195951135, 0.0, 0.920202836632;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000144813214739, -1.69983168063e-05, 0.0286213566286, -0.00332018673511, -1.69983168063e-05, 0.000144813214739, -0.00332018673511, 0.0286213566286;
+ B << 0.000170158358449, -4.23434605168e-05, 0.0335364259492, -0.00823525605573, -4.23434605168e-05, 0.000170158358449, -0.00823525605573, 0.0335364259492;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,33 +73,33 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.848849954683, 0.0396601987617, 5.07410924717, 1.93309188139, 0.0396601987617, 0.848849954683, 1.93309188139, 5.07410924717;
+ L << 0.798791664199, 0.0897184892451, 3.57844915691, 3.42875197165, 0.0897184892451, 0.798791664199, 3.42875197165, 3.57844915691;
Eigen::Matrix<double, 2, 4> K;
- K << 122.814750097, 4.68501085975, 3.50201207752, 0.435944310585, 3.50201207752, 0.435944310585, 122.814750097, 4.68501085975;
+ K << 118.526561433, 4.19900632703, 7.79020074229, 0.921948843301, 7.79020074228, 0.921948843301, 118.526561433, 4.19900632703;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.84475614898, 0.00630513438581, 4.80790352862, 0.482569304809, 0.00630513438581, 1.23496685529, 1.52827048914, 35.0095906678;
+ L << 0.780361183952, 0.0157698223449, 2.31127571816, 1.09059511674, 0.0157698223449, 1.23293541944, 3.02157734826, 34.5051463402;
Eigen::Matrix<double, 2, 4> K;
- K << 122.438502625, 4.65238426191, -3.6222797894, -0.114901477949, 9.82445972218, 1.21630521516, 139.573569983, 11.6848475814;
+ K << 116.74766142, 4.04674599343, -6.38397728262, -0.174173971494, 20.8480994015, 2.49474383494, 135.799605924, 11.0272286082;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.23976956418, 0.051008821609, 35.4898554745, 3.96774253511, 0.051008821609, 0.839953440087, 1.36277593575, 4.73889175169;
+ L << 1.26735275667, 0.0994717602348, 37.3766125148, 7.14618746927, 0.0994717602348, 0.745943846725, 2.20133516364, 1.9922178483;
Eigen::Matrix<double, 2, 4> K;
- K << 139.573569983, 11.6848475814, 9.82445972219, 1.21630521516, -3.62227978939, -0.114901477949, 122.438502625, 4.65238426191;
+ K << 135.799605924, 11.0272286082, 20.8480994015, 2.49474383494, -6.38397728263, -0.174173971495, 116.74766142, 4.04674599343;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.23189783926, 0.00790011087396, 34.8130461883, 0.806392261077, 0.00790011087396, 1.23189783926, 0.806392261077, 34.8130461883;
+ L << 1.22020283663, 0.0195951135, 33.6437810428, 1.9756574066, 0.0195951135, 1.22020283663, 1.9756574066, 33.6437810428;
Eigen::Matrix<double, 2, 4> K;
- K << 139.67425847, 11.6895445084, 2.52787999423, 0.599798020725, 2.52787999422, 0.599798020724, 139.67425847, 11.6895445084;
+ K << 136.125709167, 11.0402255404, 6.0764292977, 1.24911698877, 6.0764292977, 1.24911698877, 136.125709167, 11.0402255404;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
}
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index df05328..540741c 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.548849954683, 0.0396601987617, 0.0396601987617, 0.548849954683;
+ A << 0.498791664199, 0.0897184892451, 0.0897184892451, 0.498791664199;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0648871256127, -0.0057041694345, -0.0057041694345, 0.0648871256127;
+ B << 0.0720868114296, -0.0129038552514, -0.0129038552514, 0.0720868114296;
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.547617329816, 0.00612721792429, 0.0523174915529, 0.932105674456;
+ A << 0.491841728751, 0.0145551608494, 0.124279814134, 0.921454874639;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0650644091693, -0.00257509141326, -0.00752461776602, 0.0285340095421;
+ B << 0.0730863931411, -0.00611711060145, -0.0178746738354, 0.0330102308046;
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.932105674456, 0.0523174915529, 0.00612721792429, 0.547617329816;
+ A << 0.921454874639, 0.124279814134, 0.0145551608494, 0.491841728751;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0285340095421, -0.00752461776602, -0.00257509141326, 0.0650644091693;
+ B << 0.0330102308046, -0.0178746738354, -0.00611711060145, 0.0730863931411;
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.931897839258, 0.00790011087396, 0.00790011087396, 0.931897839258;
+ A << 0.920202836632, 0.0195951135, 0.0195951135, 0.920202836632;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0286213566286, -0.00332018673511, -0.00332018673511, 0.0286213566286;
+ B << 0.0335364259492, -0.00823525605573, -0.00823525605573, 0.0335364259492;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,33 +73,33 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.528849954683, 0.0396601987617, 0.0396601987617, 0.528849954683;
+ L << 0.478791664199, 0.0897184892451, 0.0897184892451, 0.478791664199;
Eigen::Matrix<double, 2, 2> K;
- K << -0.740281917714, 0.546140778145, 0.546140778145, -0.740281917714;
+ K << -1.22029287467, 1.0261517351, 1.0261517351, -1.22029287467;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.524323257635, 0.0520813668112, 0.0520813668112, 0.915399746637;
+ L << 0.450790078471, 0.12008012565, 0.12008012565, 0.922506524919;
Eigen::Matrix<double, 2, 2> K;
- K << -0.740249343264, 0.560664230143, 1.6383045686, 11.786792809;
+ K << -1.22005303899, 1.06234079364, 3.10424257883, 10.3132835345;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.915985613609, 0.00645546174383, 0.00645546174383, 0.523737390662;
+ L << 0.918551885829, 0.0172871978996, 0.0172871978996, 0.454744717561;
Eigen::Matrix<double, 2, 2> K;
- K << 11.786792809, 1.6383045686, 0.560664230143, -0.740249343264;
+ K << 10.3132835345, 3.10424257883, 1.06234079364, -1.22005303899;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.911897839258, 0.00790011087396, 0.00790011087396, 0.911897839258;
+ L << 0.900202836632, 0.0195951135, 0.0195951135, 0.900202836632;
Eigen::Matrix<double, 2, 2> K;
- K << 11.7867933868, 1.64333460977, 1.64333460977, 11.7867933868;
+ K << 10.3132878272, 3.11684016931, 3.11684016931, 10.3132878272;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}