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();
diff --git a/frc971/queues/gyro.q b/frc971/queues/gyro.q
index c86fcab..0885738 100644
--- a/frc971/queues/gyro.q
+++ b/frc971/queues/gyro.q
@@ -1,5 +1,6 @@
package frc971.sensors;
+// Published on ".frc971.sensors.gyro_reading"
message GyroReading {
// Positive is counter-clockwise (Austin says "it's Positive").
// Right-hand coordinate system around the Z-axis going up.
@@ -8,7 +9,6 @@
// The angular velocity in radians/sec
double velocity;
};
-queue GyroReading gyro_reading;
// Published on ".frc971.sensors.gyro_part_id"
message Uid {
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 2ef83ce..c6e72a4 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -119,23 +119,6 @@
)
cc_library(
- name = "lpd8806",
- srcs = [
- "LPD8806.cc",
- ],
- hdrs = [
- "LPD8806.h",
- ],
- restricted_to = ["//tools:roborio"],
- deps = [
- "//aos/mutex",
- "//aos/time",
- "//frc971/queues:gyro",
- "//third_party:wpilib",
- ],
-)
-
-cc_library(
name = "loop_output_handler",
srcs = [
"loop_output_handler.cc",
diff --git a/frc971/wpilib/LPD8806.cc b/frc971/wpilib/LPD8806.cc
deleted file mode 100644
index 6db54d5..0000000
--- a/frc971/wpilib/LPD8806.cc
+++ /dev/null
@@ -1,87 +0,0 @@
-#include "frc971/wpilib/LPD8806.h"
-
-#include "frc971/queues/gyro.q.h"
-
-#include "frc971/wpilib/ahal/SPI.h"
-#undef ERROR
-
-namespace frc971 {
-namespace wpilib {
-
-LPD8806::LPD8806(int chips)
- : chips_(chips),
- data_(new LED[chips * 2]),
- spi_(new frc::SPI(frc::SPI::kOnboardCS1)) {
- memset(data_.get(), 0, sizeof(LED[chips_ * 2]));
-
- // 2 MHz is the maximum frequency the datasheet recommends.
- spi_->SetClockRate(2e6);
- spi_->SetChipSelectActiveHigh();
-
- // Clock is inverted due to the level translator chip.
- spi_->SetClockActiveLow();
- spi_->SetSampleDataOnRising();
- spi_->SetMSBFirst();
-}
-
-void LPD8806::SetColor(int led, uint32_t hex_color) {
- CHECK_LT(led, chips_ * 2);
- ::aos::MutexLocker locker(&data_mutex_);
- data_[led].red = TranslateColor(hex_color, RED);
- data_[led].green = TranslateColor(hex_color, GREEN);
- data_[led].blue = TranslateColor(hex_color, BLUE);
-}
-
-uint8_t LPD8806::TranslateColor(uint32_t hex_color, Type type) {
- switch (type) {
- case RED:
- return ((hex_color >> 16) & 0xFF) | 0x01;
- case GREEN:
- return ((hex_color >> 8) & 0xFF) | 0x01;
- case BLUE:
- return (hex_color & 0xFF) | 0x01;
- }
-
- LOG(FATAL, "Not sure what color type %d is\n", static_cast<int>(type));
-}
-
-void LPD8806::operator()() {
- // The buffer we're going to send.
- // With 64 leds, it takes about 1ms to send them all, which fits within the
- // 5ms cycle used by the gyro_reader.
- LED buffer[64];
-
- while (run_) {
- if (next_led_to_send_ < chips_ * 2) {
- ::aos::MutexLocker locker(&data_mutex_);
- memcpy(buffer, &data_[next_led_to_send_], sizeof(buffer));
- next_led_to_send_ += 64;
- } else {
- CHECK_EQ(chips_ * 2, next_led_to_send_);
- next_led_to_send_ = 0;
- memset(buffer, 0, sizeof(buffer));
- }
-
- // Wait until right after the gyro gets a reading.
- ::frc971::sensors::gyro_reading.FetchNextBlocking();
-
- int spi_send_result = (spi_->Write(
- static_cast<uint8_t *>(static_cast<void *>(buffer)), sizeof(buffer)));
-
- switch (spi_send_result) {
- case -1:
- LOG(INFO, "SPI::Write failed\n");
- // Who knows what state it's in now, so start fresh next cycle.
- next_led_to_send_ = chips_ * 2;
- break;
- case sizeof(buffer):
- break;
- default:
- LOG(FATAL, "SPI::Write returned something weird: %d\n",
- static_cast<int>(spi_send_result));
- }
- }
-}
-
-} // namespace wpilib
-} // namespace frc971
diff --git a/frc971/wpilib/LPD8806.h b/frc971/wpilib/LPD8806.h
deleted file mode 100644
index 697f180..0000000
--- a/frc971/wpilib/LPD8806.h
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef FRC971_WPILIB_LPD8806_H_
-#define FRC971_WPILIB_LPD8806_H_
-
-#include <atomic>
-#include <memory>
-
-#include "aos/mutex/mutex.h"
-
-#include "frc971/wpilib/ahal/SPI.h"
-#undef ERROR
-
-namespace frc971 {
-namespace wpilib {
-
-// Handles sending out colors to a string of LPD8806 LED drivers with a chip to
-// gate the clock+data lines attached to SPI CS1.
-//
-// This is currently implemented by sending data for an entire 64 LED strip
-// right after receiving a gyro message to avoid interfering with the SPI
-// transactions for the gyro.
-//
-// This is designed to be passed into ::std::thread's constructor so it will run
-// as a separate thread.
-class LPD8806 {
- public:
- // chips is the number of actual chips in the string. There will be twice this
- // many LEDs.
- LPD8806(int chips);
-
- // For ::std::thread to call.
- //
- // Loops until Quit() is called writing values out.
- void operator()();
-
- void Quit() { run_ = false; }
-
- int chips() const { return chips_; }
-
- // Sets the color to be sent out next refresh cycle for a given LED.
- // red, green, and blue are 0-1 (inclusive).
- void SetColor(int led, uint32_t hex_color);
-
- private:
- struct LED {
- uint8_t green;
- uint8_t red;
- uint8_t blue;
- };
-
- enum Type { GREEN, RED, BLUE };
-
- static_assert(sizeof(LED[4]) == 12, "LED needs to pack into arrays nicely");
-
- uint8_t TranslateColor(uint32_t hex_color, Type type);
-
- const int chips_;
- int next_led_to_send_ = 0;
-
- ::std::unique_ptr<LED[]> data_;
- ::aos::Mutex data_mutex_;
-
- ::std::unique_ptr<frc::SPI> spi_;
-
- ::std::atomic<bool> run_{true};
-};
-
-} // namespace wpilib
-} // namespace frc971
-
-#endif // FRC971_WPILIB_LPD8806_H_
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 925588e..894e187 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -29,7 +29,10 @@
joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
".aos.joystick_state")),
uid_sender_(event_loop_->MakeSender<::frc971::sensors::Uid>(
- ".frc971.sensors.gyro_part_id")) {
+ ".frc971.sensors.gyro_part_id")),
+ gyro_reading_sender_(
+ event_loop_->MakeSender<::frc971::sensors::GyroReading>(
+ ".frc971.sensors.gyro_reading")) {
PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
@@ -119,7 +122,7 @@
const double angle_rate = gyro_.ExtractAngle(result);
const double new_angle = angle_rate / static_cast<double>(kReadingRate);
- auto message = ::frc971::sensors::gyro_reading.MakeMessage();
+ auto message = gyro_reading_sender_.MakeMessage();
if (zeroed) {
angle += (new_angle + zero_offset) * number_readings;
message->angle = angle;
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 61b7704..338ad32 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -32,6 +32,7 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
::aos::Sender<::frc971::sensors::Uid> uid_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
// Readings per second.
static const int kReadingRate = 200;
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 6680e8c..a8f0e7f 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -21,7 +21,6 @@
#include "y2014/control_loops/shooter/shooter.q.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 847adf7..7b6d950 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -19,7 +19,6 @@
#include "y2014_bot3/control_loops/rollers/rollers.q.h"
using ::frc971::control_loops::drivetrain_queue;
-using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::POVLocation;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 3052b5b..9137402 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -59,7 +59,6 @@
// obstacle, it ends up confusing the localizer; as such, starting there
// is just prone to causing confusion.
SetStartingPosition({3.0, 2.0, 0.0});
- ::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
}
@@ -165,9 +164,7 @@
}
}
- virtual ~LocalizedDrivetrainTest() {
- ::frc971::sensors::gyro_reading.Clear();
- }
+ virtual ~LocalizedDrivetrainTest() {}
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
dt_config_;