tweaked constants and disabled auto-shifting
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2b16c0b..921fab0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -212,14 +212,14 @@
const double low_omega = MotorSpeed(0, ::std::abs(velocity));
const double high_omega = MotorSpeed(1.0, ::std::abs(velocity));
- LOG(DEBUG, "velocity %f\n", velocity);
double high_torque = ((12.0 - high_omega / Kv) * Kt / kR);
double low_torque = ((12.0 - low_omega / Kv) * Kt / kR);
double high_power = high_torque * high_omega;
double low_power = low_torque * low_omega;
- if ((current == HIGH ||
- high_power > low_power + /*50*/50) &&
- high_power > low_power - /*50*/200) {
+
+ // TODO(aschuh): Do this right!
+ if ((current == HIGH || high_power > low_power + 160) &&
+ ::std::abs(velocity) > 0.14) {
return HIGH;
} else {
return LOW;
@@ -244,7 +244,7 @@
// TODO(austin): Fix the upshift logic to include states.
Gear requested_gear;
- if (constants::GetValues().clutch_transmission) {
+ if (false) {
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
@@ -272,6 +272,12 @@
} else {
left_gear_ = shift_down;
}
+ } else {
+ if (requested_gear == HIGH && left_gear_ == SHIFTING_DOWN) {
+ left_gear_ = SHIFTING_UP;
+ } else if (requested_gear == LOW && left_gear_ == SHIFTING_UP) {
+ left_gear_ = SHIFTING_DOWN;
+ }
}
}
if (right_gear_ != requested_gear) {
@@ -281,6 +287,12 @@
} else {
right_gear_ = shift_down;
}
+ } else {
+ if (requested_gear == HIGH && right_gear_ == SHIFTING_DOWN) {
+ right_gear_ = SHIFTING_UP;
+ } else if (requested_gear == LOW && right_gear_ == SHIFTING_UP) {
+ right_gear_ = SHIFTING_DOWN;
+ }
}
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index 7e514b5..7822056 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDogDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00927226867138, 0.0, 9.67653917682e-05, 0.0, 0.858140318975, 0.0, 0.0183781356125, 0.0, 9.67653917682e-05, 1.0, 0.00927226867138, 0.0, 0.0183781356125, 0.0, 0.858140318975;
+ A << 1.0, 0.00923787174605, 0.0, 0.000131162317098, 0.0, 0.851672729447, 0.0, 0.0248457251406, 0.0, 0.000131162317098, 1.0, 0.00923787174605, 0.0, 0.0248457251406, 0.0, 0.851672729447;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000120661741455, -1.60442188265e-05, 0.0235210928559, -0.00304719305118, -1.60442188265e-05, 0.000120661741455, -0.00304719305118, 0.0235210928559;
+ B << 0.000126364935405, -2.17474127771e-05, 0.0245934537462, -0.00411955394149, -2.17474127771e-05, 0.000126364935405, -0.00411955394149, 0.0245934537462;
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 @@
StateFeedbackController<4, 2, 2> MakeDogDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.69814031898, 0.0183781356125, 65.3030659057, 2.40312922858, 0.0183781356125, 1.69814031898, 2.40312922858, 65.3030659057;
+ L << 1.69167272945, 0.0248457251406, 64.4706646869, 3.2355304474, 0.0248457251406, 1.69167272945, 3.2355304474, 64.4706646869;
Eigen::Matrix<double, 2, 4> K;
- K << 257.282562369, 15.135374919, 33.3313439577, 2.7466676405, 33.3313439577, 2.7466676405, 257.282562369, 15.135374919;
+ K << 248.918529922, 14.4460993245, 41.6953764051, 3.43594323497, 41.6953764051, 3.43594323497, 248.918529922, 14.4460993245;
return StateFeedbackController<4, 2, 2>(L, K, MakeDogDrivetrainPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index d8421a2..b3d4277 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> MakeDogVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.858140318975, 0.0183781356125, 0.0183781356125, 0.858140318975;
+ A << 0.851672729447, 0.0248457251406, 0.0248457251406, 0.851672729447;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0235210928559, -0.00304719305118, -0.00304719305118, 0.0235210928559;
+ B << 0.0245934537462, -0.00411955394149, -0.00411955394149, 0.0245934537462;
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> MakeDogVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.857986344025, 0.00408284222985, 0.0195370380139, 0.968532564253;
+ A << 0.851389310398, 0.00553670185935, 0.0264939835067, 0.967000817219;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0235466227252, -0.00148084354724, -0.00323934525959, 0.0114132132839;
+ B << 0.0246404461385, -0.00200815724925, -0.00439284398274, 0.0119687766843;
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> MakeDogVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.968532564253, 0.0195370380139, 0.00408284222985, 0.857986344025;
+ A << 0.967000817219, 0.0264939835067, 0.00553670185935, 0.851389310398;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0114132132839, -0.00323934525959, -0.00148084354724, 0.0235466227252;
+ B << 0.0119687766843, -0.00439284398274, -0.00200815724925, 0.0246404461385;
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> MakeDogVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.968497658015, 0.0043351996773, 0.0043351996773, 0.968497658015;
+ A << 0.966936300149, 0.00589655754287, 0.00589655754287, 0.966936300149;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0114258737482, -0.00157237338764, -0.00157237338764, 0.0114258737482;
+ B << 0.0119921769728, -0.00213867661221, -0.00213867661221, 0.0119921769728;
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> MakeDogVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.838140318975, 0.0183781356125, 0.0183781356125, 0.838140318975;
+ L << 0.831672729447, 0.0248457251406, 0.0248457251406, 0.831672729447;
Eigen::Matrix<double, 2, 2> K;
- K << 11.2651379288, 2.240762619, 2.240762619, 11.2651379288;
+ K << 10.7028500331, 2.80305051463, 2.80305051463, 10.7028500331;
return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.838267905918, 0.0195319064993, 0.0195319064993, 0.94825100236;
+ L << 0.831852326508, 0.0264837489415, 0.0264837489415, 0.946537801108;
Eigen::Matrix<double, 2, 2> K;
- K << 11.2651385254, 2.24416423621, 4.90910926672, 32.9269398272;
+ K << 10.7028511964, 2.80768406175, 6.14180888507, 31.6936764099;
return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.950927164662, 0.00446324489791, 0.00446324489791, 0.835591743616;
+ L << 0.951097545753, 0.0063707209266, 0.0063707209266, 0.827292581863;
Eigen::Matrix<double, 2, 2> K;
- K << 32.9269398272, 4.90910926672, 2.24416423621, 11.2651385254;
+ K << 31.6936764099, 6.14180888507, 2.80768406175, 10.7028511964;
return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeDogVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.948497658015, 0.0043351996773, 0.0043351996773, 0.948497658015;
+ L << 0.946936300149, 0.00589655754287, 0.00589655754287, 0.946936300149;
Eigen::Matrix<double, 2, 2> K;
- K << 32.9269398561, 4.91066546682, 4.91066546682, 32.9269398561;
+ K << 31.6936764663, 6.14392885659, 6.14392885659, 31.6936764663;
return StateFeedbackController<2, 2, 2>(L, K, MakeDogVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c39886a..fcca56a 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -62,10 +62,7 @@
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- if is_clutch:
- self.J = 4.5
- else:
- self.J = 4.9
+ self.J = 4.5
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).