Tuned drivetrain.
Change-Id: I63b2d1ee22772dfde9317aacb0355506a4c3af00
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
index 0c191d1..31fd7e3 100644
--- a/bot3/control_loops/drivetrain/drivetrain.cc
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -243,11 +243,11 @@
// Mass of the robot, in kg.
static constexpr double m = 68;
// Radius of the robot, in meters (from last year).
- static constexpr double rb = 0.9603 / 2.0;
+ static constexpr double rb = 0.66675 / 2.0;
static constexpr double kWheelRadius = 0.0515938;
// Resistance of the motor, divided by the number of motors.
static constexpr double kR =
- (12.0 / kStallCurrent / 2 + 0.03) / (0.93 * 0.93);
+ (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
// Motor velocity constant
static constexpr double Kv =
((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -318,10 +318,12 @@
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
+ quickturn_ = quickturn;
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
- wheel_ *= 2.3;
- quickturn_ = quickturn;
+ if (!quickturn_) {
+ wheel_ *= 1.5;
+ }
static const double kThrottleDeadband = 0.05;
if (::std::abs(throttle) < kThrottleDeadband) {
diff --git a/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index ed8e990..75bb2a1 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.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
+ A << 0.909050248724, 0.0192068507303, 0.0192068507303, 0.909050248724;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
+ B << 0.0232454208683, -0.00490898900238, -0.00490898900238, 0.0232454208683;
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.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
+ A << 0.909050248724, 0.0192068507303, 0.0192068507303, 0.909050248724;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
+ B << 0.0232454208683, -0.00490898900238, -0.00490898900238, 0.0232454208683;
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.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
+ A << 0.909050248724, 0.0192068507303, 0.0192068507303, 0.909050248724;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
+ B << 0.0232454208683, -0.00490898900238, -0.00490898900238, 0.0232454208683;
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.896256126872, 0.0320009725823, 0.0320009725823, 0.896256126872;
+ A << 0.909050248724, 0.0192068507303, 0.0192068507303, 0.909050248724;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0265154105376, -0.00817897867162, -0.00817897867162, 0.0265154105376;
+ B << 0.0232454208683, -0.00490898900238, -0.00490898900238, 0.0232454208683;
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.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
+ L << 0.889050248724, 0.0192068507303, 0.0192068507303, 0.889050248724;
Eigen::Matrix<double, 2, 2> K;
- K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
+ K << 14.0983425164, 3.80356456422, 3.80356456422, 14.0983425164;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
+ A_inv << 1.1005404965, -0.0232527487546, -0.0232527487546, 1.1005404965;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
+ L << 0.889050248724, 0.0192068507303, 0.0192068507303, 0.889050248724;
Eigen::Matrix<double, 2, 2> K;
- K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
+ K << 14.0983425164, 3.80356456422, 3.80356456422, 14.0983425164;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
+ A_inv << 1.1005404965, -0.0232527487546, -0.0232527487546, 1.1005404965;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
+ L << 0.889050248724, 0.0192068507303, 0.0192068507303, 0.889050248724;
Eigen::Matrix<double, 2, 2> K;
- K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
+ K << 14.0983425164, 3.80356456422, 3.80356456422, 14.0983425164;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
+ A_inv << 1.1005404965, -0.0232527487546, -0.0232527487546, 1.1005404965;
return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.876256126872, 0.0320009725823, 0.0320009725823, 0.876256126872;
+ L << 0.889050248724, 0.0192068507303, 0.0192068507303, 0.889050248724;
Eigen::Matrix<double, 2, 2> K;
- K << 12.7592804818, 5.1426265988, 5.1426265988, 12.7592804818;
+ K << 14.0983425164, 3.80356456422, 3.80356456422, 14.0983425164;
Eigen::Matrix<double, 2, 2> A_inv;
- A_inv << 1.11717672672, -0.0398889789754, -0.0398889789754, 1.11717672672;
+ A_inv << 1.1005404965, -0.0232527487546, -0.0232527487546, 1.1005404965;
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 c3144be..45c3387 100755
--- a/bot3/control_loops/python/drivetrain.py
+++ b/bot3/control_loops/python/drivetrain.py
@@ -63,7 +63,7 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 8
+ self.J = 10
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).