Merge changes I67c430da,Id4ae2ce1
* changes:
Parameterize spline drivetrain & slightly detune left/right vel
Check for instability in trajectory generation
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.fbs b/frc971/control_loops/drivetrain/drivetrain_config.fbs
index 645eeac..0947063 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_config.fbs
@@ -98,6 +98,15 @@
max_controllable_offset:double = 0.1 (id: 2);
}
+table SplineFollowerConfig {
+ // Q should be a 5x5 positive-definite matrix; it is used as the state cost
+ // of the LQR controller for the spline-following mode.
+ q:frc971.fbs.Matrix (id: 0);
+ // R should be a 2x2 positive-definite matrix; it is used as the input cost
+ // of the LQR controller for the spline-following mode.
+ r:frc971.fbs.Matrix (id: 1);
+}
+
// These constants are all specified by the drivetrain python code, and
// so are separated out for easy codegen.
table DrivetrainLoopConfig {
@@ -135,6 +144,7 @@
is_simulated:bool = false (id: 14);
down_estimator_config:DownEstimatorConfig (id: 15);
line_follow_config:LineFollowConfig (id: 16);
+ spline_follower_config:SplineFollowerConfig (id: 20);
top_button_use:PistolTopButtonUse = kShift (id: 17);
second_button_use:PistolSecondButtonUse = kShiftLow (id: 18);
bottom_button_use:PistolBottomButtonUse = kSlowDown (id: 19);
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 8e0a58d..3f56082 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -50,6 +50,33 @@
}
};
+struct SplineFollowerConfig {
+ // The line-following uses an LQR controller with states of
+ // [longitudinal_position, lateral_positoin, theta, left_velocity,
+ // right_velocity] and inputs of [left_voltage, right_voltage]. These Q and R
+ // matrices are the costs for state and input respectively.
+ Eigen::Matrix<double, 5, 5> Q = Eigen::Matrix<double, 5, 5>(
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal() << ::std::pow(60.0, 2.0),
+ ::std::pow(60.0, 2.0), ::std::pow(40.0, 2.0), ::std::pow(30.0, 2.0),
+ ::std::pow(30.0, 2.0))
+ .finished()
+ .asDiagonal());
+ Eigen::Matrix2d R = Eigen::Matrix2d(
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal() << 5.0, 5.0)
+ .finished()
+ .asDiagonal());
+
+ static SplineFollowerConfig FromFlatbuffer(
+ const fbs::SplineFollowerConfig *fbs) {
+ if (fbs == nullptr) {
+ return {};
+ }
+ return SplineFollowerConfig{
+ .Q = ToEigenOrDie<5, 5>(*CHECK_NOTNULL(fbs->q())),
+ .R = ToEigenOrDie<2, 2>(*CHECK_NOTNULL(fbs->r()))};
+ }
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -125,6 +152,8 @@
PistolSecondButtonUse second_button_use = PistolSecondButtonUse::kShiftLow;
PistolBottomButtonUse bottom_button_use = PistolBottomButtonUse::kSlowDown;
+ SplineFollowerConfig spline_follower_config{};
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
@@ -229,7 +258,9 @@
.line_follow_config =
LineFollowConfig::FromFlatbuffer(fbs.line_follow_config()),
ASSIGN(top_button_use), ASSIGN(second_button_use),
- ASSIGN(bottom_button_use)
+ ASSIGN(bottom_button_use),
+ .spline_follower_config = SplineFollowerConfig::FromFlatbuffer(
+ fbs.spline_follower_config()),
#undef ASSIGN
};
}
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 163311a..cf9929f 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -770,17 +770,8 @@
// Set up reasonable gain matrices. Current choices of gains are arbitrary
// and just setup to work well enough for the simulation tests.
- // TODO(james): Tune this on a real robot.
- // TODO(james): Pull these out into a config.
- Eigen::Matrix<double, 5, 5> Q;
- Q.setIdentity();
- Q.diagonal() << 30.0, 30.0, 20.0, 15.0, 15.0;
- Q *= 2.0;
- Q = (Q * Q).eval();
-
- Eigen::Matrix<double, 2, 2> R;
- R.setIdentity();
- R *= 5.0;
+ Eigen::Matrix<double, 5, 5> Q = config_->spline_follower_config.Q;
+ Eigen::Matrix<double, 2, 2> R = config_->spline_follower_config.R;
Eigen::Matrix<double, 5, 5> P = Q;
@@ -822,6 +813,7 @@
const Eigen::Matrix<double, 2, 5> K = RBPBinv * APB.transpose();
plan_gains_[i].second = K.cast<float>();
P = AP * A_discrete - APB * K + Q;
+ CHECK_LT(P.norm(), 1e30) << "LQR calculations became unstable.";
}
}
diff --git a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
index 194b123..1127d6a 100644
--- a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
+++ b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
@@ -10,7 +10,7 @@
"quickturn_wheel_multiplier": 1.2,
"wheel_multiplier": 1.2,
"pistol_grip_shift_enables_line_follow": false,
- "imu_transform":{
+ "imu_transform": {
"rows": 3,
"cols": 3,
"data": [1, 0, 0, 0, 1, 0, 0, 0, 1]
@@ -22,5 +22,21 @@
},
"top_button_use": "kNone",
"second_button_use": "kTurn1",
- "bottom_button_use": "kControlLoopDriving"
+ "bottom_button_use": "kControlLoopDriving",
+ "spline_follower_config": {
+ "q": {
+ "rows": 5,
+ "cols": 5,
+ "data": [3600, 0, 0, 0, 0,
+ 0, 3600, 0, 0, 0,
+ 0, 0, 1600, 0, 0,
+ 0, 0, 0, 16, 0,
+ 0, 0, 0, 0, 16]
+ },
+ "r": {
+ "rows": 2,
+ "cols": 2,
+ "data": [5, 0, 0, 5]
+ }
+ }
}