tuning stuff that we did at NASA
diff --git a/aos/externals/.gitignore b/aos/externals/.gitignore
index 73cb3e2..694129e 100644
--- a/aos/externals/.gitignore
+++ b/aos/externals/.gitignore
@@ -25,3 +25,7 @@
/libevent-2.0.21-prefix/
/libevent-2.0.21.tar.gz
/libevent-2.0.21/
+/libcdd-094g-prefix/
+/libcdd-094g.tar.gz
+/libcdd-094g/
+
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 122e2d2..3e24dc2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -150,7 +150,7 @@
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())) {
- ttrust_ = 1.0;
+ ttrust_ = 1.1;
wheel_ = 0.0;
throttle_ = 0.0;
@@ -158,7 +158,7 @@
highgear_ = true;
}
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- const double kWheelNonLinearity = 0.1;
+ const double kWheelNonLinearity = 0.4;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
wheel_ = sin(angular_range * wheel) / sin(angular_range);
@@ -197,6 +197,28 @@
(ttrust_ * min_K_sum + min_FF_sum));
}
+ double MaxVelocity() {
+ const Eigen::Matrix<double, 2, 2> FF =
+ loop_->B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+
+ constexpr int kHighGearController = 3;
+ const Eigen::Matrix<double, 2, 2> FF_high =
+ loop_->controller(kHighGearController).plant.B.inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() -
+ loop_->controller(kHighGearController).plant.A);
+
+ ::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
+ int min_FF_sum_index;
+ const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
+ //const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double high_min_FF_sum = FF_high.col(0).sum();
+
+ const double adjusted_ff_voltage = ::aos::Clip(
+ 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ return adjusted_ff_voltage / min_FF_sum;
+ }
+
void Update() {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
@@ -210,31 +232,38 @@
const double fvel = FilterVelocity(throttle_);
const double sign_svel = wheel_ * ((fvel > 0.0) ? 1.0 : -1.0);
- const double svel = ::std::abs(fvel) * wheel_;
- const double left_velocity = fvel - svel;
- const double right_velocity = fvel + svel;
-
- // K * R = w
- Eigen::Matrix<double,1,2> equality_k;
- equality_k << 1 + sign_svel, -(1 - sign_svel);
- const double equality_w = 0.0;
+ double steering_velocity;
+ if (quickturn_) {
+ steering_velocity = wheel_ * MaxVelocity();
+ } else {
+ steering_velocity = ::std::abs(fvel) * wheel_;
+ }
+ const double left_velocity = fvel - steering_velocity;
+ const double right_velocity = fvel + steering_velocity;
// Integrate velocity to get the position.
// This position is used to get integral control.
loop_->R << left_velocity, right_velocity;
+ if (!quickturn_) {
+ // K * R = w
+ Eigen::Matrix<double,1,2> equality_k;
+ equality_k << 1 + sign_svel, -(1 - sign_svel);
+ const double equality_w = 0.0;
+
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
U_Poly_.H() * (loop_->K() + FF),
U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat);
// Limit R back inside the box.
- const Eigen::Matrix<double, 2, 1> boxed_R =
+ loop_->R =
CoerceGoal(R_poly, equality_k, equality_w, loop_->R);
+ }
- const Eigen::Matrix<double, 2, 1> FF_volts = FF * boxed_R;
+ const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R;
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (boxed_R - loop_->X_hat) + FF_volts;
+ loop_->K() * (loop_->R - loop_->X_hat) + FF_volts;
for (int i = 0; i < 2; i++) {
loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index cdfe744..249de70 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -5,6 +5,7 @@
#include "aos/controls/polytope.h"
#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/controls/polytope.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace frc971 {
@@ -23,7 +24,9 @@
explicit DrivetrainLoop(
control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
: aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false>(
- my_drivetrain) {}
+ my_drivetrain) {
+ ::aos::controls::HPolytope<0>::Init();
+ }
protected:
// Executes one cycle of the control loop.
diff --git a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
index e543c9f..071b257 100644
--- a/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00931379160739, 0.0, 4.70184876909e-06, 0.0, 0.865971883056, 0.0, 0.000895808426591, 0.0, 4.70184876909e-06, 1.0, 0.00931379160739, 0.0, 0.000895808426591, 0.0, 0.865971883056;
+ A << 1.0, 0.00948696019317, 0.0, 3.55966215909e-06, 0.0, 0.899177606502, 0.0, 0.000686937184856, 0.0, 3.55966215909e-06, 1.0, 0.00948696019317, 0.0, 0.000686937184856, 0.0, 0.899177606502;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000126707931029, -8.6819330098e-07, 0.0247482041615, -0.000165410440259, -8.6819330098e-07, 0.000126707931029, -0.000165410440259, 0.0247482041615;
+ B << 9.50723568824e-05, -6.59647588097e-07, 0.0186835844877, -0.000127297602107, -6.59647588097e-07, 9.50723568824e-05, -0.000127297602107, 0.0186835844877;
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> MakeDrivetrainController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.70597188306, 0.000895808426591, 66.3158545945, 0.117712892743, 0.000895808426591, 1.70597188306, 0.117712892743, 66.3158545945;
+ L << 1.7391776065, 0.000686937184856, 70.7236123469, 0.0920942992696, 0.000686937184856, 1.7391776065, 0.0920942992696, 70.7236123469;
Eigen::Matrix<double, 2, 4> K;
- K << 240.432225842, 14.3659115621, 1.60698530163, 0.13242189318, 1.60698530163, 0.13242189318, 240.432225842, 14.3659115621;
+ K << 318.476158856, 20.8163224602, 2.1698861574, 0.178798182963, 2.1698861574, 0.178798182963, 318.476158856, 20.8163224602;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainPlantCoefficients());
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
index 31a029f..d07f22b 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain_motor_plant.cc
@@ -75,7 +75,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.879177606502, 0.000686937184856, 0.000686937184856, 0.879177606502;
Eigen::Matrix<double, 2, 2> K;
- K << 32.0714744818, 0.255280724401, 0.255280724401, 32.0714744818;
+ K << 21.3663935118, 0.182343374572, 0.182343374572, 21.3663935118;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
@@ -83,7 +83,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.879178111554, 0.000716636558747, 0.000716636558747, 0.958034852635;
Eigen::Matrix<double, 2, 2> K;
- K << 32.0714744826, 0.255470827831, 0.558842435881, 76.1557821586;
+ K << 21.3663935124, 0.182479162737, 0.399173168486, 53.6921632348;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
@@ -91,7 +91,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.958040379369, 0.000149803514919, 0.000149803514919, 0.87917258482;
Eigen::Matrix<double, 2, 2> K;
- K << 76.1557821586, 0.558842435881, 0.255470827831, 32.0714744826;
+ K << 53.6921632348, 0.399173168486, 0.182479162737, 21.3663935124;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
@@ -99,7 +99,7 @@
Eigen::Matrix<double, 2, 2> L;
L << 0.958035518136, 0.000156145735499, 0.000156145735499, 0.958035518136;
Eigen::Matrix<double, 2, 2> K;
- K << 76.1557821586, 0.558929371597, 0.558929371597, 76.1557821586;
+ K << 53.6921632349, 0.399235265426, 0.399235265426, 53.6921632349;
return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 9fa7841..0251408 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -117,7 +117,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.3, 0.3])
+ self.PlaceControllerPoles([0.5, 0.5])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index be4e188..a2404d0 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -82,7 +82,7 @@
bool is_control_loop_driving = false;
double left_goal = 0.0;
double right_goal = 0.0;
- const double wheel = data.GetAxis(kSteeringWheel);
+ const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
const double kThrottleGain = 1.0 / 2.5;