Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 4f09da1..227d3ca 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -20,20 +20,20 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -41,24 +41,25 @@
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
#include "y2019/jevois/spi.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2019::control_loops::superstructure::SuperstructureQueue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
+namespace superstructure = ::y2019::control_loops::superstructure;
namespace chrono = ::std::chrono;
using aos::make_unique;
@@ -133,15 +134,14 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -233,79 +233,106 @@
void RunIteration() override {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ drivetrain_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed = -drivetrain_velocity_translate(
- drivetrain_right_encoder_->GetPeriod());
+ drivetrain_builder.add_right_encoder(
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ builder.Send(drivetrain_builder.Finish());
}
const auto values = constants::GetValues();
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
// Elevator
- CopyPosition(elevator_encoder_, &superstructure_message->elevator,
+ frc971::PotAndAbsolutePositionT elevator;
+ CopyPosition(elevator_encoder_, &elevator,
Values::kElevatorEncoderCountsPerRevolution(),
Values::kElevatorEncoderRatio(), elevator_pot_translate,
false, values.elevator.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &elevator);
+
// Intake
- CopyPosition(intake_encoder_, &superstructure_message->intake_joint,
+ frc971::AbsolutePositionT intake_joint;
+ CopyPosition(intake_encoder_, &intake_joint,
Values::kIntakeEncoderCountsPerRevolution(),
Values::kIntakeEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
// Wrist
- CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ frc971::PotAndAbsolutePositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist,
Values::kWristEncoderCountsPerRevolution(),
Values::kWristEncoderRatio(), wrist_pot_translate, false,
values.wrist.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &wrist);
// Stilts
- CopyPosition(stilts_encoder_, &superstructure_message->stilts,
+ frc971::PotAndAbsolutePositionT stilts;
+ CopyPosition(stilts_encoder_, &stilts,
Values::kStiltsEncoderCountsPerRevolution(),
Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
values.stilts.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &stilts);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_intake_joint(intake_joint_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_stilts(stilts_offset);
// Suction
constexpr float kMinVoltage = 0.5;
constexpr float kMaxVoltage = 2.1;
- superstructure_message->suction_pressure =
+ position_builder.add_suction_pressure(
(vacuum_sensor_->GetVoltage() - kMinVoltage) /
- (kMaxVoltage - kMinVoltage);
+ (kMaxVoltage - kMinVoltage));
- superstructure_message->platform_left_detect =
- !platform_left_detect_->Get();
- superstructure_message->platform_right_detect =
- !platform_right_detect_->Get();
+ position_builder.add_platform_left_detect(!platform_left_detect_->Get());
+ position_builder.add_platform_right_detect(
+ !platform_right_detect_->Get());
- superstructure_message.Send();
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+
+ uint32_t mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
+ mode |= 1 << i;
}
}
- AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
+
+ auto auto_mode_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+ auto_mode_builder.add_mode(mode);
+
+ builder.Send(auto_mode_builder.Finish());
}
}
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
@@ -319,7 +346,6 @@
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
- // TODO(sabina): Add wrist and elevator hall effects.
};
class CameraReader {
@@ -363,7 +389,7 @@
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
- } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log) {
+ } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log()) {
to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
@@ -388,25 +414,49 @@
return;
}
- const auto now = aos::monotonic_clock::now();
+ const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
for (const auto &received : unpacked->frames) {
- auto to_send = camera_frame_sender_.MakeMessage();
+ auto builder = camera_frame_sender_.MakeBuilder();
+
+ std::array<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>,
+ 3>
+ targets;
+
+ for (size_t i = 0; i < received.targets.size(); ++i) {
+ y2019::control_loops::drivetrain::CameraTarget::Builder
+ camera_target_builder = builder.MakeBuilder<
+ y2019::control_loops::drivetrain::CameraTarget>();
+
+ camera_target_builder.add_distance(received.targets[i].distance);
+ camera_target_builder.add_height(received.targets[i].height);
+ camera_target_builder.add_heading(received.targets[i].heading);
+ camera_target_builder.add_skew(received.targets[i].skew);
+
+ targets[i] = camera_target_builder.Finish();
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>>>
+ targets_offset = builder.fbb()->CreateVector(targets.begin(),
+ received.targets.size());
+
+ y2019::control_loops::drivetrain::CameraFrame::Builder
+ camera_frame_builder =
+ builder
+ .MakeBuilder<y2019::control_loops::drivetrain::CameraFrame>();
+
+ camera_frame_builder.add_targets(targets_offset);
+
// Add an extra 10ms delay to account for unmodeled delays that Austin
// thinks exists.
- to_send->timestamp =
+ camera_frame_builder.add_timestamp(
std::chrono::nanoseconds(
(now - received.age - ::std::chrono::milliseconds(10))
- .time_since_epoch()).count();
- to_send->num_targets = received.targets.size();
- for (size_t i = 0; i < received.targets.size(); ++i) {
- to_send->targets[i].distance = received.targets[i].distance;
- to_send->targets[i].height = received.targets[i].height;
- to_send->targets[i].heading = received.targets[i].heading;
- to_send->targets[i].skew = received.targets[i].skew;
- }
- to_send->camera = received.camera_index;
- AOS_LOG_STRUCT(DEBUG, "camera_frames", *to_send);
- to_send.Send();
+ .time_since_epoch())
+ .count());
+ camera_frame_builder.add_camera(received.camera_index);
+ builder.Send(camera_frame_builder.Finish());
}
if (dummy_spi_) {
@@ -458,14 +508,13 @@
};
class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
- event_loop,
- ".y2019.control_loops.superstructure.superstructure_queue.output"),
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure"),
robot_state_fetcher_(
- event_loop->MakeFetcher<::aos::RobotState>(".aos.robot_state")) {}
+ event_loop->MakeFetcher<::aos::RobotState>("/aos")) {}
void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
elevator_victor_ = ::std::move(t);
@@ -488,28 +537,27 @@
}
private:
- void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage,
+ void Write(const superstructure::Output &output) override {
+ elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage(),
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage,
+ intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage,
+ wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage,
+ stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
robot_state_fetcher_.Fetch();
const double battery_voltage = robot_state_fetcher_.get()
- ? robot_state_fetcher_->voltage_battery
+ ? robot_state_fetcher_->voltage_battery()
: 12.0;
// Throw a fast low pass filter on the battery voltage so we don't respond
@@ -518,7 +566,7 @@
0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
suction_victor_->SetSpeed(::aos::Clip(
- output.pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
+ output.pump_voltage() / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -543,11 +591,12 @@
public:
SolenoidWriter(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- superstructure_fetcher_(event_loop->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
- status_light_fetcher_(event_loop->MakeFetcher<::y2019::StatusLight>(
- ".y2019.status_light")) {
+ superstructure_fetcher_(
+ event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
+ status_light_fetcher_(
+ event_loop->MakeFetcher<::y2019::StatusLight>("/superstructure")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
@@ -580,36 +629,40 @@
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- big_suction_cup0_->Set(!superstructure_fetcher_->intake_suction_bottom);
- big_suction_cup1_->Set(!superstructure_fetcher_->intake_suction_bottom);
- small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
- small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
+ big_suction_cup0_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ big_suction_cup1_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top());
+ small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top());
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
+ ::aos::Clip(superstructure_fetcher_->intake_roller_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
pcm_.Flush();
- to_log.read_solenoids = pcm_.GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_.GetAll());
+ builder.Send(to_log_builder.Finish());
}
status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
+ StatusLightT color;
if (!status_light_fetcher_.get() ||
- status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
+ status_light_fetcher_.context().monotonic_sent_time +
+ chrono::milliseconds(100) <
event_loop_->monotonic_now()) {
- StatusLight color;
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
@@ -623,15 +676,13 @@
light_flash_ = 0;
}
- AOS_LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
} else {
- AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
- SetColor(*status_light_fetcher_.get());
+ status_light_fetcher_->UnPackTo(&color);
}
+ SetColor(color);
}
- void SetColor(const StatusLight &status_light) {
+ void SetColor(const StatusLightT status_light) {
// Save CAN bandwidth and CPU at the cost of RT. Only change the light when
// it actually changes. This is pretty low priority anyways.
static int time_since_last_send = 0;
@@ -669,11 +720,12 @@
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Output>
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Output>
superstructure_fetcher_;
::aos::Fetcher<::y2019::StatusLight> status_light_fetcher_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
+
::ctre::phoenix::CANifier canifier_{0};
double last_red_ = -1.0;
@@ -691,19 +743,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
@@ -734,7 +789,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
CameraReader camera_reader(&imu_event_loop);
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
@@ -757,7 +812,7 @@
// variety so all the Victors are written as SPs.
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
@@ -779,7 +834,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));