Swap y2024 drivetrain to use ConstantsFetcher
Simultaneously, remove the line following configuration from the y2024
drivetrain, since that code will not be relevant to this year.
Change-Id: I536bd2dd18080bbaae5d83cb151bbd2353d7896c
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/drivetrain/BUILD b/y2024/control_loops/drivetrain/BUILD
index a362cb9..e9f93ca 100644
--- a/y2024/control_loops/drivetrain/BUILD
+++ b/y2024/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("//aos:config.bzl", "aos_config")
+load("//tools/build_rules:template.bzl", "jinja2_template")
load("//frc971/control_loops/drivetrain:drivetrain_config.bzl", "drivetrain_config")
genrule(
@@ -73,7 +74,9 @@
deps = [
":polydrivetrain_plants",
"//frc971:shifter_hall_effect",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops/drivetrain:drivetrain_config",
+ "//y2024/constants:constants_fbs",
],
)
@@ -117,11 +120,23 @@
":drivetrain_base",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops/drivetrain:trajectory_generator",
+ "//y2024/constants:constants_fbs",
],
)
drivetrain_config(
- name = "drivetrain_config",
- out = "drivetrain_config.json",
+ name = "drivetrain_loop_config",
+ out = "drivetrain_loop_config.json",
+)
+
+jinja2_template(
+ name = "drivetrain_config.json",
+ src = "drivetrain_config.jinja2.json",
+ includes = [
+ ":drivetrain_loop_config.json",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
)
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.cc b/y2024/control_loops/drivetrain/drivetrain_base.cc
index ebb61ac..79d35e1 100644
--- a/y2024/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2024/control_loops/drivetrain/drivetrain_base.cc
@@ -2,84 +2,18 @@
#include <chrono>
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2024/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
-#include "y2024/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2024/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-
-using ::frc971::control_loops::drivetrain::DownEstimatorConfigT;
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-using ::frc971::control_loops::drivetrain::LineFollowConfig;
-
-namespace chrono = ::std::chrono;
+#include "y2024/constants/constants_generated.h"
namespace y2024::control_loops::drivetrain {
-using ::frc971::constants::ShifterHallEffect;
-
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
-
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- // Yaw of the IMU relative to the robot frame.
- static constexpr double kImuYaw = 0.0;
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::kSimpleShifter,
- ::frc971::control_loops::drivetrain::LoopType::kClosedLoop,
- ::frc971::control_loops::drivetrain::GyroType::kSpartanGyro,
- ::frc971::control_loops::drivetrain::ImuType::kImuFlippedX,
-
- drivetrain::MakeDrivetrainLoop,
- drivetrain::MakeVelocityDrivetrainLoop,
- drivetrain::MakeKFDrivetrainLoop,
- drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius,
- drivetrain::kWheelRadius,
- drivetrain::kV,
-
- drivetrain::kHighGearRatio,
- drivetrain::kLowGearRatio,
- drivetrain::kJ,
- drivetrain::kMass,
- kThreeStateDriveShifter,
- kThreeStateDriveShifter,
- true /* default_high_gear */,
- 0 /* down_offset if using constants use
- constants::GetValues().down_error */
- ,
- 0.7 /* wheel_non_linearity */,
- 1.2 /* quickturn_wheel_multiplier */,
- 1.2 /* wheel_multiplier */,
- true /*pistol_grip_shift_enables_line_follow*/,
- (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
- 0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
- .finished(),
- false /*is_simulated*/,
- DownEstimatorConfigT{{}, 0.015, 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},
- frc971::control_loops::drivetrain::PistolTopButtonUse::kNone,
- frc971::control_loops::drivetrain::PistolSecondButtonUse::kTurn1,
- frc971::control_loops::drivetrain::PistolBottomButtonUse::
- kControlLoopDriving,
- };
-
- return kDrivetrainConfig;
+const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+GetDrivetrainConfig(aos::EventLoop *event_loop) {
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(event_loop);
+ return frc971::control_loops::drivetrain::DrivetrainConfig<double>::
+ FromFlatbuffer(
+ *CHECK_NOTNULL(constants_fetcher.constants().common()->drivetrain()));
};
} // namespace y2024::control_loops::drivetrain
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.h b/y2024/control_loops/drivetrain/drivetrain_base.h
index e7eb581..fbe85f4 100644
--- a/y2024/control_loops/drivetrain/drivetrain_base.h
+++ b/y2024/control_loops/drivetrain/drivetrain_base.h
@@ -1,12 +1,13 @@
#ifndef Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
#define Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#include "aos/events/event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
namespace y2024::control_loops::drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
-GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+GetDrivetrainConfig(aos::EventLoop *event_loop);
} // namespace y2024::control_loops::drivetrain
diff --git a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
new file mode 100644
index 0000000..194b123
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
@@ -0,0 +1,26 @@
+{
+ "shifter_type": "kSimpleShifter",
+ "loop_type": "kClosedLoop",
+ "gyro_type": "kSpartanGyro",
+ "imu_type" : "kImuX",
+ "loop_config": {% include 'y2024/control_loops/drivetrain/drivetrain_loop_config.json' %},
+ "default_high_gear": false,
+ "down_offset": 0.0,
+ "wheel_non_linearity": 0.7,
+ "quickturn_wheel_multiplier": 1.2,
+ "wheel_multiplier": 1.2,
+ "pistol_grip_shift_enables_line_follow": false,
+ "imu_transform":{
+ "rows": 3,
+ "cols": 3,
+ "data": [1, 0, 0, 0, 1, 0, 0, 0, 1]
+ },
+ "is_simulated": true,
+ "down_estimator_config": {
+ "gravity_threshold": 0.015,
+ "do_accel_corrections": 1000
+ },
+ "top_button_use": "kNone",
+ "second_button_use": "kTurn1",
+ "bottom_button_use": "kControlLoopDriving"
+}
diff --git a/y2024/control_loops/drivetrain/drivetrain_main.cc b/y2024/control_loops/drivetrain/drivetrain_main.cc
index 196fbb7..e0b2eb7 100644
--- a/y2024/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2024/control_loops/drivetrain/drivetrain_main.cc
@@ -22,10 +22,10 @@
localizer = std::make_unique<
::frc971::control_loops::drivetrain::PuppetLocalizer>(
&event_loop,
- ::y2024::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop));
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
- y2024::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
- localizer.get());
+ y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop),
+ &event_loop, localizer.get());
event_loop.Run();
diff --git a/y2024/control_loops/drivetrain/trajectory_generator_main.cc b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
index 260f4a3..f53f79a 100644
--- a/y2024/control_loops/drivetrain/trajectory_generator_main.cc
+++ b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
@@ -3,7 +3,9 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain::TrajectoryGenerator;
@@ -17,9 +19,12 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
::aos::ShmEventLoop event_loop(&config.message());
TrajectoryGenerator generator(
- &event_loop, ::y2024::control_loops::drivetrain::GetDrivetrainConfig());
+ &event_loop,
+ ::y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop));
event_loop.OnRun([]() {
if (FLAGS_skip_renicing) {