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/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index a2dcaf6..70af274 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -2,14 +2,13 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
@@ -26,25 +25,23 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
+ event_loop->MakeSender<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ "/superstructure")),
drivetrain_output_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
-void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -55,7 +52,7 @@
}
const double clipped_box_distance =
- ::std::min(1.0, ::std::max(0.0, position->box_distance));
+ ::std::min(1.0, ::std::max(0.0, position->box_distance()));
const double box_velocity =
(clipped_box_distance - last_box_distance_) / 0.005;
@@ -64,31 +61,34 @@
filtered_box_velocity_ =
box_velocity * kFilteredBoxVelocityAlpha +
(1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
- status->filtered_box_velocity = filtered_box_velocity_;
constexpr double kCenteringAngleGain = 0.0;
const double left_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.left_intake_angle)) +
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->left_intake_angle())) +
last_intake_center_error_ * kCenteringAngleGain;
const double right_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.right_intake_angle)) -
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->right_intake_angle())) -
last_intake_center_error_ * kCenteringAngleGain;
- intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
- &(position->left_intake),
- output != nullptr ? &(output->left_intake) : nullptr,
- &(status->left_intake));
+ IntakeVoltageT left_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> left_status_offset =
+ intake_left_.Iterate(
+ unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
+ position->left_intake(),
+ output != nullptr ? &(left_intake_output) : nullptr, status->fbb());
- intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
- &(position->right_intake),
- output != nullptr ? &(output->right_intake) : nullptr,
- &(status->right_intake));
+ IntakeVoltageT right_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> right_status_offset =
+ intake_right_.Iterate(
+ unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
+ position->right_intake(),
+ output != nullptr ? &(right_intake_output) : nullptr, status->fbb());
const double intake_center_error =
intake_right_.output_position() - intake_left_.output_position();
@@ -97,63 +97,81 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
- bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw() : false;
if (unsafe_goal) {
- if (unsafe_goal->open_threshold != 0.0) {
- if (arm_.current_node() != unsafe_goal->arm_goal_position ||
- arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ if (unsafe_goal->open_threshold() != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position() ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold()) {
open_claw = false;
}
}
}
- arm_.Iterate(
- monotonic_now,
- unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
- unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
- &(position->arm), position->claw_beambreak_triggered,
- position->box_back_beambreak_triggered, intake_clear_of_box,
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
- unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
- output != nullptr ? &(output->voltage_proximal) : nullptr,
- output != nullptr ? &(output->voltage_distal) : nullptr,
- output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
+ const uint32_t arm_goal_position =
+ unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u;
+
+ double voltage_proximal_output = 0.0;
+ double voltage_distal_output = 0.0;
+ bool release_arm_brake_output = false;
+ bool claw_grabbed_output = false;
+ flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset =
+ arm_.Iterate(
+ monotonic_now,
+ unsafe_goal != nullptr ? &(arm_goal_position) : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box() : false, open_claw,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw() : false,
+ position->arm(), position->claw_beambreak_triggered(),
+ position->box_back_beambreak_triggered(), intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch() > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
+ output != nullptr ? &voltage_proximal_output : nullptr,
+ output != nullptr ? &voltage_distal_output : nullptr,
+ output != nullptr ? &release_arm_brake_output : nullptr,
+ output != nullptr ? &claw_grabbed_output : nullptr, status->fbb());
+
+
+ bool hook_release_output = false;
+ bool forks_release_output = false;
+ double voltage_winch_output = 0.0;
if (output) {
if (unsafe_goal) {
- output->hook_release = unsafe_goal->hook_release;
- output->voltage_winch = unsafe_goal->voltage_winch;
- output->forks_release = unsafe_goal->deploy_fork;
- } else {
- output->voltage_winch = 0.0;
- output->hook_release = false;
- output->forks_release = false;
+ hook_release_output = unsafe_goal->hook_release();
+ voltage_winch_output = unsafe_goal->voltage_winch();
+ forks_release_output = unsafe_goal->deploy_fork();
}
}
- status->estopped = status->left_intake.estopped ||
- status->right_intake.estopped || status->arm.estopped;
+ Status::Builder status_builder = status->MakeBuilder<Status>();
- status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
- status->arm.zeroed;
+ status_builder.add_left_intake(left_status_offset);
+ status_builder.add_right_intake(right_status_offset);
+ status_builder.add_arm(arm_status_offset);
+
+ status_builder.add_filtered_box_velocity(filtered_box_velocity_);
+ const bool estopped =
+ intake_left_.estopped() || intake_right_.estopped() || arm_.estopped();
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_zeroed(intake_left_.zeroed() && intake_right_.zeroed() &&
+ arm_.zeroed());
if (output && unsafe_goal) {
- double roller_voltage = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ double roller_voltage =
+ ::std::max(-kMaxIntakeRollerVoltage,
+ ::std::min(unsafe_goal->intake()->roller_voltage(),
+ kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 14;
- if (unsafe_goal->intake.roller_voltage < 0.0 ||
- unsafe_goal->disable_box_correct) {
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ if (unsafe_goal->intake()->roller_voltage() < 0.0 ||
+ unsafe_goal->disable_box_correct()) {
+ left_intake_output.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
stuck_count_ = 0;
} else {
- const bool stuck = position->box_distance < 0.20 &&
- filtered_box_velocity_ > -0.05 &&
- !position->box_back_beambreak_triggered;
+ const bool stuck = position->box_distance() < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered();
// Make sure we don't declare ourselves re-stuck too quickly. We want to
// wait 400 ms before triggering the stuck condition again.
if (!stuck) {
@@ -171,11 +189,11 @@
rotation_state_ = RotationState::STUCK;
++stuck_count_;
last_stuck_time_ = monotonic_now;
- } else if (position->left_intake.beam_break) {
+ } else if (position->left_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
- } else if (position->right_intake.beam_break) {
+ } else if (position->right_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
@@ -190,7 +208,7 @@
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->right_intake.beam_break) {
+ if (position->right_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -200,7 +218,7 @@
}
break;
case RotationState::ROTATING_RIGHT:
- if (position->left_intake.beam_break) {
+ if (position->left_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -214,7 +232,7 @@
constexpr double kHoldVoltage = 1.0;
constexpr double kStuckVoltage = 10.0;
- if (position->box_back_beambreak_triggered &&
+ if (position->box_back_beambreak_triggered() &&
roller_voltage > kHoldVoltage) {
roller_voltage = kHoldVoltage;
}
@@ -226,31 +244,31 @@
centering_gain = 0.0;
}
}
- output->left_intake.voltage_rollers =
+ left_intake_output.voltage_rollers =
roller_voltage - intake_center_error * centering_gain;
- output->right_intake.voltage_rollers =
+ right_intake_output.voltage_rollers =
roller_voltage + intake_center_error * centering_gain;
} break;
case RotationState::STUCK: {
if (roller_voltage > kHoldVoltage) {
- output->left_intake.voltage_rollers = -kStuckVoltage;
- output->right_intake.voltage_rollers = -kStuckVoltage;
+ left_intake_output.voltage_rollers = -kStuckVoltage;
+ right_intake_output.voltage_rollers = -kStuckVoltage;
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->left_intake.beam_break) {
- output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ if (position->left_intake()->beam_break()) {
+ left_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ left_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
- output->right_intake.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = roller_voltage;
- if (position->right_intake.beam_break) {
- output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ left_intake_output.voltage_rollers = roller_voltage;
+ if (position->right_intake()->beam_break()) {
+ right_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ right_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
break;
}
@@ -260,29 +278,31 @@
rotation_count_ = 0;
stuck_count_ = 0;
}
- status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ status_builder.add_rotation_state(static_cast<uint32_t>(rotation_state_));
drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
- if (status->estopped) {
+ if (estopped) {
SendColors(0.5, 0.0, 0.0);
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
SendColors(0.5, 0.5, 0.0);
} else if (rotation_state_ == RotationState::ROTATING_LEFT ||
rotation_state_ == RotationState::ROTATING_RIGHT) {
SendColors(0.5, 0.20, 0.0);
} else if (rotation_state_ == RotationState::STUCK) {
SendColors(0.5, 0.0, 0.5);
- } else if (position->box_back_beambreak_triggered) {
+ } else if (position->box_back_beambreak_triggered()) {
SendColors(0.0, 0.0, 0.5);
- } else if (position->box_distance < 0.2) {
+ } else if (position->box_distance() < 0.2) {
SendColors(0.0, 0.5, 0.0);
} else if (drivetrain_output_fetcher_.get() &&
- ::std::max(::std::abs(drivetrain_output_fetcher_->left_voltage),
- ::std::abs(drivetrain_output_fetcher_->right_voltage)) >
+ ::std::max(
+ ::std::abs(drivetrain_output_fetcher_->left_voltage()),
+ ::std::abs(drivetrain_output_fetcher_->right_voltage())) >
11.5) {
SendColors(0.5, 0.0, 0.5);
} else {
@@ -290,15 +310,41 @@
}
last_box_distance_ = clipped_box_distance;
+
+ if (output) {
+ flatbuffers::Offset<IntakeVoltage> left_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &left_intake_output);
+ flatbuffers::Offset<IntakeVoltage> right_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &right_intake_output);
+
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_left_intake(left_intake_offset);
+ output_builder.add_right_intake(right_intake_offset);
+ output_builder.add_voltage_proximal(voltage_proximal_output);
+ output_builder.add_voltage_distal(voltage_distal_output);
+ output_builder.add_release_arm_brake(release_arm_brake_output);
+ output_builder.add_claw_grabbed(claw_grabbed_output);
+
+ output_builder.add_hook_release(hook_release_output);
+ output_builder.add_forks_release(forks_release_output);
+ output_builder.add_voltage_winch(voltage_winch_output);
+
+ output->Send(output_builder.Finish());
+ }
+
+ status->Send(status_builder.Finish());
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
- if (!new_status_light.Send()) {
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}