Provide 2023 target selector
This should let us do the exact same logic as y2019. This means that the
driver has to control drivetrain velocity directly (not, e.g.,
position), but is a logical first step.
This has an extremely initial tuning that should work sanely.
Does not support going backwards yet.
Change-Id: I29675be6e6d73106563f4abc6e823a1ffcb39946
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 9150e8a..51f92b1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -49,6 +49,31 @@
int do_accel_corrections = 50;
};
+// Configuration for line-following mode.
+struct LineFollowConfig {
+ // The line-following uses an LQR controller with states of [theta,
+ // linear_velocity, angular_velocity] and inputs of [left_voltage,
+ // right_voltage].
+ // These Q and R matrices are the costs for state and input respectively.
+ Eigen::Matrix3d Q =
+ Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
+ .finished()
+ .asDiagonal());
+ Eigen::Matrix2d R =
+ Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal());
+
+ // The driver can use their steering controller to adjust where we attempt to
+ // place things laterally. This specifies how much range on either side of
+ // zero we allow them, in meters.
+ double max_controllable_offset = 0.1;
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -118,6 +143,8 @@
DownEstimatorConfig down_estimator_config{};
+ LineFollowConfig line_follow_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) {