Convert gyro_reading over to event loops.
LPD8806 wasn't going to fit in well, but it's never been used on a
robot. So take the easy way out and remove it.
Change-Id: Ib4d29b40d63a8663878ff1b074057be8b2df9259
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e7cfae6..b45f016 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -20,7 +20,6 @@
#include "frc971/shifter_hall_effect.h"
#include "frc971/wpilib/imu.q.h"
-using frc971::sensors::gyro_reading;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -39,6 +38,9 @@
".frc971.control_loops.drivetrain.localizer_control")),
imu_values_fetcher_(
event_loop->MakeFetcher<::frc971::IMUValues>(".frc971.imu_values")),
+ gyro_reading_fetcher_(
+ event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
+ ".frc971.sensors.gyro_reading")),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -209,16 +211,16 @@
}
break;
case GyroType::SPARTAN_GYRO:
- if (gyro_reading.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
- last_gyro_rate_ = gyro_reading->velocity;
+ if (gyro_reading_fetcher_.Fetch()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ last_gyro_rate_ = gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_SPARTAN_GYRO:
- if (gyro_reading.FetchLatest()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
- last_gyro_rate_ = -gyro_reading->velocity;
+ if (gyro_reading_fetcher_.Fetch()) {
+ LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 39d7054..33ff770 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -15,6 +15,7 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/queues/gyro.q.h"
#include "frc971/wpilib/imu.q.h"
namespace frc971 {
@@ -49,6 +50,7 @@
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
+ ::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
LocalizerInterface *localizer_;
StateFeedbackLoop<7, 2, 4> kf_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 412b783..3f20554 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -57,7 +57,6 @@
localizer_(dt_config_),
drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
drivetrain_motor_plant_(&simulation_event_loop_, dt_config_) {
- ::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
}
@@ -142,7 +141,7 @@
} while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
}
- virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
+ virtual ~DrivetrainTest() {}
private:
::std::vector<double> actual_x_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 5df1666..a5cf6af 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -93,7 +93,9 @@
".frc971.control_loops.drivetrain_queue.position",
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
- gyro_reading_(::frc971::sensors::gyro_reading.name()),
+ gyro_reading_sender_(
+ event_loop->MakeSender<::frc971::sensors::GyroReading>(
+ ".frc971.sensors.gyro_reading")),
velocity_drivetrain_(
::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
StateFeedbackHybridPlant<2, 2, 2>,
@@ -131,8 +133,7 @@
}
{
- ::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
- gyro_reading_.MakeMessage();
+ auto gyro = gyro_reading_sender_.MakeMessage();
gyro->angle =
(right_encoder - left_encoder) / (dt_config_.robot_radius * 2.0);
gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 7914b50..cdde176 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -82,7 +82,7 @@
DrivetrainPlant drivetrain_plant_;
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
- ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
// This state is [x, y, theta, left_velocity, right_velocity].
::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
index db058b8..6749329 100644
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/replay_drivetrain.cc
@@ -26,7 +26,6 @@
replayer.ProcessFile(argv[i]);
}
}
- ::frc971::sensors::gyro_reading.Clear();
::frc971::control_loops::drivetrain_queue.goal.Clear();
::frc971::control_loops::drivetrain_queue.status.Clear();
::frc971::control_loops::drivetrain_queue.position.Clear();