Pull drivetrain estimators out into a separate class
This lets us run the estimators without re-running the controllers.
This will be used to tune the estimators on log files.
Change-Id: I4fa50be17bfc5136b988a5a8289ab732cb3ec2a1
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7b7fbca..b33c48d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -27,13 +27,10 @@
namespace control_loops {
namespace drivetrain {
-DrivetrainLoop::DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
- ::aos::EventLoop *event_loop,
- LocalizerInterface *localizer,
- const ::std::string &name)
- : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name),
- dt_config_(dt_config),
+DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer)
+ : dt_config_(dt_config),
localizer_control_fetcher_(
event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
@@ -41,19 +38,16 @@
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
- down_estimator_(dt_config),
+ down_estimator_(dt_config_),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
- dt_openloop_(dt_config_, &kf_),
- dt_closedloop_(dt_config_, &kf_, localizer_),
- dt_spline_(dt_config_),
- dt_line_follow_(dt_config_, localizer->target_selector()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
left_high_requested_(dt_config_.default_high_gear),
right_high_requested_(dt_config_.default_high_gear) {
- ::aos::controls::HPolytope<0>::Init();
- event_loop->SetRuntimeRealtimePriority(30);
+ last_voltage_.setZero();
+ last_last_voltage_.setZero();
+ aos::controls::HPolytope<0>::Init();
event_loop->OnRun([this]() {
// On the first fetch, make sure that we are caught all the way up to the
// present.
@@ -64,63 +58,38 @@
}
}
-int DrivetrainLoop::ControllerIndexFromGears() {
- if (MaybeHigh(left_gear_)) {
- if (MaybeHigh(right_gear_)) {
- return 3;
- } else {
- return 2;
- }
- } else {
- if (MaybeHigh(right_gear_)) {
- return 1;
- } else {
- return 0;
- }
- }
+flatbuffers::Offset<LocalizerState> DrivetrainFilters::PopulateLocalizerState(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return localizer_->PopulateStatus(fbb);
+}
+flatbuffers::Offset<ImuZeroerState> DrivetrainFilters::PopulateImuZeroerState(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ return imu_zeroer_.PopulateStatus(fbb);
}
-Gear ComputeGear(double shifter_position,
- const constants::ShifterHallEffect &shifter_config,
- bool high_requested) {
- if (shifter_position < shifter_config.clear_low) {
- return Gear::LOW;
- } else if (shifter_position > shifter_config.clear_high) {
- return Gear::HIGH;
- } else {
- if (high_requested) {
- return Gear::SHIFTING_UP;
- } else {
- return Gear::SHIFTING_DOWN;
- }
- }
+flatbuffers::Offset<DownEstimatorState>
+DrivetrainFilters::PopulateDownEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb,
+ aos::monotonic_clock::time_point monotonic_now) {
+ return down_estimator_.PopulateStatus(fbb, monotonic_now);
}
-void DrivetrainLoop::RunIteration(
- const drivetrain::Goal *goal, const drivetrain::Position *position,
- aos::Sender<drivetrain::Output>::Builder *output,
- aos::Sender<drivetrain::Status>::Builder *status) {
- const monotonic_clock::time_point monotonic_now =
- event_loop()->monotonic_now();
+void DrivetrainFilters::Reset(aos::monotonic_clock::time_point monotonic_now,
+ const drivetrain::Position *position) {
+ // If all the sensors got reset (e.g., due to wpilib_interface restarting),
+ // reset the localizer and down estimator to avoid weird jumps in the
+ // filters.
+ down_estimator_.Reset();
+ // Just reset the localizer to the current state, except for the encoders.
+ LocalizerInterface::Ekf::State X_hat = localizer_->Xhat();
+ X_hat(LocalizerInterface::StateIdx::kLeftEncoder) = position->left_encoder();
+ X_hat(LocalizerInterface::StateIdx::kRightEncoder) =
+ position->right_encoder();
+ localizer_->Reset(monotonic_now, X_hat);
+}
- if (!has_been_enabled_ && output) {
- has_been_enabled_ = true;
- }
-
- if (WasReset()) {
- // If all the sensors got reset (e.g., due to wpilib_interface restarting),
- // reset the localizer and down estimator to avoid weird jumps in the
- // filters.
- down_estimator_.Reset();
- // Just reset the localizer to the current state, except for the encoders.
- LocalizerInterface::Ekf::State X_hat = localizer_->Xhat();
- X_hat(LocalizerInterface::StateIdx::kLeftEncoder) =
- position->left_encoder();
- X_hat(LocalizerInterface::StateIdx::kRightEncoder) =
- position->right_encoder();
- localizer_->Reset(monotonic_now, X_hat);
- }
-
+void DrivetrainFilters::Correct(aos::monotonic_clock::time_point monotonic_now,
+ const drivetrain::Position *position) {
// TODO(austin): Put gear detection logic here.
switch (dt_config_.shifter_type) {
case ShifterType::SIMPLE_SHIFTER:
@@ -147,21 +116,6 @@
break;
}
- kf_.set_index(ControllerIndexFromGears());
-
- flatbuffers::Offset<GearLogging> gear_logging_offset;
- // Set the gear-logging parts of the status
- if (status) {
- GearLogging::Builder gear_logging_builder =
- status->MakeBuilder<GearLogging>();
- gear_logging_builder.add_left_state(static_cast<uint32_t>(left_gear_));
- gear_logging_builder.add_right_state(static_cast<uint32_t>(right_gear_));
- gear_logging_builder.add_left_loop_high(MaybeHigh(left_gear_));
- gear_logging_builder.add_right_loop_high(MaybeHigh(right_gear_));
- gear_logging_builder.add_controller_index(kf_.index());
- gear_logging_offset = gear_logging_builder.Finish();
- }
-
while (imu_values_fetcher_.FetchNext()) {
CHECK(imu_values_fetcher_->has_readings());
last_gyro_time_ = monotonic_now;
@@ -249,30 +203,133 @@
last_gyro_rate_ = 0.0;
}
+ localizer_->Update(
+ {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
+ monotonic_now, position->left_encoder(), position->right_encoder(),
+ down_estimator_.avg_recent_yaw_rates(),
+ down_estimator_.avg_recent_accel());
+
+ // If we get a new message setting the absolute position, then reset the
+ // localizer.
+ if (localizer_control_fetcher_.Fetch()) {
+ VLOG(1) << "localizer_control "
+ << aos::FlatbufferToJson(localizer_control_fetcher_.get());
+ localizer_->ResetPosition(
+ monotonic_now, localizer_control_fetcher_->x(),
+ localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
+ localizer_control_fetcher_->theta_uncertainty(),
+ !localizer_control_fetcher_->keep_current_theta());
+ }
+
+ kf_.set_index(ControllerIndexFromGears());
+
{
Eigen::Matrix<double, 4, 1> Y;
Y << position->left_encoder(), position->right_encoder(), last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
- localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
- monotonic_now, position->left_encoder(),
- position->right_encoder(),
- down_estimator_.avg_recent_yaw_rates(),
- down_estimator_.avg_recent_accel());
- // If we get a new message setting the absolute position, then reset the
- // localizer.
- if (localizer_control_fetcher_.Fetch()) {
- VLOG(1) << "localizer_control "
- << aos::FlatbufferToJson(localizer_control_fetcher_.get());
- localizer_->ResetPosition(
- monotonic_now, localizer_control_fetcher_->x(),
- localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
- localizer_control_fetcher_->theta_uncertainty(),
- !localizer_control_fetcher_->keep_current_theta());
+ }
+}
+
+Eigen::Matrix<double, 2, 1> DrivetrainFilters::VoltageError() const {
+ static_assert(kLeftError + 1 == kRightError);
+ Eigen::Matrix<double, 2, 2> error_K;
+ error_K << kf_.controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
+ kf_.controller().K(kRightVoltage, kRightError);
+ const Eigen::Matrix<double, 2, 1> voltage_error =
+ error_K * kf_.X_hat().block<2, 1>(kLeftError, 0);
+ return voltage_error;
+}
+
+void DrivetrainFilters::UpdateObserver(Eigen::Matrix<double, 2, 1> U) {
+ last_last_voltage_ = last_voltage_;
+
+ kf_.UpdateObserver(last_voltage_, dt_config_.dt);
+
+ last_voltage_ = U;
+}
+
+int DrivetrainFilters::ControllerIndexFromGears() const {
+ if (MaybeHigh(left_gear_)) {
+ if (MaybeHigh(right_gear_)) {
+ return 3;
+ } else {
+ return 2;
+ }
+ } else {
+ if (MaybeHigh(right_gear_)) {
+ return 1;
+ } else {
+ return 0;
}
}
+}
+flatbuffers::Offset<GearLogging> DrivetrainFilters::CreateGearLogging(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ GearLogging::Builder gear_logging_builder(*fbb);
+ gear_logging_builder.add_left_state(static_cast<uint32_t>(left_gear_));
+ gear_logging_builder.add_right_state(static_cast<uint32_t>(right_gear_));
+ gear_logging_builder.add_left_loop_high(MaybeHigh(left_gear_));
+ gear_logging_builder.add_right_loop_high(MaybeHigh(right_gear_));
+ gear_logging_builder.add_controller_index(ControllerIndexFromGears());
+ return gear_logging_builder.Finish();
+}
- dt_openloop_.SetPosition(position, left_gear_, right_gear_);
+Gear DrivetrainFilters::ComputeGear(
+ double shifter_position, const constants::ShifterHallEffect &shifter_config,
+ bool high_requested) const {
+ if (shifter_position < shifter_config.clear_low) {
+ return Gear::LOW;
+ } else if (shifter_position > shifter_config.clear_high) {
+ return Gear::HIGH;
+ } else {
+ if (high_requested) {
+ return Gear::SHIFTING_UP;
+ } else {
+ return Gear::SHIFTING_DOWN;
+ }
+ }
+}
+
+DrivetrainLoop::DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
+ const ::std::string &name)
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
+ dt_config_(dt_config),
+ filters_(dt_config, event_loop, localizer),
+ dt_openloop_(dt_config_, filters_.kf()),
+ dt_closedloop_(dt_config_, filters_.kf(), localizer),
+ dt_spline_(dt_config_),
+ dt_line_follow_(dt_config_, localizer->target_selector()) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void DrivetrainLoop::RunIteration(
+ const drivetrain::Goal *goal, const drivetrain::Position *position,
+ aos::Sender<drivetrain::Output>::Builder *output,
+ aos::Sender<drivetrain::Status>::Builder *status) {
+ const monotonic_clock::time_point monotonic_now =
+ event_loop()->monotonic_now();
+
+ if (!has_been_enabled_ && output) {
+ has_been_enabled_ = true;
+ }
+
+ if (WasReset()) {
+ filters_.Reset(monotonic_now, position);
+ }
+
+ filters_.Correct(monotonic_now, position);
+
+ // Set the gear-logging parts of the status
+ CHECK(status);
+ flatbuffers::Offset<GearLogging> gear_logging_offset =
+ filters_.CreateGearLogging(status->fbb());
+
+ dt_openloop_.SetPosition(position, filters_.left_gear(),
+ filters_.right_gear());
ControllerType controller_type = ControllerType::POLYDRIVE;
if (goal) {
@@ -291,21 +348,14 @@
controller_type == ControllerType::MOTION_PROFILE);
const Eigen::Matrix<double, 5, 1> trajectory_state =
- (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
- localizer_->theta(), localizer_->left_velocity(),
- localizer_->right_velocity())
- .finished();
+ filters_.trajectory_state();
{
// TODO(james): The regular Kalman Filter's voltage error terms are
// currently unusable--either don't use voltage error at all for the spline
// following code, or use the EKF's voltage error estimates.
- static_assert(kLeftError + 1 == kRightError);
- Eigen::Matrix<double, 2, 2> error_K;
- error_K << kf_.controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
- kf_.controller().K(kRightVoltage, kRightError);
const Eigen::Matrix<double, 2, 1> voltage_error =
- 0 * error_K * kf_.X_hat().block<2, 1>(kLeftError, 0);
+ 0 * filters_.VoltageError();
dt_spline_.Update(
output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
trajectory_state, voltage_error);
@@ -340,11 +390,11 @@
// set the output status of the control loop state
if (status) {
Eigen::Matrix<double, 2, 1> linear =
- dt_config_.LeftRightToLinear(kf_.X_hat());
+ dt_config_.LeftRightToLinear(filters_.DrivetrainXHat());
Eigen::Matrix<double, 2, 1> angular =
- dt_config_.LeftRightToAngular(kf_.X_hat());
+ dt_config_.LeftRightToAngular(filters_.DrivetrainXHat());
- angular(0, 0) = localizer_->theta();
+ angular(0, 0) = filters_.localizer_theta();
Eigen::Matrix<double, 4, 1> gyro_left_right =
dt_config_.AngularLinearToLeftRight(linear, angular);
@@ -356,20 +406,20 @@
dt_openloop_.PopulateStatus(status->fbb());
const flatbuffers::Offset<DownEstimatorState> down_estimator_state_offset =
- down_estimator_.PopulateStatus(status->fbb(), monotonic_now);
+ filters_.PopulateDownEstimatorState(status->fbb(), monotonic_now);
const flatbuffers::Offset<LocalizerState> localizer_offset =
- localizer_->PopulateStatus(status->fbb());
+ filters_.PopulateLocalizerState(status->fbb());
const flatbuffers::Offset<ImuZeroerState> zeroer_offset =
- imu_zeroer_.PopulateStatus(status->fbb());
+ filters_.PopulateImuZeroerState(status->fbb());
flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
dt_line_follow_.PopulateStatus(status);
flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
dt_spline_.MakeTrajectoryLogging(status);
- StatusBuilder builder = status->MakeBuilder<Status>();
+ Status::Builder builder = status->MakeBuilder<Status>();
dt_closedloop_.PopulateStatus(&builder);
@@ -382,21 +432,25 @@
if (dt_spline_.enable()) {
dt_spline_.PopulateStatus(&builder);
} else {
- builder.add_robot_speed(
- (kf_.X_hat(kLeftVelocity) + kf_.X_hat(kRightVelocity)) / 2.0);
+ builder.add_robot_speed((filters_.DrivetrainXHat(kLeftVelocity) +
+ filters_.DrivetrainXHat(kRightVelocity)) /
+ 2.0);
builder.add_output_was_capped(dt_closedloop_.output_was_capped());
- builder.add_uncapped_left_voltage(kf_.U_uncapped(kLeftVoltage));
- builder.add_uncapped_right_voltage(kf_.U_uncapped(kRightVoltage));
+ builder.add_uncapped_left_voltage(
+ filters_.DrivetrainUUncapped(kLeftVoltage));
+ builder.add_uncapped_right_voltage(
+ filters_.DrivetrainUUncapped(kRightVoltage));
}
- builder.add_left_voltage_error(kf_.X_hat(kLeftError));
- builder.add_right_voltage_error(kf_.X_hat(kRightError));
- builder.add_estimated_angular_velocity_error(kf_.X_hat(kAngularError));
- builder.add_estimated_heading(localizer_->theta());
+ builder.add_left_voltage_error(filters_.DrivetrainXHat(kLeftError));
+ builder.add_right_voltage_error(filters_.DrivetrainXHat(kRightError));
+ builder.add_estimated_angular_velocity_error(
+ filters_.DrivetrainXHat(kAngularError));
+ builder.add_estimated_heading(filters_.localizer_theta());
- builder.add_x(localizer_->x());
- builder.add_y(localizer_->y());
- builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
+ builder.add_x(filters_.x());
+ builder.add_y(filters_.y());
+ builder.add_theta(::aos::math::NormalizeAngle(filters_.localizer_theta()));
builder.add_cim_logging(cim_logging_offset);
builder.add_poly_drive_logging(poly_drive_logging_offset);
@@ -414,8 +468,8 @@
if (output) {
left_voltage = output_struct.left_voltage;
right_voltage = output_struct.right_voltage;
- left_high_requested_ = output_struct.left_high;
- right_high_requested_ = output_struct.right_high;
+ filters_.set_left_high_requested(output_struct.left_high);
+ filters_.set_right_high_requested(output_struct.right_high);
}
const double scalar = robot_state().voltage_battery() / 12.0;
@@ -431,16 +485,12 @@
// Gyro heading vs left-right
// Voltage error.
- last_last_left_voltage_ = last_left_voltage_;
- last_last_right_voltage_ = last_right_voltage_;
- Eigen::Matrix<double, 2, 1> U;
- U(kLeftVoltage) = last_left_voltage_;
- U(kRightVoltage) = last_right_voltage_;
- last_left_voltage_ = left_voltage;
- last_right_voltage_ = right_voltage;
-
- last_state_ = kf_.X_hat();
- kf_.UpdateObserver(U, dt_config_.dt);
+ {
+ Eigen::Matrix<double, 2, 1> U;
+ U(kLeftVoltage) = left_voltage;
+ U(kRightVoltage) = right_voltage;
+ filters_.UpdateObserver(U);
+ }
if (output) {
output->Send(Output::Pack(*output->fbb(), &output_struct));
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f282cc0..f979eb9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -29,6 +29,114 @@
namespace control_loops {
namespace drivetrain {
+namespace chrono = std::chrono;
+
+// A class to hold all the estimators in use in the drivetrain. This lets us
+// run them on a log file without running the controllers, making them easier to
+// tune.
+class DrivetrainFilters {
+ public:
+ DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer);
+
+ double localizer_theta() const { return localizer_->theta(); }
+ double x() const { return localizer_->x(); }
+ double y() const { return localizer_->y(); }
+
+ // Returns the current gear for both sides.
+ Gear left_gear() const { return left_gear_; }
+ Gear right_gear() const { return right_gear_; }
+
+ // Tracks the shift requests for each side.
+ void set_left_high_requested(bool value) { left_high_requested_ = value; }
+ void set_right_high_requested(bool value) { right_high_requested_ = value; }
+
+ // Returns a pointer to the drivetrain kalman filter.
+ StateFeedbackLoop<7, 2, 4> *kf() { return &kf_; }
+
+ // Populates the various state flatbuffers used for diagnostics.
+ flatbuffers::Offset<LocalizerState> PopulateLocalizerState(
+ flatbuffers::FlatBufferBuilder *fbb);
+ flatbuffers::Offset<ImuZeroerState> PopulateImuZeroerState(
+ flatbuffers::FlatBufferBuilder *fbb);
+ flatbuffers::Offset<DownEstimatorState> PopulateDownEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb,
+ aos::monotonic_clock::time_point monotonic_now);
+ flatbuffers::Offset<GearLogging> CreateGearLogging(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ // Returns the current localizer state.
+ Eigen::Matrix<double, 5, 1> trajectory_state() {
+ return (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
+ localizer_->theta(), localizer_->left_velocity(),
+ localizer_->right_velocity())
+ .finished();
+ }
+
+ // Resets all filters when wpilib_interface resets.
+ void Reset(aos::monotonic_clock::time_point monotonic_now,
+ const drivetrain::Position *position);
+
+ // Corrects all the filters.
+ void Correct(aos::monotonic_clock::time_point monotonic_now,
+ const drivetrain::Position *position);
+ // Runs the predict step for all filters. Should be called with the undelayed
+ // U.
+ void UpdateObserver(Eigen::Matrix<double, 2, 1> U);
+
+ // Returns the negative of the voltage error from the drivetrain controller.
+ // This can be used for integral control.
+ Eigen::Matrix<double, 2, 1> VoltageError() const;
+
+ // Returns the current drivetrain state.
+ Eigen::Matrix<double, 7, 1> DrivetrainXHat() const { return kf_.X_hat(); }
+ double DrivetrainXHat(int index) const { return kf_.X_hat(index); }
+
+ // Returns the current uncapped voltage from the kalman filter.
+ double DrivetrainUUncapped(int index) const { return kf_.U_uncapped(index); }
+
+ private:
+ // Returns the current controller index for the current gear.
+ int ControllerIndexFromGears() const;
+
+ // Computes which gear a shifter is in.
+ Gear ComputeGear(double shifter_position,
+ const constants::ShifterHallEffect &shifter_config,
+ bool high_requested) const;
+
+ const DrivetrainConfig<double> dt_config_;
+
+ aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
+ aos::Fetcher<frc971::IMUValuesBatch> imu_values_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+
+ zeroing::ImuZeroer imu_zeroer_;
+ DrivetrainUkf down_estimator_;
+ aos::monotonic_clock::time_point last_imu_update_ =
+ aos::monotonic_clock::min_time;
+ LocalizerInterface *localizer_;
+ StateFeedbackLoop<7, 2, 4> kf_;
+
+ // Current gears for each drive side.
+ Gear left_gear_;
+ Gear right_gear_;
+
+ // Shift request.
+ bool left_high_requested_;
+ bool right_high_requested_;
+
+ // Last acceleration and yaw rate.
+ aos::monotonic_clock::time_point last_gyro_time_ =
+ aos::monotonic_clock::min_time;
+ double last_accel_ = 0.0;
+ double last_gyro_rate_ = 0.0;
+
+ // Last applied voltage.
+ Eigen::Matrix<double, 2, 1> last_voltage_;
+ Eigen::Matrix<double, 2, 1> last_last_voltage_;
+};
+
class DrivetrainLoop
: public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
@@ -41,8 +149,6 @@
virtual ~DrivetrainLoop() {}
- int ControllerIndexFromGears();
-
protected:
// Executes one cycle of the control loop.
void RunIteration(
@@ -55,48 +161,15 @@
flatbuffers::Offset<drivetrain::Output> Zero(
aos::Sender<drivetrain::Output>::Builder *builder) override;
- double last_gyro_rate_ = 0.0;
-
const DrivetrainConfig<double> dt_config_;
+ DrivetrainFilters filters_;
- ::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
- ::aos::Fetcher<::frc971::IMUValuesBatch> imu_values_fetcher_;
- ::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
-
- zeroing::ImuZeroer imu_zeroer_;
- DrivetrainUkf down_estimator_;
- aos::monotonic_clock::time_point last_imu_update_ =
- aos::monotonic_clock::min_time;
- LocalizerInterface *localizer_;
-
- StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
SplineDrivetrain dt_spline_;
LineFollowDrivetrain dt_line_follow_;
- ::aos::monotonic_clock::time_point last_gyro_time_ =
- ::aos::monotonic_clock::min_time;
-
- // Current gears for each drive side.
- Gear left_gear_;
- Gear right_gear_;
-
- double last_left_voltage_ = 0;
- double last_right_voltage_ = 0;
- // The left/right voltages previous to last_*_voltage_.
- double last_last_left_voltage_ = 0;
- double last_last_right_voltage_ = 0;
-
- bool left_high_requested_;
- bool right_high_requested_;
bool has_been_enabled_ = false;
-
- double last_accel_ = 0.0;
-
- // Last kalman filter state.
- ::Eigen::Matrix<double, 7, 1> last_state_ =
- ::Eigen::Matrix<double, 7, 1>::Zero();
};
} // namespace drivetrain