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/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index cdb5a61..e8fdb3c 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -11,6 +11,7 @@
using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
namespace chrono = ::std::chrono;
@@ -61,7 +62,20 @@
.finished(),
false /*is_simulated*/,
DownEstimatorConfig{.gravity_threshold = 0.015,
- .do_accel_corrections = 1000}};
+ .do_accel_corrections = 1000},
+ LineFollowConfig{
+ .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()),
+ .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 10.0 / ::std::pow(12.0, 2),
+ 10.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal()),
+ .max_controllable_offset = 0.5}};
return kDrivetrainConfig;
};