Initial tuning/cleanup of 2022 localizer
This is based on some data collection off of the 2020 robot with the IMU
somewhat loosely mounted. Should behave reasonably--when the wheels are
not slipping, this currently tends to be less precise than traditional
methods, but otherwise behaves reasonably, and does handle substantial
wheel slip reasonably.
General changes:
* General tuning.
* Update some 2020 references to refer to 2022.
* Unwrap the encoder readings from the pico board.
* Make use of the pico timestamps.
Next steps are:
* Adding actual connectors for image corrections.
* Making the turret able to aim.
* Tuning this against driver-practice based IMU readings--use TODOs
in the code as a starting point.
Change-Id: Ie3effe2cbb822317f6cd4a201cce939517a4044f
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index bb52a40..2c4d1a4 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -14,6 +14,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
+#include "frc971/zeroing/wrap.h"
namespace frc971::controls {
@@ -57,7 +58,6 @@
// TODO:
// * Implement paying attention to camera readings.
// * Tune for ADIS16505/real robot.
-// * Tune down CPU usage to run on a pi.
class ModelBasedLocalizer {
public:
static constexpr size_t kX = 0;
@@ -90,7 +90,9 @@
static constexpr size_t kNModelInputs = 2;
// Branching period, in cycles.
- static constexpr int kBranchPeriod = 1;
+ // Needs 10 to even stay alive, and still at ~96% CPU.
+ // ~20 gives ~55-60% CPU.
+ static constexpr int kBranchPeriod = 20;
typedef Eigen::Matrix<double, kNModelStates, 1> ModelState;
typedef Eigen::Matrix<double, kNAccelStates, 1> AccelState;
@@ -122,6 +124,8 @@
AccelState accel_state() const { return current_state_.accel_state; };
+ void set_longitudinal_offset(double offset) { long_offset_ = offset; }
+
private:
struct CombinedState {
AccelState accel_state;
@@ -155,6 +159,13 @@
const AccelInput &accel_inputs,
const Eigen::Vector2d &filtered_accel,
const ModelInput &model_inputs);
+ void UpdateState(
+ CombinedState *state,
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
+ const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
+ const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
+ const AccelInput &accel_input, const ModelInput &model_input,
+ aos::monotonic_clock::duration dt);
const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
const StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
@@ -186,6 +197,10 @@
aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
bool using_model_;
+ // X position of the IMU, in meters. 0 = center of robot, positive = ahead of
+ // center, negative = behind center.
+ double long_offset_ = -0.15;
+
double last_residual_ = 0.0;
double filtered_residual_ = 0.0;
Eigen::Vector2d filtered_residual_accel_ = Eigen::Vector2d::Zero();
@@ -206,6 +221,8 @@
aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+ ModelBasedLocalizer *localizer() { return &model_based_; }
+
private:
aos::EventLoop *event_loop_;
ModelBasedLocalizer model_based_;
@@ -215,6 +232,14 @@
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
aos::monotonic_clock::min_time;
+ std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
+ aos::monotonic_clock::duration pico_offset_error_;
+ // t = pico_offset_ + pico_timestamp.
+ // Note that this can drift over sufficiently long time periods!
+ std::optional<std::chrono::nanoseconds> pico_offset_;
+
+ zeroing::UnwrapSensor left_encoder_;
+ zeroing::UnwrapSensor right_encoder_;
};
} // namespace frc971::controls
#endif // Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_