Save more localizer history for slow pi updates
Turret pi was being slow and we didn't have enough data saved to be able
to figure out where we were and use it. Heap allocate too while we are
here so we don't run out of stack.
Change-Id: If4c9545dd33c928e1235c360566d3ac83e73992c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index ecdabb8..2a972b6 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -137,7 +137,7 @@
};
static constexpr int kNInputs = 4;
// Number of previous samples to save.
- static constexpr int kSaveSamples = 80;
+ static constexpr int kSaveSamples = 200;
// Whether we should completely rerun the entire stored history of
// kSaveSamples on every correction. Enabling this will increase overall CPU
// usage substantially; however, leaving it disabled makes it so that we are
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
index dec9118..24c876f 100644
--- a/y2020/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -15,11 +15,13 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
- ::y2020::control_loops::drivetrain::Localizer localizer(
- &event_loop, ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::y2020::control_loops::drivetrain::Localizer> localizer =
+ std::make_unique<y2020::control_loops::drivetrain::Localizer>(
+ &event_loop,
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
::y2020::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
- &localizer);
+ localizer.get());
event_loop.Run();