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/actors/BUILD b/y2016/actors/BUILD
index f5b2566..f80128d 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
filegroup(
name = "binaries",
@@ -10,14 +10,13 @@
],
)
-queue_library(
- name = "superstructure_action_queue",
+flatbuffer_cc_library(
+ name = "superstructure_action_fbs",
srcs = [
- "superstructure_action.q",
+ "superstructure_action.fbs",
],
- deps = [
- "//aos/actions:action_queue",
- ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -29,12 +28,14 @@
"superstructure_actor.h",
],
deps = [
- ":superstructure_action_queue",
+ ":superstructure_action_fbs",
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
- "//y2016/control_loops/superstructure:superstructure_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
],
)
@@ -44,10 +45,10 @@
"superstructure_actor_main.cc",
],
deps = [
+ ":superstructure_action_fbs",
":superstructure_action_lib",
- ":superstructure_action_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -65,13 +66,13 @@
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2016/control_loops/drivetrain:drivetrain_base",
- "//y2016/control_loops/shooter:shooter_queue",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
- "//y2016/queues:profile_params",
- "//y2016/vision:vision_queue",
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/shooter:shooter_status_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2016/vision:vision_fbs",
],
)
@@ -86,14 +87,13 @@
],
)
-queue_library(
- name = "vision_align_action_queue",
+flatbuffer_cc_library(
+ name = "vision_align_action_fbs",
srcs = [
- "vision_align_action.q",
+ "vision_align_action.fbs",
],
- deps = [
- "//aos/actions:action_queue",
- ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -105,19 +105,19 @@
"vision_align_actor.h",
],
deps = [
- ":vision_align_action_queue",
+ ":vision_align_action_fbs",
"//aos:math",
"//aos/actions:action_lib",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
"//aos/util:phased_loop",
"//aos/util:trapezoid_profile",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//third_party/eigen",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:drivetrain_base",
- "//y2016/vision:vision_queue",
+ "//y2016/vision:vision_fbs",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -127,8 +127,8 @@
"vision_align_actor_main.cc",
],
deps = [
+ ":vision_align_action_fbs",
":vision_align_action_lib",
- ":vision_align_action_queue",
"//aos:init",
],
)
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 498a6f1..a665c28 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -8,39 +8,52 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
using ::aos::monotonic_clock;
+using ::frc971::ProfileParametersT;
+namespace superstructure = y2016::control_loops::superstructure;
+namespace shooter = y2016::control_loops::shooter;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-const ProfileParameters kSlowDrive = {0.8, 2.5};
-const ProfileParameters kLowBarDrive = {1.3, 2.5};
-const ProfileParameters kMoatDrive = {1.2, 3.5};
-const ProfileParameters kRealignDrive = {2.0, 2.5};
-const ProfileParameters kRockWallDrive = {0.8, 2.5};
-const ProfileParameters kFastDrive = {3.0, 2.5};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
-const ProfileParameters kSlowTurn = {0.8, 3.0};
-const ProfileParameters kFastTurn = {3.0, 10.0};
-const ProfileParameters kStealTurn = {4.0, 15.0};
-const ProfileParameters kSwerveTurn = {2.0, 7.0};
-const ProfileParameters kFinishTurn = {2.0, 5.0};
+const ProfileParametersT kSlowDrive = MakeProfileParameters(0.8, 2.5);
+const ProfileParametersT kLowBarDrive = MakeProfileParameters(1.3, 2.5);
+const ProfileParametersT kMoatDrive = MakeProfileParameters(1.2, 3.5);
+const ProfileParametersT kRealignDrive = MakeProfileParameters(2.0, 2.5);
+const ProfileParametersT kRockWallDrive = MakeProfileParameters(0.8, 2.5);
+const ProfileParametersT kFastDrive = MakeProfileParameters(3.0, 2.5);
-const ProfileParameters kTwoBallLowDrive = {1.7, 3.5};
-const ProfileParameters kTwoBallFastDrive = {3.0, 1.5};
-const ProfileParameters kTwoBallReturnDrive = {3.0, 1.9};
-const ProfileParameters kTwoBallReturnSlow = {3.0, 2.5};
-const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};
-const ProfileParameters kTwoBallBallPickupAccel = {2.0, 2.5};
+const ProfileParametersT kSlowTurn = MakeProfileParameters(0.8, 3.0);
+const ProfileParametersT kFastTurn = MakeProfileParameters(3.0, 10.0);
+const ProfileParametersT kStealTurn = MakeProfileParameters(4.0, 15.0);
+const ProfileParametersT kSwerveTurn = MakeProfileParameters(2.0, 7.0);
+const ProfileParametersT kFinishTurn = MakeProfileParameters(2.0, 5.0);
+
+const ProfileParametersT kTwoBallLowDrive = MakeProfileParameters(1.7, 3.5);
+const ProfileParametersT kTwoBallFastDrive = MakeProfileParameters(3.0, 1.5);
+const ProfileParametersT kTwoBallReturnDrive = MakeProfileParameters(3.0, 1.9);
+const ProfileParametersT kTwoBallReturnSlow = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kTwoBallBallPickup = MakeProfileParameters(2.0, 1.75);
+const ProfileParametersT kTwoBallBallPickupAccel =
+ MakeProfileParameters(2.0, 2.5);
} // namespace
@@ -51,66 +64,66 @@
actors::VisionAlignActor::MakeFactory(event_loop)),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
shooter_goal_sender_(
- event_loop
- ->MakeSender<::y2016::control_loops::shooter::ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ event_loop->MakeSender<::y2016::control_loops::shooter::Goal>(
+ "/shooter")),
shooter_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::shooter::ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")),
+ event_loop->MakeFetcher<::y2016::control_loops::shooter::Status>(
+ "/shooter")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
- superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")) {}
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
constexpr double kDoNotTurnCare = 2.0;
void AutonomousActor::MoveSuperstructure(
double intake, double shoulder, double wrist,
- const ProfileParameters intake_params,
- const ProfileParameters shoulder_params,
- const ProfileParameters wrist_params, bool traverse_up,
+ const ProfileParametersT intake_params,
+ const ProfileParametersT shoulder_params,
+ const ProfileParametersT wrist_params, bool traverse_up,
double roller_power) {
superstructure_goal_ = {intake, shoulder, wrist};
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->angle_intake = intake;
- new_superstructure_goal->angle_shoulder = shoulder;
- new_superstructure_goal->angle_wrist = wrist;
+ superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
- new_superstructure_goal->max_angular_velocity_intake =
- intake_params.max_velocity;
- new_superstructure_goal->max_angular_velocity_shoulder =
- shoulder_params.max_velocity;
- new_superstructure_goal->max_angular_velocity_wrist =
- wrist_params.max_velocity;
+ superstructure_goal_builder.add_angle_intake(intake);
+ superstructure_goal_builder.add_angle_shoulder(shoulder);
+ superstructure_goal_builder.add_angle_wrist(wrist);
- new_superstructure_goal->max_angular_acceleration_intake =
- intake_params.max_acceleration;
- new_superstructure_goal->max_angular_acceleration_shoulder =
- shoulder_params.max_acceleration;
- new_superstructure_goal->max_angular_acceleration_wrist =
- wrist_params.max_acceleration;
+ superstructure_goal_builder.add_max_angular_velocity_intake(
+ intake_params.max_velocity);
+ superstructure_goal_builder.add_max_angular_velocity_shoulder(
+ shoulder_params.max_velocity);
+ superstructure_goal_builder.add_max_angular_velocity_wrist(
+ wrist_params.max_velocity);
- new_superstructure_goal->voltage_top_rollers = roller_power;
- new_superstructure_goal->voltage_bottom_rollers = roller_power;
+ superstructure_goal_builder.add_max_angular_acceleration_intake(
+ intake_params.max_acceleration);
+ superstructure_goal_builder.add_max_angular_acceleration_shoulder(
+ shoulder_params.max_acceleration);
+ superstructure_goal_builder.add_max_angular_acceleration_wrist(
+ wrist_params.max_acceleration);
- new_superstructure_goal->traverse_unlatched = true;
- new_superstructure_goal->traverse_down = !traverse_up;
- new_superstructure_goal->voltage_climber = 0.0;
- new_superstructure_goal->unfold_climber = false;
+ superstructure_goal_builder.add_voltage_top_rollers(roller_power);
+ superstructure_goal_builder.add_voltage_bottom_rollers(roller_power);
- if (!new_superstructure_goal.Send()) {
+ superstructure_goal_builder.add_traverse_unlatched(true);
+ superstructure_goal_builder.add_traverse_down(!traverse_up);
+ superstructure_goal_builder.add_voltage_climber(0.0);
+ superstructure_goal_builder.add_unfold_climber(false);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -118,12 +131,14 @@
void AutonomousActor::OpenShooter() {
shooter_speed_ = 0.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = true;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = false;
- if (!shooter_goal.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(true);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(false);
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -131,13 +146,15 @@
void AutonomousActor::CloseShooter() {
shooter_speed_ = 0.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = false;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(false);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -149,13 +166,15 @@
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = false;
- shooter_goal->force_lights_on = force_lights_on;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(false);
+ shooter_goal_builder.add_force_lights_on(force_lights_on);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
}
@@ -165,20 +184,22 @@
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- initial_shots = shooter_status_fetcher_->shots;
+ initial_shots = shooter_status_fetcher_->shots();
}
// In auto, we want to have the lights on whenever possible since we have no
// hope of a human aligning the robot.
bool force_lights_on = shooter_speed_ > 1.0;
- auto shooter_goal = shooter_goal_sender_.MakeMessage();
- shooter_goal->angular_velocity = shooter_speed_;
- shooter_goal->clamp_open = false;
- shooter_goal->push_to_shooter = true;
- shooter_goal->force_lights_on = force_lights_on;
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ shooter::Goal::Builder shooter_goal_builder =
+ builder.MakeBuilder<shooter::Goal>();
+ shooter_goal_builder.add_angular_velocity(shooter_speed_);
+ shooter_goal_builder.add_clamp_open(false);
+ shooter_goal_builder.add_push_to_shooter(true);
+ shooter_goal_builder.add_force_lights_on(force_lights_on);
- if (!shooter_goal.Send()) {
+ if (!builder.Send(shooter_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
@@ -191,7 +212,7 @@
// Wait for the shot count to change so we know when the shot is complete.
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- if (initial_shots < shooter_status_fetcher_->shots) {
+ if (initial_shots < shooter_status_fetcher_->shots()) {
return;
}
}
@@ -208,8 +229,8 @@
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get()) {
- if (shooter_status_fetcher_->left.ready &&
- shooter_status_fetcher_->right.ready) {
+ if (shooter_status_fetcher_->left()->ready() &&
+ shooter_status_fetcher_->right()->ready()) {
return;
}
}
@@ -218,7 +239,7 @@
}
void AutonomousActor::AlignWithVisionGoal() {
- actors::VisionAlignActionParams params;
+ vision_align_action::VisionAlignActionParamsT params;
vision_action_ = vision_align_actor_factory_.Make(params);
vision_action_->Start();
}
@@ -238,25 +259,25 @@
vision_status_fetcher_.Fetch();
if (vision_status_fetcher_.get()) {
- vision_valid = (vision_status_fetcher_->left_image_valid &&
- vision_status_fetcher_->right_image_valid);
- last_angle = vision_status_fetcher_->horizontal_angle;
+ vision_valid = (vision_status_fetcher_->left_image_valid() &&
+ vision_status_fetcher_->right_image_valid());
+ last_angle = vision_status_fetcher_->horizontal_angle();
}
drivetrain_status_fetcher_.Fetch();
drivetrain_goal_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get() && drivetrain_goal_fetcher_.get()) {
- const double left_goal = drivetrain_goal_fetcher_->left_goal;
- const double right_goal = drivetrain_goal_fetcher_->right_goal;
+ const double left_goal = drivetrain_goal_fetcher_->left_goal();
+ const double right_goal = drivetrain_goal_fetcher_->right_goal();
const double left_current =
- drivetrain_status_fetcher_->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position();
const double right_current =
- drivetrain_status_fetcher_->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position();
const double left_velocity =
- drivetrain_status_fetcher_->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity();
const double right_velocity =
- drivetrain_status_fetcher_->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity();
if (vision_valid && ::std::abs(last_angle) < 0.02 &&
::std::abs((left_goal - right_goal) -
@@ -287,21 +308,23 @@
constexpr double kProfileError = 1e-5;
constexpr double kEpsilon = 0.15;
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
- if (::std::abs(superstructure_status_fetcher_->intake.goal_angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
superstructure_goal_.intake) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->intake
- .goal_angular_velocity) < kProfileError) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->goal_angular_velocity()) <
+ kProfileError) {
AOS_LOG(DEBUG, "Profile done.\n");
- if (::std::abs(superstructure_status_fetcher_->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
superstructure_goal_.intake) < kEpsilon &&
- ::std::abs(superstructure_status_fetcher_->intake
- .angular_velocity) < kEpsilon) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->angular_velocity()) <
+ kEpsilon) {
AOS_LOG(INFO, "Near goal, done.\n");
return true;
}
@@ -310,28 +333,26 @@
}
bool AutonomousActor::SuperstructureProfileDone() {
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
constexpr double kProfileError = 1e-5;
- return ::std::abs(
- superstructure_status_fetcher_->intake.goal_angle -
- superstructure_goal_.intake) < kProfileError &&
+ return ::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
+ superstructure_goal_.intake) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()->goal_angle() -
+ superstructure_goal_.shoulder) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->wrist()->goal_angle() -
+ superstructure_goal_.wrist) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->intake()
+ ->goal_angular_velocity()) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()
+ ->goal_angular_velocity()) < kProfileError &&
::std::abs(
- superstructure_status_fetcher_->shoulder.goal_angle -
- superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->wrist.goal_angle -
- superstructure_goal_.wrist) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->intake
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->shoulder
- .goal_angular_velocity) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->wrist
- .goal_angular_velocity) < kProfileError;
+ superstructure_status_fetcher_->wrist()->goal_angular_velocity()) <
+ kProfileError;
}
bool AutonomousActor::SuperstructureDone() {
@@ -340,18 +361,21 @@
constexpr double kEpsilon = 0.03;
if (SuperstructureProfileDone()) {
AOS_LOG(DEBUG, "Profile done.\n");
- if (::std::abs(superstructure_status_fetcher_->intake.angle -
+ if (::std::abs(superstructure_status_fetcher_->intake()->angle() -
superstructure_goal_.intake) < (kEpsilon + 0.1) &&
- ::std::abs(superstructure_status_fetcher_->shoulder.angle -
+ ::std::abs(superstructure_status_fetcher_->shoulder()->angle() -
superstructure_goal_.shoulder) < (kEpsilon + 0.05) &&
- ::std::abs(superstructure_status_fetcher_->wrist.angle -
+ ::std::abs(superstructure_status_fetcher_->wrist()->angle() -
superstructure_goal_.wrist) < (kEpsilon + 0.01) &&
- ::std::abs(superstructure_status_fetcher_->intake
- .angular_velocity) < (kEpsilon + 0.1) &&
- ::std::abs(superstructure_status_fetcher_->shoulder
- .angular_velocity) < (kEpsilon + 0.10) &&
- ::std::abs(superstructure_status_fetcher_->wrist
- .angular_velocity) < (kEpsilon + 0.05)) {
+ ::std::abs(
+ superstructure_status_fetcher_->intake()->angular_velocity()) <
+ (kEpsilon + 0.1) &&
+ ::std::abs(
+ superstructure_status_fetcher_->shoulder()->angular_velocity()) <
+ (kEpsilon + 0.10) &&
+ ::std::abs(
+ superstructure_status_fetcher_->wrist()->angular_velocity()) <
+ (kEpsilon + 0.05)) {
AOS_LOG(INFO, "Near goal, done.\n");
return true;
}
@@ -379,55 +403,71 @@
superstructure_status_fetcher_.Fetch();
return SuperstructureProfileDone() ||
- superstructure_status_fetcher_->shoulder.angle < 0.1;
+ superstructure_status_fetcher_->shoulder()->angle() < 0.1;
});
}
void AutonomousActor::BackLongShotLowBarTwoBall() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShotTwoBall() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.55,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShotTwoBallFinish() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03, {7.0, 40.0},
- {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.00, M_PI / 2.0 - 0.2, -0.625 + 0.03,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackLongShot() {
AOS_LOG(INFO, "Expanding for back long shot\n");
- MoveSuperstructure(0.80, M_PI / 2.0 - 0.2, -0.62, {7.0, 40.0}, {4.0, 6.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.80, M_PI / 2.0 - 0.2, -0.62,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::BackMiddleShot() {
AOS_LOG(INFO, "Expanding for back middle shot\n");
- MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665, {7.0, 40.0}, {4.0, 10.0},
- {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(-0.05, M_PI / 2.0 - 0.2, -0.665,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::FrontLongShot() {
AOS_LOG(INFO, "Expanding for front long shot\n");
- MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02, {7.0, 40.0},
- {4.0, 6.0}, {10.0, 25.0}, false, 0.0);
+ MoveSuperstructure(0.80, M_PI / 2.0 + 0.1, M_PI + 0.41 + 0.02,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 6.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
}
void AutonomousActor::FrontMiddleShot() {
AOS_LOG(INFO, "Expanding for front middle shot\n");
- MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.44, {7.0, 40.0},
- {4.0, 10.0}, {10.0, 25.0}, true, 0.0);
+ MoveSuperstructure(-0.05, M_PI / 2.0 + 0.1, M_PI + 0.44,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 0.0);
}
void AutonomousActor::TuckArm(bool low_bar, bool traverse_down) {
- MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0, {7.0, 40.0},
- {4.0, 10.0}, {10.0, 25.0}, !traverse_down, 0.0);
+ MoveSuperstructure(low_bar ? -0.05 : 2.0, -0.010, 0.0,
+ MakeProfileParameters(7.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), !traverse_down, 0.0);
}
void AutonomousActor::DoFullShot() {
@@ -498,10 +538,10 @@
if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double distance_to_go = (left_error + right_error) / 2.0;
const double distance_compensation =
goal_distance - tip_distance - distance_to_go;
@@ -540,7 +580,7 @@
void AutonomousActor::CloseIfBall() {
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (ball_detected) {
CloseShooter();
}
@@ -563,7 +603,7 @@
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (ball_detected) {
return;
}
@@ -574,8 +614,9 @@
void AutonomousActor::TwoBallAuto() {
const monotonic_clock::time_point start_time = monotonic_now();
OpenShooter();
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
if (ShouldCancel()) return;
AOS_LOG(INFO, "Waiting for the intake to come down.\n");
@@ -593,19 +634,21 @@
bool first_ball_there = true;
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
first_ball_there = ball_detected;
AOS_LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
::aos::time::DurationInSeconds(monotonic_now() - start_time));
}
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 0.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
AOS_LOG(INFO,
"Shutting off rollers at %f seconds, starting to straighten out\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
- MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 0.0);
+ MoveSuperstructure(-0.05, -0.010, 0.0, MakeProfileParameters(8.0, 40.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 0.0);
CloseShooter();
if (!WaitForDriveNear(kDriveDistance - 2.4, kDoNotTurnCare)) return;
@@ -674,13 +717,15 @@
constexpr double kBallSmallWallTurn = -0.11;
StartDrive(0, kBallSmallWallTurn, kTwoBallBallPickup, kFinishTurn);
- MoveSuperstructure(0.03, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.03, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
if (!WaitForDriveProfileDone()) return;
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- false, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), false, 12.0);
AOS_LOG(INFO, "Done backing up %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
@@ -699,7 +744,7 @@
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- const bool ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ const bool ball_detected = ball_detector_fetcher_->voltage() > 2.5;
if (!ball_detected) {
if (!WaitForDriveDone()) return;
AOS_LOG(INFO, "Aborting, no ball %f\n",
@@ -760,8 +805,9 @@
void AutonomousActor::StealAndMoveOverBy(double distance) {
OpenShooter();
- MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- true, 12.0);
+ MoveSuperstructure(0.10, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 12.0);
if (ShouldCancel()) return;
AOS_LOG(INFO, "Waiting for the intake to come down.\n");
@@ -770,8 +816,9 @@
StartDrive(-distance, M_PI / 2.0, kFastDrive, kStealTurn);
WaitForBallOrDriveDone();
if (ShouldCancel()) return;
- MoveSuperstructure(1.0, -0.010, 0.0, {8.0, 60.0}, {4.0, 10.0}, {10.0, 25.0},
- true, 12.0);
+ MoveSuperstructure(1.0, -0.010, 0.0, MakeProfileParameters(8.0, 60.0),
+ MakeProfileParameters(4.0, 10.0),
+ MakeProfileParameters(10.0, 25.0), true, 12.0);
if (!WaitForDriveDone()) return;
StartDrive(0.0, M_PI / 2.0, kFastDrive, kStealTurn);
@@ -779,15 +826,15 @@
}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
InitializeEncoders();
ResetDrivetrain();
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
LowBarDrive();
if (!WaitForDriveDone()) return true;
@@ -914,7 +961,7 @@
} break;
default:
- AOS_LOG(ERROR, "Invalid auto mode %d\n", params.mode);
+ AOS_LOG(ERROR, "Invalid auto mode %d\n", params->mode());
return true;
}
diff --git a/y2016/actors/autonomous_actor.h b/y2016/actors/autonomous_actor.h
index 8d7baaa..538322b 100644
--- a/y2016/actors/autonomous_actor.h
+++ b/y2016/actors/autonomous_actor.h
@@ -6,25 +6,25 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2016/actors/vision_align_actor.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace actors {
-using ::frc971::ProfileParameters;
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void WaitForBallOrDriveDone();
@@ -41,9 +41,9 @@
SuperstructureGoal superstructure_goal_;
void MoveSuperstructure(double intake, double shoulder, double wrist,
- const ProfileParameters intake_params,
- const ProfileParameters shoulder_params,
- const ProfileParameters wrist_params,
+ const frc971::ProfileParametersT intake_params,
+ const frc971::ProfileParametersT shoulder_params,
+ const frc971::ProfileParametersT wrist_params,
bool traverse_up, double roller_power);
void WaitForSuperstructure();
void WaitForSuperstructureProfile();
@@ -91,13 +91,12 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
- ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
- shooter_goal_sender_;
- ::aos::Fetcher<::y2016::control_loops::shooter::ShooterQueue::Status>
+ ::aos::Sender<::y2016::control_loops::shooter::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<::y2016::control_loops::shooter::Status>
shooter_status_fetcher_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::superstructure::Goal>
superstructure_goal_sender_;
};
diff --git a/y2016/actors/autonomous_actor_main.cc b/y2016/actors/autonomous_actor_main.cc
index 1e2628b..79a1250 100644
--- a/y2016/actors/autonomous_actor_main.cc
+++ b/y2016/actors/autonomous_actor_main.cc
@@ -1,13 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "y2016/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2016/actors/superstructure_action.fbs b/y2016/actors/superstructure_action.fbs
new file mode 100644
index 0000000..405f35b
--- /dev/null
+++ b/y2016/actors/superstructure_action.fbs
@@ -0,0 +1,16 @@
+namespace y2016.actors.superstructure_action;
+
+// Parameters to send with start.
+table SuperstructureActionParams {
+ partial_angle:double;
+ delay_time:double;
+ full_angle:double;
+ shooter_angle:double;
+}
+
+table Goal {
+ run:uint;
+ params:SuperstructureActionParams;
+}
+
+root_type Goal;
diff --git a/y2016/actors/superstructure_action.q b/y2016/actors/superstructure_action.q
deleted file mode 100644
index 1b937aa..0000000
--- a/y2016/actors/superstructure_action.q
+++ /dev/null
@@ -1,23 +0,0 @@
-package y2016.actors;
-
-import "aos/actions/actions.q";
-
-// Parameters to send with start.
-struct SuperstructureActionParams {
- double partial_angle;
- double delay_time;
- double full_angle;
- double shooter_angle;
-};
-
-queue_group SuperstructureActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- SuperstructureActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index 2875cbd..bed7b3a 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -2,10 +2,11 @@
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "y2016/actors/superstructure_actor.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
namespace y2016 {
namespace actors {
@@ -13,29 +14,28 @@
namespace chrono = ::std::chrono;
SuperstructureActor::SuperstructureActor(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actors::SuperstructureActionQueueGroup>(
- event_loop, ".y2016.actors.superstructure_action"),
+ : aos::common::actions::ActorBase<superstructure_action::Goal>(
+ event_loop, "/superstructure_action"),
superstructure_goal_sender_(
- event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")) {}
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool SuperstructureActor::RunAction(
- const actors::SuperstructureActionParams ¶ms) {
+ const superstructure_action::SuperstructureActionParams *params) {
AOS_LOG(INFO, "Starting superstructure action\n");
- MoveSuperstructure(params.partial_angle, params.shooter_angle, false);
+ MoveSuperstructure(params->partial_angle(), params->shooter_angle(), false);
WaitForSuperstructure();
if (ShouldCancel()) return true;
- MoveSuperstructure(params.partial_angle, params.shooter_angle, true);
+ MoveSuperstructure(params->partial_angle(), params->shooter_angle(), true);
if (!WaitOrCancel(chrono::duration_cast<::aos::monotonic_clock::duration>(
- chrono::duration<double>(params.delay_time))))
+ chrono::duration<double>(params->delay_time()))))
return true;
- MoveSuperstructure(params.full_angle, params.shooter_angle, true);
+ MoveSuperstructure(params->full_angle(), params->shooter_angle(), true);
WaitForSuperstructure();
if (ShouldCancel()) return true;
return true;
@@ -45,45 +45,46 @@
bool unfold_climber) {
superstructure_goal_ = {0, shoulder, shooter};
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->angle_intake = 0;
- new_superstructure_goal->angle_shoulder = shoulder;
- new_superstructure_goal->angle_wrist = shooter;
+ control_loops::superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
- new_superstructure_goal->max_angular_velocity_intake = 7.0;
- new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
- new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+ superstructure_goal_builder.add_angle_intake(0);
+ superstructure_goal_builder.add_angle_shoulder(shoulder);
+ superstructure_goal_builder.add_angle_wrist(shooter);
- new_superstructure_goal->max_angular_acceleration_intake = 40.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 5.0;
- new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+ superstructure_goal_builder.add_max_angular_velocity_intake(7.0);
+ superstructure_goal_builder.add_max_angular_velocity_shoulder(4.0);
+ superstructure_goal_builder.add_max_angular_velocity_wrist(10.0);
- new_superstructure_goal->voltage_top_rollers = 0;
- new_superstructure_goal->voltage_bottom_rollers = 0;
+ superstructure_goal_builder.add_max_angular_acceleration_intake(40.0);
+ superstructure_goal_builder.add_max_angular_acceleration_shoulder(5.0);
+ superstructure_goal_builder.add_max_angular_acceleration_wrist(25.0);
- new_superstructure_goal->traverse_unlatched = true;
- new_superstructure_goal->traverse_down = false;
- new_superstructure_goal->voltage_climber = 0.0;
- new_superstructure_goal->unfold_climber = unfold_climber;
+ superstructure_goal_builder.add_voltage_top_rollers(0);
+ superstructure_goal_builder.add_voltage_bottom_rollers(0);
- if (!new_superstructure_goal.Send()) {
+ superstructure_goal_builder.add_traverse_unlatched(true);
+ superstructure_goal_builder.add_traverse_down(false);
+ superstructure_goal_builder.add_voltage_climber(0.0);
+ superstructure_goal_builder.add_unfold_climber(unfold_climber);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure move failed.\n");
}
}
bool SuperstructureActor::SuperstructureProfileDone() {
constexpr double kProfileError = 1e-2;
- return ::std::abs(superstructure_status_fetcher_->intake.goal_angle -
+ return ::std::abs(superstructure_status_fetcher_->intake()->goal_angle() -
superstructure_goal_.intake) < kProfileError &&
- ::std::abs(superstructure_status_fetcher_->shoulder.goal_angle -
+ ::std::abs(superstructure_status_fetcher_->shoulder()->goal_angle() -
superstructure_goal_.shoulder) < kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->intake.goal_angular_velocity) <
- kProfileError &&
- ::std::abs(
- superstructure_status_fetcher_->shoulder.goal_angular_velocity) <
- kProfileError;
+ ::std::abs(superstructure_status_fetcher_->intake()
+ ->goal_angular_velocity()) < kProfileError &&
+ ::std::abs(superstructure_status_fetcher_->shoulder()
+ ->goal_angular_velocity()) < kProfileError;
}
bool SuperstructureActor::SuperstructureDone() {
@@ -91,8 +92,8 @@
// We are no longer running if we are in the zeroing states (below 12), or
// estopped.
- if (superstructure_status_fetcher_->state < 12 ||
- superstructure_status_fetcher_->state == 16) {
+ if (superstructure_status_fetcher_->state() < 12 ||
+ superstructure_status_fetcher_->state() == 16) {
AOS_LOG(ERROR, "Superstructure no longer running, aborting action\n");
return true;
}
diff --git a/y2016/actors/superstructure_actor.h b/y2016/actors/superstructure_actor.h
index ab84db5..f1ae1fe 100644
--- a/y2016/actors/superstructure_actor.h
+++ b/y2016/actors/superstructure_actor.h
@@ -5,29 +5,30 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "y2016/actors/superstructure_action.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/actors/superstructure_action_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
namespace y2016 {
namespace actors {
class SuperstructureActor
- : public ::aos::common::actions::ActorBase<SuperstructureActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<superstructure_action::Goal> {
public:
typedef ::aos::common::actions::TypedActionFactory<
- SuperstructureActionQueueGroup>
+ superstructure_action::Goal>
Factory;
explicit SuperstructureActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2016.actors.superstructure_action");
+ return Factory(event_loop, "/superstructure_action");
}
private:
- ::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
// Internal struct holding superstructure goals sent by autonomous to the
// loop.
@@ -37,7 +38,8 @@
double wrist;
};
SuperstructureGoal superstructure_goal_;
- bool RunAction(const actors::SuperstructureActionParams ¶ms) override;
+ bool RunAction(
+ const superstructure_action::SuperstructureActionParams *params) override;
void MoveSuperstructure(double shoulder, double shooter, bool unfold_climber);
void WaitForSuperstructure();
bool SuperstructureProfileDone();
diff --git a/y2016/actors/superstructure_actor_main.cc b/y2016/actors/superstructure_actor_main.cc
index a97802c..fe512d7 100644
--- a/y2016/actors/superstructure_actor_main.cc
+++ b/y2016/actors/superstructure_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2016/actors/superstructure_action.q.h"
#include "y2016/actors/superstructure_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::SuperstructureActor superstructure(&event_loop);
event_loop.Run();
diff --git a/y2016/actors/vision_align_action.fbs b/y2016/actors/vision_align_action.fbs
new file mode 100644
index 0000000..1dd66fc
--- /dev/null
+++ b/y2016/actors/vision_align_action.fbs
@@ -0,0 +1,13 @@
+namespace y2016.actors.vision_align_action;
+
+// Parameters to send with start.
+table VisionAlignActionParams {
+ run:int;
+}
+
+table Goal {
+ run:uint;
+ params:VisionAlignActionParams;
+}
+
+root_type Goal;
diff --git a/y2016/actors/vision_align_action.q b/y2016/actors/vision_align_action.q
deleted file mode 100644
index f9f3024..0000000
--- a/y2016/actors/vision_align_action.q
+++ /dev/null
@@ -1,20 +0,0 @@
-package y2016.actors;
-
-import "aos/actions/actions.q";
-
-// Parameters to send with start.
-struct VisionAlignActionParams {
- int32_t run;
-};
-
-queue_group VisionAlignActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- VisionAlignActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 1685d22..e1c1892 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -6,34 +6,31 @@
#include <Eigen/Dense>
-#include "aos/util/phased_loop.h"
-#include "aos/logging/logging.h"
-#include "aos/util/trapezoid_profile.h"
#include "aos/commonmath.h"
+#include "aos/logging/logging.h"
#include "aos/time/time.h"
-
-#include "y2016/actors/vision_align_actor.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/trapezoid_profile.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "y2016/constants.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
VisionAlignActor::VisionAlignActor(::aos::EventLoop *event_loop)
- : aos::common::actions::ActorBase<actors::VisionAlignActionQueueGroup>(
- event_loop, ".y2016.actors.vision_align_action"),
+ : aos::common::actions::ActorBase<vision_align_action::Goal>(
+ event_loop, "/vision_align_action"),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")),
+ "/superstructure")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")) {}
bool VisionAlignActor::RunAction(
- const actors::VisionAlignActionParams & /*params*/) {
+ const vision_align_action::VisionAlignActionParams * /*params*/) {
const double robot_radius =
control_loops::drivetrain::GetDrivetrainConfig().robot_radius;
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
@@ -52,31 +49,36 @@
if (!vision_status_fetcher_.Fetch()) {
continue;
}
- const auto &vision_status = *vision_status_fetcher_;
-
- if (!vision_status.left_image_valid || !vision_status.right_image_valid) {
+ if (!vision_status_fetcher_->left_image_valid() ||
+ !vision_status_fetcher_->right_image_valid()) {
continue;
}
const double side_distance_change =
- vision_status.horizontal_angle * robot_radius;
+ vision_status_fetcher_->horizontal_angle() * robot_radius;
if (!::std::isfinite(side_distance_change)) {
continue;
}
- const double left_current = vision_status.drivetrain_left_position;
- const double right_current = vision_status.drivetrain_right_position;
+ const double left_current =
+ vision_status_fetcher_->drivetrain_left_position();
+ const double right_current =
+ vision_status_fetcher_->drivetrain_right_position();
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->wheel = 0.0;
- drivetrain_message->throttle = 0.0;
- drivetrain_message->highgear = false;
- drivetrain_message->quickturn = false;
- drivetrain_message->controller_type = 1;
- drivetrain_message->left_goal = left_current + side_distance_change;
- drivetrain_message->right_goal = right_current - side_distance_change;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- if (!drivetrain_message.Send()) {
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Goal>();
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_highgear(false);
+ goal_builder.add_quickturn(false);
+ goal_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(left_current + side_distance_change);
+ goal_builder.add_right_goal(right_current - side_distance_change);
+
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending drivetrain goal failed\n");
}
}
diff --git a/y2016/actors/vision_align_actor.h b/y2016/actors/vision_align_actor.h
index 586f7d0..ac25439 100644
--- a/y2016/actors/vision_align_actor.h
+++ b/y2016/actors/vision_align_actor.h
@@ -5,32 +5,32 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/actors/vision_align_action.q.h"
-#include "y2016/vision/vision.q.h"
+#include "y2016/actors/vision_align_action_generated.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace actors {
class VisionAlignActor
- : public ::aos::common::actions::ActorBase<VisionAlignActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<vision_align_action::Goal> {
public:
- typedef ::aos::common::actions::TypedActionFactory<
- VisionAlignActionQueueGroup>
+ typedef ::aos::common::actions::TypedActionFactory<vision_align_action::Goal>
Factory;
explicit VisionAlignActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2016.actors.vision_align_action");
+ return Factory(event_loop, "/vision_align_action");
}
- bool RunAction(const actors::VisionAlignActionParams ¶ms) override;
+ bool RunAction(
+ const vision_align_action::VisionAlignActionParams *params) override;
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
};
diff --git a/y2016/actors/vision_align_actor_main.cc b/y2016/actors/vision_align_actor_main.cc
index 2fd9eb2..c7aaf50 100644
--- a/y2016/actors/vision_align_actor_main.cc
+++ b/y2016/actors/vision_align_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2016/actors/vision_align_action.q.h"
#include "y2016/actors/vision_align_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::actors::VisionAlignActor vision_align(&event_loop);
event_loop.Run();