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/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index bf527ec..5156a61 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -2,10 +2,7 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2016/control_loops/shooter/shooter_plant.h"
namespace y2016 {
@@ -49,80 +46,99 @@
loop_->Update(disabled);
}
-void ShooterSide::SetStatus(ShooterSideStatus *status) {
+flatbuffers::Offset<ShooterSideStatus> ShooterSide::SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ ShooterSideStatus::Builder shooter_side_status_builder(*fbb);
// Compute the oldest point in the history.
const int oldest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
// Compute the distance moved over that time period.
- status->avg_angular_velocity =
+ const double avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
(::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
+ shooter_side_status_builder.add_avg_angular_velocity(avg_angular_velocity);
- status->angular_velocity = loop_->X_hat(1, 0);
+ shooter_side_status_builder.add_angular_velocity(loop_->X_hat(1, 0));
// Ready if average angular velocity is close to the goal.
- status->ready = (std::abs(loop_->next_R(1, 0) -
- status->avg_angular_velocity) < kTolerance &&
- loop_->next_R(1, 0) > 1.0);
+ shooter_side_status_builder.add_ready(
+ (std::abs(loop_->next_R(1, 0) - avg_angular_velocity) < kTolerance &&
+ loop_->next_R(1, 0) > 1.0));
+
+ return shooter_side_status_builder.Finish();
}
Shooter::Shooter(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<ShooterQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shots_(0),
last_pre_shot_timeout_(::aos::monotonic_clock::min_time) {}
-void Shooter::RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) {
+void Shooter::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const ::aos::monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (goal) {
// Update position/goal for our two shooter sides.
- left_.set_goal(goal->angular_velocity);
- right_.set_goal(goal->angular_velocity);
-
- // Turn the lights on if we are supposed to spin.
- if (output) {
- if (::std::abs(goal->angular_velocity) > 0.0) {
- output->lights_on = true;
- if (goal->shooting_forwards) {
- output->forwards_flashlight = true;
- output->backwards_flashlight = false;
- } else {
- output->forwards_flashlight = false;
- output->backwards_flashlight = true;
- }
- }
- if (goal->force_lights_on) {
- output->lights_on = true;
- }
- }
+ left_.set_goal(goal->angular_velocity());
+ right_.set_goal(goal->angular_velocity());
}
- left_.set_position(position->theta_left);
- right_.set_position(position->theta_right);
+ left_.set_position(position->theta_left());
+ right_.set_position(position->theta_right());
left_.Update(output == nullptr);
right_.Update(output == nullptr);
- left_.SetStatus(&status->left);
- right_.SetStatus(&status->right);
- status->ready = (status->left.ready && status->right.ready);
+ flatbuffers::Offset<ShooterSideStatus> left_status_offset =
+ left_.SetStatus(status->fbb());
+ flatbuffers::Offset<ShooterSideStatus> right_status_offset =
+ right_.SetStatus(status->fbb());
+
+ ShooterSideStatus *left_status =
+ GetMutableTemporaryPointer(*status->fbb(), left_status_offset);
+ ShooterSideStatus *right_status =
+ GetMutableTemporaryPointer(*status->fbb(), right_status_offset);
+
+ const bool ready = (left_status->ready() && right_status->ready());
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_ready((left_status->ready() && right_status->ready()));
+ status_builder.add_left(left_status_offset);
+ status_builder.add_right(right_status_offset);
if (output) {
- output->voltage_left = left_.voltage();
- output->voltage_right = right_.voltage();
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage_left(left_.voltage());
+ output_builder.add_voltage_right(right_.voltage());
+ // Turn the lights on if we are supposed to spin.
if (goal) {
+ bool lights_on = false;
+ if (::std::abs(goal->angular_velocity()) > 0.0) {
+ lights_on = true;
+ if (goal->shooting_forwards()) {
+ output_builder.add_forwards_flashlight(true);
+ output_builder.add_backwards_flashlight(false);
+ } else {
+ output_builder.add_forwards_flashlight(false);
+ output_builder.add_backwards_flashlight(true);
+ }
+ }
+ if (goal->force_lights_on()) {
+ lights_on = true;
+ }
+ output_builder.add_lights_on(lights_on);
+
bool shoot = false;
switch (state_) {
case ShooterLatchState::PASS_THROUGH:
- if (goal->push_to_shooter) {
- if (::std::abs(goal->angular_velocity) > 10) {
- if (status->ready) {
+ if (goal->push_to_shooter()) {
+ if (::std::abs(goal->angular_velocity()) > 10) {
+ if (ready) {
state_ = ShooterLatchState::WAITING_FOR_SPINDOWN;
shoot = true;
}
@@ -134,22 +150,22 @@
break;
case ShooterLatchState::WAITING_FOR_SPINDOWN:
shoot = true;
- if (left_.velocity() < goal->angular_velocity * 0.9 ||
- right_.velocity() < goal->angular_velocity * 0.9) {
+ if (left_.velocity() < goal->angular_velocity() * 0.9 ||
+ right_.velocity() < goal->angular_velocity() * 0.9) {
state_ = ShooterLatchState::WAITING_FOR_SPINUP;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
break;
case ShooterLatchState::WAITING_FOR_SPINUP:
shoot = true;
- if (left_.velocity() > goal->angular_velocity * 0.95 &&
- right_.velocity() > goal->angular_velocity * 0.95) {
+ if (left_.velocity() > goal->angular_velocity() * 0.95 &&
+ right_.velocity() > goal->angular_velocity() * 0.95) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
@@ -160,18 +176,22 @@
break;
case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
shoot = true;
- if (!goal->push_to_shooter) {
+ if (!goal->push_to_shooter()) {
state_ = ShooterLatchState::PASS_THROUGH;
}
break;
}
- output->clamp_open = goal->clamp_open;
- output->push_to_shooter = shoot;
+ output_builder.add_clamp_open(goal->clamp_open());
+ output_builder.add_push_to_shooter(shoot);
}
+
+ output->Send(output_builder.Finish());
}
- status->shots = shots_;
+ status_builder.add_shots(shots_);
+
+ status->Send(status_builder.Finish());
}
} // namespace shooter