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) {
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index a53538b..95f14a3 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -95,6 +95,8 @@
const DrivetrainConfig<double> &dt_config,
TargetSelectorInterface *target_selector)
: dt_config_(dt_config),
+ Q_(dt_config_.line_follow_config.Q),
+ R_(dt_config_.line_follow_config.R),
A_d_(ADiscrete(dt_config_)),
B_d_(BDiscrete(dt_config_)),
K_(CalcK(A_d_, B_d_, Q_, R_)),
@@ -131,7 +133,8 @@
}
// Set an adjustment that lets the driver tweak the offset for where we place
// the target left/right.
- side_adjust_ = -goal->wheel() * 0.1;
+ side_adjust_ =
+ -goal->wheel() * dt_config_.line_follow_config.max_controllable_offset;
}
bool LineFollowDrivetrain::SetOutput(
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 24f8cd4..c5b03f0 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -1,7 +1,6 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#include "Eigen/Dense"
-
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -56,21 +55,10 @@
double relative_y_offset, double velocity_sign);
const DrivetrainConfig<double> dt_config_;
- // TODO(james): These should probably be factored out somewhere.
// TODO(james): This controller is not actually asymptotically stable, due to
// the varying goal theta.
- const ::Eigen::DiagonalMatrix<double, 3> Q_ =
- (::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();
- const ::Eigen::DiagonalMatrix<double, 2> R_ =
- (::Eigen::DiagonalMatrix<double, 2>().diagonal()
- << 1.0 / ::std::pow(12.0, 2),
- 1.0 / ::std::pow(12.0, 2))
- .finished()
- .asDiagonal();
+ const ::Eigen::Matrix<double, 3, 3> Q_;
+ const ::Eigen::Matrix<double, 2, 2> R_;
// The matrices we use for the linear controller.
// State for these is [theta, linear_velocity, angular_velocity]
const ::Eigen::Matrix<double, 3, 3> A_d_;
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 3125793..33095f2 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -7,7 +7,9 @@
PuppetLocalizer::PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config)
+ &dt_config,
+ std::unique_ptr<frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector)
: event_loop_(event_loop),
dt_config_(dt_config),
ekf_(dt_config),
@@ -17,7 +19,8 @@
"/localizer")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")) {
+ "/aos")),
+ target_selector_(std::move(target_selector)) {
ekf_.set_ignore_accel(true);
event_loop->OnRun([this, event_loop]() {
@@ -25,7 +28,11 @@
HybridEkf::State::Zero(), ekf_.P());
});
- target_selector_.set_has_target(false);
+ if (!target_selector_) {
+ auto selector = std::make_unique<TrivialTargetSelector>();
+ selector->set_has_target(false);
+ target_selector_ = std::move(selector);
+ }
}
void PuppetLocalizer::Reset(
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 4f8f4f3..24e2f88 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -32,14 +32,17 @@
PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config);
+ &dt_config,
+ std::unique_ptr<
+ frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector = {});
frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
const override {
return ekf_.X_hat().cast<double>();
}
- frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
+ frc971::control_loops::drivetrain::TargetSelectorInterface *target_selector()
override {
- return &target_selector_;
+ return target_selector_.get();
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
@@ -97,7 +100,8 @@
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
- frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+ std::unique_ptr<frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 67d681e..d590f1d 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -15,6 +15,7 @@
// state updates and then determine what poes we should be driving to.
class TargetSelectorInterface {
public:
+ virtual ~TargetSelectorInterface() {}
// Take the state as [x, y, theta, left_vel, right_vel]
// If unable to determine what target to go for, returns false. If a viable
// target is selected, then returns true and sets target_pose.
@@ -108,6 +109,7 @@
// manually set the target selector state.
class TrivialTargetSelector : public TargetSelectorInterface {
public:
+ virtual ~TrivialTargetSelector() {}
bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &, double) override {
return has_target_;
}